pcl_visualizer.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2012, Open Perception, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: pcl_visualizer.hpp 6220 2012-07-06 22:34:56Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_PCL_VISUALIZER_IMPL_H_
00041 #define PCL_PCL_VISUALIZER_IMPL_H_
00042 
00043 #include <vtkCellData.h>
00044 #include <vtkSmartPointer.h>
00045 #include <vtkCellArray.h>
00046 #include <vtkProperty2D.h>
00047 #include <vtkMapper2D.h>
00048 #include <vtkLeaderActor2D.h>
00049 #include <pcl/common/time.h>
00050 #include <vtkAlgorithmOutput.h>
00051 
00053 template <typename PointT> bool
00054 pcl::visualization::PCLVisualizer::addPointCloud (
00055     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00056     const std::string &id, int viewport)
00057 {
00058   // Convert the PointCloud to VTK PolyData
00059   PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
00060   return (addPointCloud<PointT> (cloud, geometry_handler, id, viewport));
00061 }
00062 
00064 template <typename PointT> bool
00065 pcl::visualization::PCLVisualizer::addPointCloud (
00066     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00067     const PointCloudGeometryHandler<PointT> &geometry_handler,
00068     const std::string &id, int viewport)
00069 {
00070   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00071   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00072 
00073   if (am_it != cloud_actor_map_->end ())
00074   {
00075     PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00076     return (false);
00077   }
00078 
00079   //PointCloudColorHandlerRandom<PointT> color_handler (cloud);
00080   PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
00081   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
00082 }
00083 
00085 template <typename PointT> bool
00086 pcl::visualization::PCLVisualizer::addPointCloud (
00087     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00088     const GeometryHandlerConstPtr &geometry_handler,
00089     const std::string &id, int viewport)
00090 {
00091   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00092   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00093 
00094   if (am_it != cloud_actor_map_->end ())
00095   {
00096     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00097     // be done such as checking if a specific handler already exists, etc.
00098     am_it->second.geometry_handlers.push_back (geometry_handler);
00099     return (true);
00100   }
00101 
00102   //PointCloudColorHandlerRandom<PointT> color_handler (cloud);
00103   PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
00104   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
00105 }
00106 
00108 template <typename PointT> bool
00109 pcl::visualization::PCLVisualizer::addPointCloud (
00110     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00111     const PointCloudColorHandler<PointT> &color_handler,
00112     const std::string &id, int viewport)
00113 {
00114   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00115   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00116 
00117   if (am_it != cloud_actor_map_->end ())
00118   {
00119     PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00120 
00121     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00122     // be done such as checking if a specific handler already exists, etc.
00123     //cloud_actor_map_[id].color_handlers.push_back (color_handler);
00124     //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_));
00125     return (false);
00126   }
00127   // Convert the PointCloud to VTK PolyData
00128   PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
00129   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
00130 }
00131 
00133 template <typename PointT> bool
00134 pcl::visualization::PCLVisualizer::addPointCloud (
00135     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00136     const ColorHandlerConstPtr &color_handler,
00137     const std::string &id, int viewport)
00138 {
00139   // Check to see if this entry already exists (has it been already added to the visualizer?)
00140   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00141   if (am_it != cloud_actor_map_->end ())
00142   {
00143     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00144     // be done such as checking if a specific handler already exists, etc.
00145     am_it->second.color_handlers.push_back (color_handler);
00146     return (true);
00147   }
00148 
00149   PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
00150   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
00151 }
00152 
00154 template <typename PointT> bool
00155 pcl::visualization::PCLVisualizer::addPointCloud (
00156     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00157     const GeometryHandlerConstPtr &geometry_handler,
00158     const ColorHandlerConstPtr &color_handler,
00159     const std::string &id, int viewport)
00160 {
00161   // Check to see if this entry already exists (has it been already added to the visualizer?)
00162   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00163   if (am_it != cloud_actor_map_->end ())
00164   {
00165     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00166     // be done such as checking if a specific handler already exists, etc.
00167     am_it->second.geometry_handlers.push_back (geometry_handler);
00168     am_it->second.color_handlers.push_back (color_handler);
00169     return (true);
00170   }
00171   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
00172 }
00173 
00175 template <typename PointT> bool
00176 pcl::visualization::PCLVisualizer::addPointCloud (
00177     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00178     const PointCloudColorHandler<PointT> &color_handler,
00179     const PointCloudGeometryHandler<PointT> &geometry_handler,
00180     const std::string &id, int viewport)
00181 {
00182   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00183   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00184 
00185   if (am_it != cloud_actor_map_->end ())
00186   {
00187     PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00188     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00189     // be done such as checking if a specific handler already exists, etc.
00190     //cloud_actor_map_[id].geometry_handlers.push_back (geometry_handler);
00191     //cloud_actor_map_[id].color_handlers.push_back (color_handler);
00192     //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_));
00193     return (false);
00194   }
00195   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
00196 }
00197 
00199 template <typename PointT> void
00200 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
00201     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00202     vtkSmartPointer<vtkPolyData> &polydata,
00203     vtkSmartPointer<vtkIdTypeArray> &initcells)
00204 {
00205   vtkSmartPointer<vtkCellArray> vertices;
00206   if (!polydata)
00207   {
00208     allocVtkPolyData (polydata);
00209     vertices = vtkSmartPointer<vtkCellArray>::New ();
00210     polydata->SetVerts (vertices);
00211   }
00212 
00213   // Create the supporting structures
00214   vertices = polydata->GetVerts ();
00215   if (!vertices)
00216     vertices = vtkSmartPointer<vtkCellArray>::New ();
00217 
00218   vtkIdType nr_points = cloud->points.size ();
00219   // Create the point set
00220   vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
00221   if (!points)
00222   {
00223     points = vtkSmartPointer<vtkPoints>::New ();
00224     points->SetDataTypeToFloat ();
00225     polydata->SetPoints (points);
00226   }
00227   points->SetNumberOfPoints (nr_points);
00228 
00229   // Get a pointer to the beginning of the data array
00230   float *data = ((vtkFloatArray*)points->GetData ())->GetPointer (0);
00231 
00232   // Set the points
00233   if (cloud->is_dense)
00234   {
00235     for (vtkIdType i = 0; i < nr_points; ++i)
00236       memcpy (&data[i * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
00237   }
00238   else
00239   {
00240     vtkIdType j = 0;    // true point index
00241     for (vtkIdType i = 0; i < nr_points; ++i)
00242     {
00243       // Check if the point is invalid
00244       if (!pcl_isfinite (cloud->points[i].x) ||
00245           !pcl_isfinite (cloud->points[i].y) ||
00246           !pcl_isfinite (cloud->points[i].z))
00247         continue;
00248 
00249       memcpy (&data[j * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
00250       j++;
00251     }
00252     nr_points = j;
00253     points->SetNumberOfPoints (nr_points);
00254   }
00255 
00256   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
00257   updateCells (cells, initcells, nr_points);
00258 
00259   // Set the cells and the vertices
00260   vertices->SetCells (nr_points, cells);
00261 }
00262 
00264 template <typename PointT> void
00265 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
00266     const pcl::visualization::PointCloudGeometryHandler<PointT> &geometry_handler,
00267     vtkSmartPointer<vtkPolyData> &polydata,
00268     vtkSmartPointer<vtkIdTypeArray> &initcells)
00269 {
00270   vtkSmartPointer<vtkCellArray> vertices;
00271   if (!polydata)
00272   {
00273     allocVtkPolyData (polydata);
00274     vertices = vtkSmartPointer<vtkCellArray>::New ();
00275     polydata->SetVerts (vertices);
00276   }
00277 
00278   // Use the handler to obtain the geometry
00279   vtkSmartPointer<vtkPoints> points;
00280   geometry_handler.getGeometry (points);
00281   polydata->SetPoints (points);
00282 
00283   vtkIdType nr_points = points->GetNumberOfPoints ();
00284 
00285   // Create the supporting structures
00286   vertices = polydata->GetVerts ();
00287   if (!vertices)
00288     vertices = vtkSmartPointer<vtkCellArray>::New ();
00289 
00290   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
00291   updateCells (cells, initcells, nr_points);
00292   // Set the cells and the vertices
00293   vertices->SetCells (nr_points, cells);
00294 }
00295 
00297 template <typename PointT> bool
00298 pcl::visualization::PCLVisualizer::addPolygon (
00299     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00300     double r, double g, double b, const std::string &id, int viewport)
00301 {
00302   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00303   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00304   if (am_it != shape_actor_map_->end ())
00305   {
00306     PCL_WARN ("[addPolygon] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00307     return (false);
00308   }
00309 
00310   vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (cloud);
00311 
00312   // Create an Actor
00313   vtkSmartPointer<vtkLODActor> actor;
00314   createActorFromVTKDataSet (data, actor);
00315   actor->GetProperty ()->SetRepresentationToWireframe ();
00316   actor->GetProperty ()->SetColor (r, g, b);
00317   actor->GetMapper ()->ScalarVisibilityOff ();
00318   addActorToRenderer (actor, viewport);
00319 
00320   // Save the pointer/ID pair to the global actor map
00321   (*shape_actor_map_)[id] = actor;
00322   return (true);
00323 }
00324 
00326 template <typename PointT> bool
00327 pcl::visualization::PCLVisualizer::addPolygon (
00328     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00329     const std::string &id, int viewport)
00330 {
00331   return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5, id, viewport));
00332     return (false);
00333 }
00334 
00336 template <typename P1, typename P2> bool
00337 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
00338 {
00339   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00340   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00341   if (am_it != shape_actor_map_->end ())
00342   {
00343     PCL_WARN ("[addLine] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00344     return (false);
00345   }
00346 
00347   vtkSmartPointer<vtkDataSet> data = createLine (pt1.getVector4fMap (), pt2.getVector4fMap ());
00348 
00349   // Create an Actor
00350   vtkSmartPointer<vtkLODActor> actor;
00351   createActorFromVTKDataSet (data, actor);
00352   actor->GetProperty ()->SetRepresentationToWireframe ();
00353   actor->GetProperty ()->SetColor (r, g, b);
00354   actor->GetMapper ()->ScalarVisibilityOff ();
00355   addActorToRenderer (actor, viewport);
00356 
00357   // Save the pointer/ID pair to the global actor map
00358   (*shape_actor_map_)[id] = actor;
00359   return (true);
00360 }
00361 
00363 template <typename P1, typename P2> bool
00364 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
00365 {
00366   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00367   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00368   if (am_it != shape_actor_map_->end ())
00369   {
00370     PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00371     return (false);
00372   }
00373 
00374   // Create an Actor
00375   vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
00376   leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
00377   leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
00378   leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
00379   leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
00380   leader->SetArrowStyleToFilled ();
00381   leader->AutoLabelOn ();
00382 
00383   leader->GetProperty ()->SetColor (r, g, b);
00384   addActorToRenderer (leader, viewport);
00385 
00386   // Save the pointer/ID pair to the global actor map
00387   (*shape_actor_map_)[id] = leader;
00388   return (true);
00389 }
00390 
00392 template <typename P1, typename P2> bool
00393 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, bool display_length, const std::string &id, int viewport)
00394 {
00395   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00396   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00397   if (am_it != shape_actor_map_->end ())
00398   {
00399     PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00400     return (false);
00401   }
00402 
00403   // Create an Actor
00404   vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
00405   leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
00406   leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
00407   leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
00408   leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
00409   leader->SetArrowStyleToFilled ();
00410   leader->SetArrowPlacementToPoint1 ();
00411   if (display_length)
00412     leader->AutoLabelOn ();
00413   else
00414     leader->AutoLabelOff ();
00415 
00416   leader->GetProperty ()->SetColor (r, g, b);
00417   addActorToRenderer (leader, viewport);
00418 
00419   // Save the pointer/ID pair to the global actor map
00420   (*shape_actor_map_)[id] = leader;
00421   return (true);
00422 }
00423 
00425 template <typename P1, typename P2> bool
00426 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, const std::string &id, int viewport)
00427 {
00428   return (!addLine (pt1, pt2, 0.5, 0.5, 0.5, id, viewport));
00429 }
00430 
00432 /*template <typename P1, typename P2> bool
00433   pcl::visualization::PCLVisualizer::addLineToGroup (const P1 &pt1, const P2 &pt2, const std::string &group_id, int viewport)
00434 {
00435   vtkSmartPointer<vtkDataSet> data = createLine<P1, P2> (pt1, pt2);
00436 
00437   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00438   ShapeActorMap::iterator am_it = shape_actor_map_->find (group_id);
00439   if (am_it != shape_actor_map_->end ())
00440   {
00441     // Get the old data
00442     vtkPolyDataMapper* mapper = static_cast<vtkPolyDataMapper*>(am_it->second->GetMapper ());
00443     vtkPolyData* olddata = static_cast<vtkPolyData*>(mapper->GetInput ());
00444     // Add it to the current data
00445     vtkSmartPointer<vtkAppendPolyData> alldata = vtkSmartPointer<vtkAppendPolyData>::New ();
00446     alldata->AddInput (olddata);
00447 
00448     // Convert to vtkPolyData
00449     vtkSmartPointer<vtkPolyData> curdata = (vtkPolyData::SafeDownCast (data));
00450     alldata->AddInput (curdata);
00451 
00452     mapper->SetInput (alldata->GetOutput ());
00453 
00454     am_it->second->SetMapper (mapper);
00455     am_it->second->Modified ();
00456     return (true);
00457   }
00458 
00459   // Create an Actor
00460   vtkSmartPointer<vtkLODActor> actor;
00461   createActorFromVTKDataSet (data, actor);
00462   actor->GetProperty ()->SetRepresentationToWireframe ();
00463   actor->GetProperty ()->SetColor (1, 0, 0);
00464   addActorToRenderer (actor, viewport);
00465 
00466   // Save the pointer/ID pair to the global actor map
00467   (*shape_actor_map_)[group_id] = actor;
00468 
00469 //ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00470 //vtkSmartPointer<vtkLODActor> actor = am_it->second;
00471   //actor->GetProperty ()->SetColor (r, g, b);
00472 //actor->Modified ();
00473   return (true);
00474 }*/
00475 
00477 template <typename PointT> bool
00478 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id, int viewport)
00479 {
00480   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00481   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00482   if (am_it != shape_actor_map_->end ())
00483   {
00484     PCL_WARN ("[addSphere] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00485     return (false);
00486   }
00487 
00488   //vtkSmartPointer<vtkDataSet> data = createSphere (center.getVector4fMap (), radius);
00489   vtkSmartPointer<vtkSphereSource> data = vtkSmartPointer<vtkSphereSource>::New ();
00490   data->SetRadius (radius);
00491   data->SetCenter (double (center.x), double (center.y), double (center.z));
00492   data->SetPhiResolution (10);
00493   data->SetThetaResolution (10);
00494   data->LatLongTessellationOff ();
00495   data->Update ();
00496  
00497   // Setup actor and mapper 
00498   vtkSmartPointer <vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
00499   mapper->SetInputConnection (data->GetOutputPort ());
00500 
00501   // Create an Actor
00502   vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
00503   actor->SetMapper (mapper);
00504   //createActorFromVTKDataSet (data, actor);
00505   actor->GetProperty ()->SetRepresentationToWireframe ();
00506   actor->GetProperty ()->SetInterpolationToGouraud ();
00507   actor->GetMapper ()->ScalarVisibilityOff ();
00508   actor->GetProperty ()->SetColor (r, g, b);
00509   addActorToRenderer (actor, viewport);
00510 
00511   // Save the pointer/ID pair to the global actor map
00512   (*shape_actor_map_)[id] = actor;
00513   return (true);
00514 }
00515 
00517 template <typename PointT> bool
00518 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, const std::string &id, int viewport)
00519 {
00520   return (addSphere (center, radius, 0.5, 0.5, 0.5, id, viewport));
00521 }
00522 
00524 template<typename PointT> bool
00525 pcl::visualization::PCLVisualizer::updateSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id)
00526 {
00527   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00528   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00529   if (am_it == shape_actor_map_->end ())
00530     return (false);
00531 
00533   // Get the actor pointer
00534   vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
00535   vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer ();
00536   vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
00537 
00538   src->SetCenter (double (center.x), double (center.y), double (center.z));
00539   src->SetRadius (radius);
00540   src->Update ();
00541   actor->GetProperty ()->SetColor (r, g, b);
00542   actor->Modified ();
00543 
00544   return (true);
00545 }
00546 
00548 template <typename PointT> bool
00549 pcl::visualization::PCLVisualizer::addText3D (
00550     const std::string &text,
00551     const PointT& position,
00552     double textScale,
00553     double r,
00554     double g,
00555     double b,
00556     const std::string &id,
00557     int viewport)
00558 {
00559   std::string tid;
00560   if (id.empty ())
00561     tid = text;
00562   else
00563     tid = id;
00564 
00565   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00566   ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
00567   if (am_it != shape_actor_map_->end ())
00568   {
00569     pcl::console::print_warn ("[addText3d] A text with id <%s> already exists! Please choose a different id and retry.\n", tid.c_str ());
00570     return (false);
00571   }
00572 
00573   vtkSmartPointer<vtkVectorText> textSource = vtkSmartPointer<vtkVectorText>::New ();
00574   textSource->SetText (text.c_str());
00575   textSource->Update ();
00576 
00577   vtkSmartPointer<vtkPolyDataMapper> textMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
00578   textMapper->SetInputConnection (textSource->GetOutputPort ());
00579 
00580   // Since each follower may follow a different camera, we need different followers
00581   rens_->InitTraversal ();
00582   vtkRenderer* renderer = NULL;
00583   int i = 1;
00584   while ((renderer = rens_->GetNextItem ()) != NULL)
00585   {
00586     // Should we add the actor to all renderers or just to i-nth renderer?
00587     if (viewport == 0 || viewport == i)
00588     {
00589       vtkSmartPointer<vtkFollower> textActor = vtkSmartPointer<vtkFollower>::New ();
00590       textActor->SetMapper (textMapper);
00591       textActor->SetPosition (position.x, position.y, position.z);
00592       textActor->SetScale (textScale);
00593       textActor->GetProperty ()->SetColor (r, g, b);
00594       textActor->SetCamera (renderer->GetActiveCamera ());
00595 
00596       renderer->AddActor (textActor);
00597       renderer->Render ();
00598 
00599       // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers
00600       // for multiple viewport
00601       std::string alternate_tid = tid;
00602       alternate_tid.append(i, '*');
00603 
00604       (*shape_actor_map_)[(viewport == 0) ? tid : alternate_tid] = textActor;
00605     }
00606 
00607     ++i;
00608   }
00609 
00610   return (true);
00611 }
00612 
00614 template <typename PointNT> bool
00615 pcl::visualization::PCLVisualizer::addPointCloudNormals (
00616     const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
00617     int level, double scale, const std::string &id, int viewport)
00618 {
00619   return (addPointCloudNormals<PointNT, PointNT>(cloud, cloud, level, scale, id, viewport));
00620 }
00621 
00623 template <typename PointT, typename PointNT> bool
00624 pcl::visualization::PCLVisualizer::addPointCloudNormals (
00625     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00626     const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
00627     int level, double scale,
00628     const std::string &id, int viewport)
00629 {
00630   if (normals->points.size () != cloud->points.size ())
00631   {
00632     PCL_ERROR ("[addPointCloudNormals] The number of points differs from the number of normals!\n");
00633     return (false);
00634   }
00635   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00636   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00637 
00638   if (am_it != cloud_actor_map_->end ())
00639   {
00640     PCL_WARN ("[addPointCloudNormals] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00641     return (false);
00642   }
00643 
00644   vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
00645   vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
00646 
00647   points->SetDataTypeToFloat ();
00648   vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
00649   data->SetNumberOfComponents (3);
00650 
00651   vtkIdType nr_normals = (cloud->points.size () - 1) / level + 1 ;
00652   float* pts = new float[2 * nr_normals * 3];
00653 
00654   for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
00655   {
00656     PointT p = cloud->points[i];
00657     p.x += normals->points[i].normal[0] * scale;
00658     p.y += normals->points[i].normal[1] * scale;
00659     p.z += normals->points[i].normal[2] * scale;
00660 
00661     pts[2 * j * 3 + 0] = cloud->points[i].x;
00662     pts[2 * j * 3 + 1] = cloud->points[i].y;
00663     pts[2 * j * 3 + 2] = cloud->points[i].z;
00664     pts[2 * j * 3 + 3] = p.x;
00665     pts[2 * j * 3 + 4] = p.y;
00666     pts[2 * j * 3 + 5] = p.z;
00667 
00668     lines->InsertNextCell(2);
00669     lines->InsertCellPoint(2*j);
00670     lines->InsertCellPoint(2*j+1);
00671   }
00672 
00673   data->SetArray (&pts[0], 2 * nr_normals * 3, 0);
00674   points->SetData (data);
00675 
00676   vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
00677   polyData->SetPoints(points);
00678   polyData->SetLines(lines);
00679 
00680   vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
00681   mapper->SetInput (polyData);
00682   mapper->SetColorModeToMapScalars();
00683   mapper->SetScalarModeToUsePointData();
00684 
00685   // create actor
00686   vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
00687   actor->SetMapper (mapper);
00688 
00689   // Add it to all renderers
00690   addActorToRenderer (actor, viewport);
00691 
00692   // Save the pointer/ID pair to the global actor map
00693   (*cloud_actor_map_)[id].actor = actor;
00694   return (true);
00695 }
00696 
00698 template <typename PointT> bool
00699 pcl::visualization::PCLVisualizer::addCorrespondences (
00700    const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
00701    const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
00702    const std::vector<int> &correspondences,
00703    const std::string &id,
00704    int viewport)
00705 {
00706   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00707   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00708   if (am_it != shape_actor_map_->end ())
00709   {
00710     PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00711     return (false);
00712   }
00713 
00714   vtkSmartPointer<vtkAppendPolyData> polydata = vtkSmartPointer<vtkAppendPolyData>::New ();
00715 
00716   vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00717   line_colors->SetNumberOfComponents (3);
00718   line_colors->SetName ("Colors");
00719   // Use Red by default (can be changed later)
00720   unsigned char rgb[3];
00721   rgb[0] = 1 * 255.0;
00722   rgb[1] = 0 * 255.0;
00723   rgb[2] = 0 * 255.0;
00724 
00725   // Draw lines between the best corresponding points
00726   for (size_t i = 0; i < source_points->size (); ++i)
00727   {
00728     const PointT &p_src = source_points->points[i];
00729     const PointT &p_tgt = target_points->points[correspondences[i]];
00730 
00731     // Add the line
00732     vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
00733     line->SetPoint1 (p_src.x, p_src.y, p_src.z);
00734     line->SetPoint2 (p_tgt.x, p_tgt.y, p_tgt.z);
00735     line->Update ();
00736     polydata->AddInput (line->GetOutput ());
00737     line_colors->InsertNextTupleValue (rgb);
00738   }
00739   polydata->Update ();
00740   vtkSmartPointer<vtkPolyData> line_data = polydata->GetOutput ();
00741   line_data->GetCellData ()->SetScalars (line_colors);
00742 
00743   // Create an Actor
00744   vtkSmartPointer<vtkLODActor> actor;
00745   createActorFromVTKDataSet (line_data, actor);
00746   actor->GetProperty ()->SetRepresentationToWireframe ();
00747   actor->GetProperty ()->SetOpacity (0.5);
00748   addActorToRenderer (actor, viewport);
00749 
00750   // Save the pointer/ID pair to the global actor map
00751   (*shape_actor_map_)[id] = actor;
00752 
00753   return (true);
00754 }
00755 
00757 template <typename PointT> bool
00758 pcl::visualization::PCLVisualizer::addCorrespondences (
00759    const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
00760    const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
00761    const pcl::Correspondences &correspondences,
00762    const std::string &id,
00763    int viewport)
00764 {
00765   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00766   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00767   if (am_it != shape_actor_map_->end ())
00768   {
00769     PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00770     return (false);
00771   }
00772 
00773   vtkSmartPointer<vtkAppendPolyData> polydata = vtkSmartPointer<vtkAppendPolyData>::New ();
00774 
00775   vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00776   line_colors->SetNumberOfComponents (3);
00777   line_colors->SetName ("Colors");
00778   unsigned char rgb[3];
00779   // Use Red by default (can be changed later)
00780   rgb[0] = 1 * 255.0;
00781   rgb[1] = 0 * 255.0;
00782   rgb[2] = 0 * 255.0;
00783 
00784   // Draw lines between the best corresponding points
00785   for (size_t i = 0; i < correspondences.size (); ++i)
00786   {
00787     const PointT &p_src = source_points->points[correspondences[i].index_query];
00788     const PointT &p_tgt = target_points->points[correspondences[i].index_match];
00789 
00790     // Add the line
00791     vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
00792     line->SetPoint1 (p_src.x, p_src.y, p_src.z);
00793     line->SetPoint2 (p_tgt.x, p_tgt.y, p_tgt.z);
00794     line->Update ();
00795     polydata->AddInput (line->GetOutput ());
00796     line_colors->InsertNextTupleValue (rgb);
00797   }
00798   polydata->Update ();
00799   vtkSmartPointer<vtkPolyData> line_data = polydata->GetOutput ();
00800   line_data->GetCellData ()->SetScalars (line_colors);
00801 
00802   // Create an Actor
00803   vtkSmartPointer<vtkLODActor> actor;
00804   createActorFromVTKDataSet (line_data, actor);
00805   actor->GetProperty ()->SetRepresentationToWireframe ();
00806   actor->GetProperty ()->SetOpacity (0.5);
00807   addActorToRenderer (actor, viewport);
00808 
00809   // Save the pointer/ID pair to the global actor map
00810   (*shape_actor_map_)[id] = actor;
00811 
00812   return (true);
00813 }
00814 
00816 template <typename PointT> bool
00817 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
00818     const PointCloudGeometryHandler<PointT> &geometry_handler,
00819     const PointCloudColorHandler<PointT> &color_handler,
00820     const std::string &id,
00821     int viewport,
00822     const Eigen::Vector4f& sensor_origin,
00823     const Eigen::Quaternion<float>& sensor_orientation)
00824 {
00825   if (!geometry_handler.isCapable ())
00826   {
00827     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
00828     return (false);
00829   }
00830 
00831   if (!color_handler.isCapable ())
00832   {
00833     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
00834     return (false);
00835   }
00836 
00837   vtkSmartPointer<vtkPolyData> polydata;
00838   vtkSmartPointer<vtkIdTypeArray> initcells;
00839   // Convert the PointCloud to VTK PolyData
00840   convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
00841   // use the given geometry handler
00842   polydata->Update ();
00843 
00844   // Get the colors from the handler
00845   vtkSmartPointer<vtkDataArray> scalars;
00846   color_handler.getColor (scalars);
00847   polydata->GetPointData ()->SetScalars (scalars);
00848   double minmax[2];
00849   scalars->GetRange (minmax);
00850 
00851   // Create an Actor
00852   vtkSmartPointer<vtkLODActor> actor;
00853   createActorFromVTKDataSet (polydata, actor);
00854   actor->GetMapper ()->SetScalarRange (minmax);
00855 
00856   // Add it to all renderers
00857   addActorToRenderer (actor, viewport);
00858 
00859   // Save the pointer/ID pair to the global actor map
00860   (*cloud_actor_map_)[id].actor = actor;
00861   (*cloud_actor_map_)[id].cells = initcells;
00862 
00863   // Save the viewpoint transformation matrix to the global actor map
00864   vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
00865   convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
00866   (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
00867 
00868   return (true);
00869 }
00870 
00872 template <typename PointT> bool
00873 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
00874     const PointCloudGeometryHandler<PointT> &geometry_handler,
00875     const ColorHandlerConstPtr &color_handler,
00876     const std::string &id,
00877     int viewport,
00878     const Eigen::Vector4f& sensor_origin,
00879     const Eigen::Quaternion<float>& sensor_orientation)
00880 {
00881   if (!geometry_handler.isCapable ())
00882   {
00883     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
00884     return (false);
00885   }
00886 
00887   if (!color_handler->isCapable ())
00888   {
00889     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler->getName ().c_str ());
00890     return (false);
00891   }
00892 
00893   vtkSmartPointer<vtkPolyData> polydata;
00894   vtkSmartPointer<vtkIdTypeArray> initcells;
00895   // Convert the PointCloud to VTK PolyData
00896   convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
00897   // use the given geometry handler
00898   polydata->Update ();
00899 
00900   // Get the colors from the handler
00901   vtkSmartPointer<vtkDataArray> scalars;
00902   color_handler->getColor (scalars);
00903   polydata->GetPointData ()->SetScalars (scalars);
00904   double minmax[2];
00905   scalars->GetRange (minmax);
00906 
00907   // Create an Actor
00908   vtkSmartPointer<vtkLODActor> actor;
00909   createActorFromVTKDataSet (polydata, actor);
00910   actor->GetMapper ()->SetScalarRange (minmax);
00911 
00912   // Add it to all renderers
00913   addActorToRenderer (actor, viewport);
00914 
00915   // Save the pointer/ID pair to the global actor map
00916   (*cloud_actor_map_)[id].actor = actor;
00917   (*cloud_actor_map_)[id].cells = initcells;
00918   (*cloud_actor_map_)[id].color_handlers.push_back (color_handler);
00919 
00920   // Save the viewpoint transformation matrix to the global actor map
00921   // Save the viewpoint transformation matrix to the global actor map
00922   vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
00923   convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
00924   (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
00925 
00926   return (true);
00927 }
00928 
00930 template <typename PointT> bool
00931 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
00932     const GeometryHandlerConstPtr &geometry_handler,
00933     const PointCloudColorHandler<PointT> &color_handler,
00934     const std::string &id,
00935     int viewport,
00936     const Eigen::Vector4f& sensor_origin,
00937     const Eigen::Quaternion<float>& sensor_orientation)
00938 {
00939   if (!geometry_handler->isCapable ())
00940   {
00941     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler->getName ().c_str ());
00942     return (false);
00943   }
00944 
00945   if (!color_handler.isCapable ())
00946   {
00947     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
00948     return (false);
00949   }
00950 
00951   vtkSmartPointer<vtkPolyData> polydata;
00952   vtkSmartPointer<vtkIdTypeArray> initcells;
00953   // Convert the PointCloud to VTK PolyData
00954   convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
00955   // use the given geometry handler
00956   polydata->Update ();
00957 
00958   // Get the colors from the handler
00959   vtkSmartPointer<vtkDataArray> scalars;
00960   color_handler.getColor (scalars);
00961   polydata->GetPointData ()->SetScalars (scalars);
00962   double minmax[2];
00963   scalars->GetRange (minmax);
00964 
00965   // Create an Actor
00966   vtkSmartPointer<vtkLODActor> actor;
00967   createActorFromVTKDataSet (polydata, actor);
00968   actor->GetMapper ()->SetScalarRange (minmax);
00969 
00970   // Add it to all renderers
00971   addActorToRenderer (actor, viewport);
00972 
00973   // Save the pointer/ID pair to the global actor map
00974   (*cloud_actor_map_)[id].actor = actor;
00975   (*cloud_actor_map_)[id].cells = initcells;
00976   (*cloud_actor_map_)[id].geometry_handlers.push_back (geometry_handler);
00977 
00978   // Save the viewpoint transformation matrix to the global actor map
00979   vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
00980   convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
00981   (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
00982 
00983   return (true);
00984 }
00985 
00987 template <typename PointT> bool
00988 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00989                                                      const std::string &id)
00990 {
00991   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00992   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00993 
00994   if (am_it == cloud_actor_map_->end ())
00995     return (false);
00996 
00997   vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
00998   // Convert the PointCloud to VTK PolyData
00999   convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
01000   polydata->Update ();
01001 
01002   // Set scalars to blank, since there is no way we can update them here.
01003   vtkSmartPointer<vtkDataArray> scalars;
01004   polydata->GetPointData ()->SetScalars (scalars);
01005   polydata->Update ();
01006   double minmax[2];
01007   minmax[0] = std::numeric_limits<double>::min ();
01008   minmax[1] = std::numeric_limits<double>::max ();
01009   am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
01010   am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
01011 
01012   // Update the mapper
01013   reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
01014   return (true);
01015 }
01016 
01018 template <typename PointT> bool
01019 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
01020                                                      const PointCloudGeometryHandler<PointT> &geometry_handler,
01021                                                      const std::string &id)
01022 {
01023   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
01024   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
01025 
01026   if (am_it == cloud_actor_map_->end ())
01027     return (false);
01028 
01029   vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
01030   if (!polydata)
01031     return (false);
01032   // Convert the PointCloud to VTK PolyData
01033   convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
01034 
01035   // Set scalars to blank, since there is no way we can update them here.
01036   vtkSmartPointer<vtkDataArray> scalars;
01037   polydata->GetPointData ()->SetScalars (scalars);
01038   polydata->Update ();
01039   double minmax[2];
01040   minmax[0] = std::numeric_limits<double>::min ();
01041   minmax[1] = std::numeric_limits<double>::max ();
01042   am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
01043   am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
01044 
01045   // Update the mapper
01046   reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
01047   return (true);
01048 }
01049 
01050 
01052 template <typename PointT> bool
01053 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
01054                                                      const PointCloudColorHandler<PointT> &color_handler,
01055                                                      const std::string &id)
01056 {
01057   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
01058   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
01059 
01060   if (am_it == cloud_actor_map_->end ())
01061     return (false);
01062 
01063   // Get the current poly data
01064   vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
01065   if (!polydata)
01066     return (false);
01067   vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
01068   vtkSmartPointer<vtkPoints> points      = polydata->GetPoints ();
01069   // Copy the new point array in
01070   vtkIdType nr_points = cloud->points.size ();
01071   points->SetNumberOfPoints (nr_points);
01072 
01073   // Get a pointer to the beginning of the data array
01074   float *data = ((vtkFloatArray*)points->GetData ())->GetPointer (0);
01075 
01076   int pts = 0;
01077   // If the dataset is dense (no NaNs)
01078   if (cloud->is_dense)
01079   {
01080     for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
01081       memcpy (&data[pts], &cloud->points[i].x, 12);    // sizeof (float) * 3
01082   }
01083   else
01084   {
01085     vtkIdType j = 0;    // true point index
01086     for (vtkIdType i = 0; i < nr_points; ++i)
01087     {
01088       // Check if the point is invalid
01089       if (!isFinite (cloud->points[i]))
01090         continue;
01091 
01092       memcpy (&data[pts], &cloud->points[i].x, 12);    // sizeof (float) * 3
01093       pts += 3;
01094       j++;
01095     }
01096     nr_points = j;
01097     points->SetNumberOfPoints (nr_points);
01098   }
01099 
01100   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
01101   updateCells (cells, am_it->second.cells, nr_points);
01102 
01103   // Set the cells and the vertices
01104   vertices->SetCells (nr_points, cells);
01105 
01106   // Get the colors from the handler
01107   vtkSmartPointer<vtkDataArray> scalars;
01108   color_handler.getColor (scalars);
01109   double minmax[2];
01110   scalars->GetRange (minmax);
01111   // Update the data
01112   polydata->GetPointData ()->SetScalars (scalars);
01113   polydata->Update ();
01114 
01115   am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
01116   am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
01117 
01118   // Update the mapper
01119   reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
01120   return (true);
01121 }
01122 
01124 template <typename PointT> bool
01125 pcl::visualization::PCLVisualizer::addPolygonMesh (
01126     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
01127     const std::vector<pcl::Vertices> &vertices,
01128     const std::string &id,
01129     int viewport)
01130 {
01131   if (vertices.empty () || cloud->points.empty ())
01132     return (false);
01133 
01134   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
01135   if (am_it != cloud_actor_map_->end ())
01136   {
01137     pcl::console::print_warn (
01138                                 "[addPolygonMesh] A shape with id <%s> already exists! Please choose a different id and retry.\n",
01139                                 id.c_str ());
01140     return (false);
01141   }
01142 
01143   // Create points from polyMesh.cloud
01144   vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
01145   vtkIdType nr_points = cloud->points.size ();
01146   points->SetNumberOfPoints (nr_points);
01147   vtkSmartPointer<vtkLODActor> actor;
01148 
01149   // Get a pointer to the beginning of the data array
01150   float *data = ((vtkFloatArray*)points->GetData ())->GetPointer (0);
01151 
01152   int ptr = 0;
01153   std::vector<int> lookup;
01154   // If the dataset is dense (no NaNs)
01155   if (cloud->is_dense)
01156   {
01157     for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
01158       memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3);
01159   }
01160   else
01161   {
01162     lookup.resize (nr_points);
01163     vtkIdType j = 0;    // true point index
01164     for (vtkIdType i = 0; i < nr_points; ++i)
01165     {
01166       // Check if the point is invalid
01167       if (!isFinite (cloud->points[i]))
01168         continue;
01169 
01170       lookup[i] = j;
01171       memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3);
01172       j++;
01173       ptr += 3;
01174     }
01175     nr_points = j;
01176     points->SetNumberOfPoints (nr_points);
01177   }
01178 
01179   // Get the maximum size of a polygon
01180   int max_size_of_polygon = -1;
01181   for (size_t i = 0; i < vertices.size (); ++i)
01182     if (max_size_of_polygon < (int)vertices[i].vertices.size ())
01183       max_size_of_polygon = vertices[i].vertices.size ();
01184 
01185   if (vertices.size () > 1)
01186   {
01187     // Create polys from polyMesh.polygons
01188     vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New ();
01189     vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
01190     int idx = 0;
01191     if (lookup.size () > 0)
01192     {
01193       for (size_t i = 0; i < vertices.size (); ++i, ++idx)
01194       {
01195         size_t n_points = vertices[i].vertices.size ();
01196         *cell++ = n_points;
01197         //cell_array->InsertNextCell (n_points);
01198         for (size_t j = 0; j < n_points; j++, ++idx)
01199           *cell++ = lookup[vertices[i].vertices[j]];
01200           //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
01201       }
01202     }
01203     else
01204     {
01205       for (size_t i = 0; i < vertices.size (); ++i, ++idx)
01206       {
01207         size_t n_points = vertices[i].vertices.size ();
01208         *cell++ = n_points;
01209         //cell_array->InsertNextCell (n_points);
01210         for (size_t j = 0; j < n_points; j++, ++idx)
01211           *cell++ = vertices[i].vertices[j];
01212           //cell_array->InsertCellPoint (vertices[i].vertices[j]);
01213       }
01214     }
01215     vtkSmartPointer<vtkPolyData> polydata;
01216     allocVtkPolyData (polydata);
01217     cell_array->GetData ()->SetNumberOfValues (idx);
01218     cell_array->Squeeze ();
01219     polydata->SetStrips (cell_array);
01220     polydata->SetPoints (points);
01221 
01222     createActorFromVTKDataSet (polydata, actor, false);
01223   }
01224   else
01225   {
01226     vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
01227     size_t n_points = vertices[0].vertices.size ();
01228     polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
01229 
01230     if (lookup.size () > 0)
01231     {
01232       for (size_t j = 0; j < (n_points - 1); ++j)
01233         polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
01234     }
01235     else
01236     {
01237       for (size_t j = 0; j < (n_points - 1); ++j)
01238         polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
01239     }
01240     vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
01241     allocVtkUnstructuredGrid (poly_grid);
01242     poly_grid->Allocate (1, 1);
01243     poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
01244     poly_grid->SetPoints (points);
01245     poly_grid->Update ();
01246 
01247     createActorFromVTKDataSet (poly_grid, actor, false);
01248   }
01249   actor->GetProperty ()->SetRepresentationToSurface ();
01250   actor->GetProperty ()->BackfaceCullingOn ();
01251   actor->GetProperty ()->EdgeVisibilityOff ();
01252   actor->GetProperty ()->ShadingOff ();
01253   addActorToRenderer (actor, viewport);
01254 
01255   // Save the pointer/ID pair to the global actor map
01256   (*cloud_actor_map_)[id].actor = actor;
01257   //if (vertices.size () > 1)
01258   //  (*cloud_actor_map_)[id].cells = static_cast<vtkPolyDataMapper*>(actor->GetMapper ())->GetInput ()->GetVerts ()->GetData ();
01259   return (true);
01260 }
01261 
01263 template <typename PointT> bool
01264 pcl::visualization::PCLVisualizer::updatePolygonMesh (
01265     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
01266     const std::vector<pcl::Vertices> &verts,
01267     const std::string &id)
01268 {
01269   if (verts.empty ())
01270   {
01271      pcl::console::print_error ("[addPolygonMesh] No vertices given!\n");
01272      return (false);
01273   }
01274 
01275   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
01276   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
01277   if (am_it == cloud_actor_map_->end ())
01278     return (false);
01279 
01280   // Get the current poly data
01281   vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
01282   if (!polydata)
01283     return (false);
01284   vtkSmartPointer<vtkCellArray> cells = polydata->GetStrips ();
01285   if (!cells)
01286     return (false);
01287   vtkSmartPointer<vtkPoints> points   = polydata->GetPoints ();
01288   // Copy the new point array in
01289   vtkIdType nr_points = cloud->points.size ();
01290   points->SetNumberOfPoints (nr_points);
01291 
01292   // Get a pointer to the beginning of the data array
01293   float *data = ((vtkFloatArray*)points->GetData ())->GetPointer (0);
01294 
01295   int ptr = 0;
01296   std::vector<int> lookup;
01297   // If the dataset is dense (no NaNs)
01298   if (cloud->is_dense)
01299   {
01300     for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
01301       memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3);
01302   }
01303   else
01304   {
01305     lookup.resize (nr_points);
01306     vtkIdType j = 0;    // true point index
01307     for (vtkIdType i = 0; i < nr_points; ++i)
01308     {
01309       // Check if the point is invalid
01310       if (!isFinite (cloud->points[i]))
01311         continue;
01312 
01313       lookup [i] = j;
01314       memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3);
01315       j++;
01316       ptr += 3;
01317     }
01318     nr_points = j;
01319     points->SetNumberOfPoints (nr_points);
01320   }
01321 
01322   // Get the maximum size of a polygon
01323   int max_size_of_polygon = -1;
01324   for (size_t i = 0; i < verts.size (); ++i)
01325     if (max_size_of_polygon < (int)verts[i].vertices.size ())
01326       max_size_of_polygon = verts[i].vertices.size ();
01327 
01328   // Update the cells
01329   cells = vtkSmartPointer<vtkCellArray>::New ();
01330   vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
01331   int idx = 0;
01332   if (lookup.size () > 0)
01333   {
01334     for (size_t i = 0; i < verts.size (); ++i, ++idx)
01335     {
01336       size_t n_points = verts[i].vertices.size ();
01337       *cell++ = n_points;
01338       for (size_t j = 0; j < n_points; j++, cell++, ++idx)
01339         *cell = lookup[verts[i].vertices[j]];
01340     }
01341   }
01342   else
01343   {
01344     for (size_t i = 0; i < verts.size (); ++i, ++idx)
01345     {
01346       size_t n_points = verts[i].vertices.size ();
01347       *cell++ = n_points;
01348       for (size_t j = 0; j < n_points; j++, cell++, ++idx)
01349         *cell = verts[i].vertices[j];
01350     }
01351   }
01352   cells->GetData ()->SetNumberOfValues (idx);
01353   cells->Squeeze ();
01354   // Set the the vertices
01355   polydata->SetStrips (cells);
01356   polydata->Update ();
01357 
01358 /*
01359   vtkSmartPointer<vtkLODActor> actor;
01360   if (vertices.size () > 1)
01361   {
01362   }
01363   else
01364   {
01365     vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
01366     size_t n_points = vertices[0].vertices.size ();
01367     polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
01368 
01369     for (size_t j = 0; j < (n_points - 1); ++j)
01370       polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
01371 
01372     vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
01373     allocVtkUnstructuredGrid (poly_grid);
01374     poly_grid->Allocate (1, 1);
01375     poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
01376     poly_grid->SetPoints (points);
01377     poly_grid->Update ();
01378 
01379     createActorFromVTKDataSet (poly_grid, actor);
01380   }
01381 */
01382 
01383   // Get the colors from the handler
01384   //vtkSmartPointer<vtkDataArray> scalars;
01385   //color_handler.getColor (scalars);
01386   //polydata->GetPointData ()->SetScalars (scalars);
01387 //  polydata->Update ();
01388 
01389   am_it->second.actor->GetProperty ()->BackfaceCullingOn ();
01390 //  am_it->second.actor->Modified ();
01391 
01392   return (true);
01393 }
01394 
01395 
01397 /* Optimized function: need to do something with the signature as it colides with the general T one
01398 bool
01399 pcl::visualization::PCLVisualizer::updatePointCloud (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud,
01400                                                      const PointCloudColorHandlerRGBField<pcl::PointXYZRGB> &color_handler,
01401                                                      const std::string &id)
01402 {
01403   win_->SetAbortRender (1);
01404   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
01405   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
01406 
01407   if (am_it == cloud_actor_map_->end ())
01408     return (false);
01409 
01410   // Get the current poly data
01411   vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
01412   if (!polydata)
01413     return (false);
01414   vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
01415   vtkSmartPointer<vtkPoints> points      = polydata->GetPoints ();
01416 //  vtkUnsignedCharArray* scalars = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
01417   vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
01418   // Copy the new point array in
01419   vtkIdType nr_points = cloud->points.size ();
01420   points->SetNumberOfPoints (nr_points);
01421   scalars->SetNumberOfComponents (3);
01422   scalars->SetNumberOfTuples (nr_points);
01423   polydata->GetPointData ()->SetScalars (scalars);
01424   unsigned char* colors = scalars->GetPointer (0);
01425 
01426   // Get a pointer to the beginning of the data array
01427   float *data = ((vtkFloatArray*)points->GetData ())->GetPointer (0);
01428 
01429   // If the dataset is dense (no NaNs)
01430   if (cloud->is_dense)
01431   {
01432     for (vtkIdType i = 0; i < nr_points; ++i)
01433     {
01434       memcpy (&data[i * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
01435       int idx = i * 3;
01436       colors[idx    ] = cloud->points[i].r;
01437       colors[idx + 1] = cloud->points[i].g;
01438       colors[idx + 2] = cloud->points[i].b;
01439     }
01440   }
01441   else
01442   {
01443     vtkIdType j = 0;    // true point index
01444     for (vtkIdType i = 0; i < nr_points; ++i)
01445     {
01446       // Check if the point is invalid
01447       if (!pcl_isfinite (cloud->points[i].x) ||
01448           !pcl_isfinite (cloud->points[i].y) ||
01449           !pcl_isfinite (cloud->points[i].z))
01450         continue;
01451 
01452       memcpy (&data[j * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
01453       int idx = j * 3;
01454       colors[idx    ] = cloud->points[i].r;
01455       colors[idx + 1] = cloud->points[i].g;
01456       colors[idx + 2] = cloud->points[i].b;
01457       j++;
01458     }
01459     nr_points = j;
01460     points->SetNumberOfPoints (nr_points);
01461     scalars->SetNumberOfTuples (nr_points);
01462   }
01463 
01464   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
01465   updateCells (cells, am_it->second.cells, nr_points);
01466 
01467   // Set the cells and the vertices
01468   vertices->SetCells (nr_points, cells);
01469 
01470   // Update the mapper
01471   reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
01472   return (true);
01473 }*/
01474 
01475 
01476 #endif


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:28