Point Cloud Library (PCL)  1.3.1
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) 2010-2011, Willow Garage, 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 2723 2011-10-12 00:43:51Z 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 <vtkProperty2D.h>
00045 #include <vtkMapper2D.h>
00046 #include <vtkLeaderActor2D.h>
00047 
00049 template <typename PointT> bool 
00050 pcl::visualization::PCLVisualizer::addPointCloud (
00051     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00052     const std::string &id, int viewport)
00053 {
00054   // Convert the PointCloud to VTK PolyData
00055   PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
00056   return (addPointCloud<PointT> (cloud, geometry_handler, id, viewport));
00057 }
00058 
00060 template <typename PointT> bool 
00061 pcl::visualization::PCLVisualizer::addPointCloud (
00062     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00063     const PointCloudGeometryHandler<PointT> &geometry_handler,
00064     const std::string &id, int viewport)
00065 {
00066   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00067   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00068 
00069   if (am_it != cloud_actor_map_->end ())
00070   {
00071     PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00072     return (false);
00073   }
00074   
00075   //PointCloudColorHandlerRandom<PointT> color_handler (cloud);
00076   PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
00077   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport));
00078 }
00079 
00081 template <typename PointT> bool 
00082 pcl::visualization::PCLVisualizer::addPointCloud (
00083     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00084     const GeometryHandlerConstPtr &geometry_handler,
00085     const std::string &id, int viewport)
00086 {
00087   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00088   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00089 
00090   if (am_it != cloud_actor_map_->end ())
00091   {
00092     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00093     // be done such as checking if a specific handler already exists, etc.
00094     am_it->second.geometry_handlers.push_back (geometry_handler);
00095     return (true);
00096   }
00097 
00098   //PointCloudColorHandlerRandom<PointT> color_handler (cloud);
00099   PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
00100   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport));
00101 }
00102 
00104 template <typename PointT> bool 
00105 pcl::visualization::PCLVisualizer::addPointCloud (
00106     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00107     const PointCloudColorHandler<PointT> &color_handler, 
00108     const std::string &id, int viewport)
00109 {
00110   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00111   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00112 
00113   if (am_it != cloud_actor_map_->end ())
00114   {
00115     PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00116 
00117     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00118     // be done such as checking if a specific handler already exists, etc.
00119     //cloud_actor_map_[id].color_handlers.push_back (color_handler);
00120     //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_));
00121     return (false);
00122   }
00123   // Convert the PointCloud to VTK PolyData
00124   PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
00125   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport));
00126 }
00127 
00129 template <typename PointT> bool 
00130 pcl::visualization::PCLVisualizer::addPointCloud (
00131     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00132     const ColorHandlerConstPtr &color_handler, 
00133     const std::string &id, int viewport)
00134 {
00135   // Check to see if this entry already exists (has it been already added to the visualizer?)
00136   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00137   if (am_it != cloud_actor_map_->end ())
00138   {
00139     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00140     // be done such as checking if a specific handler already exists, etc.
00141     am_it->second.color_handlers.push_back (color_handler);
00142     return (true);
00143   }
00144 
00145   PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
00146   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport));
00147 }
00148 
00150 template <typename PointT> bool 
00151 pcl::visualization::PCLVisualizer::addPointCloud (
00152     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00153     const GeometryHandlerConstPtr &geometry_handler,  
00154     const ColorHandlerConstPtr &color_handler, 
00155     const std::string &id, int viewport)
00156 {
00157   // Check to see if this entry already exists (has it been already added to the visualizer?)
00158   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00159   if (am_it != cloud_actor_map_->end ())
00160   {
00161     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00162     // be done such as checking if a specific handler already exists, etc.
00163     am_it->second.geometry_handlers.push_back (geometry_handler);
00164     am_it->second.color_handlers.push_back (color_handler);
00165     return (true);
00166   }
00167   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport));
00168 }
00169 
00171 template <typename PointT> bool 
00172 pcl::visualization::PCLVisualizer::addPointCloud (
00173     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00174     const PointCloudColorHandler<PointT> &color_handler, 
00175     const PointCloudGeometryHandler<PointT> &geometry_handler,
00176     const std::string &id, int viewport)
00177 {
00178   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00179   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00180 
00181   if (am_it != cloud_actor_map_->end ())
00182   {
00183     PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00184     // Here we're just pushing the handlers onto the queue. If needed, something fancier could
00185     // be done such as checking if a specific handler already exists, etc.
00186     //cloud_actor_map_[id].geometry_handlers.push_back (geometry_handler);
00187     //cloud_actor_map_[id].color_handlers.push_back (color_handler);
00188     //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_));
00189     return (false);
00190   }
00191   return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport));
00192 }
00193 
00195 template <typename PointT> void 
00196 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
00197     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00198     vtkSmartPointer<vtkPolyData> &polydata,
00199     vtkSmartPointer<vtkIdTypeArray> &initcells)
00200 {
00201   vtkSmartPointer<vtkCellArray> vertices;
00202   if (!polydata)
00203   {
00204     allocVtkPolyData (polydata);
00205     vertices = vtkSmartPointer<vtkCellArray>::New ();
00206     polydata->SetVerts (vertices);
00207   }
00208 
00209   // Create the supporting structures
00210   vertices = polydata->GetVerts ();
00211   if (!vertices)
00212     vertices = vtkSmartPointer<vtkCellArray>::New ();
00213 
00214   vtkIdType nr_points = cloud->points.size ();
00215   // Create the point set
00216   vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
00217   if (!points)
00218   {
00219     points = vtkSmartPointer<vtkPoints>::New ();
00220     points->SetDataTypeToFloat ();
00221     polydata->SetPoints (points);
00222   }
00223   points->SetNumberOfPoints (nr_points);
00224 
00225   // Get a pointer to the beginning of the data array
00226   float *data = ((vtkFloatArray*)points->GetData ())->GetPointer (0);
00227 
00228   // Set the points
00229   if (cloud->is_dense)
00230   {
00231     for (vtkIdType i = 0; i < nr_points; ++i)
00232       memcpy (&data[i * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
00233   }
00234   else
00235   {
00236     vtkIdType j = 0;    // true point index
00237     for (vtkIdType i = 0; i < nr_points; ++i)
00238     {
00239       // Check if the point is invalid
00240       if (!pcl_isfinite (cloud->points[i].x) || 
00241           !pcl_isfinite (cloud->points[i].y) || 
00242           !pcl_isfinite (cloud->points[i].z))
00243         continue;
00244 
00245       memcpy (&data[j * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
00246       j++;
00247     }
00248     nr_points = j;
00249     points->SetNumberOfPoints (nr_points);
00250   }
00251 
00252   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
00253   updateCells (cells, initcells, nr_points);
00254 
00255   // Set the cells and the vertices
00256   vertices->SetCells (nr_points, cells);
00257 }
00258 
00260 template <typename PointT> void 
00261 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
00262     const pcl::visualization::PointCloudGeometryHandler<PointT> &geometry_handler, 
00263     vtkSmartPointer<vtkPolyData> &polydata,
00264     vtkSmartPointer<vtkIdTypeArray> &initcells)
00265 {
00266   vtkSmartPointer<vtkCellArray> vertices;
00267   if (!polydata)
00268   {
00269     allocVtkPolyData (polydata);
00270     vertices = vtkSmartPointer<vtkCellArray>::New ();
00271     polydata->SetVerts (vertices);
00272   }
00273 
00274   // Use the handler to obtain the geometry
00275   vtkSmartPointer<vtkPoints> points;
00276   geometry_handler.getGeometry (points);
00277   polydata->SetPoints (points);
00278 
00279   vtkIdType nr_points = points->GetNumberOfPoints ();
00280 
00281   // Create the supporting structures
00282   vertices = polydata->GetVerts ();
00283   if (!vertices)
00284     vertices = vtkSmartPointer<vtkCellArray>::New ();
00285 
00286   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
00287   updateCells (cells, initcells, nr_points);  
00288   // Set the cells and the vertices
00289   vertices->SetCells (nr_points, cells);
00290 }
00291 
00293 template <typename PointT> bool
00294 pcl::visualization::PCLVisualizer::addPolygon (
00295     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00296     double r, double g, double b, const std::string &id, int viewport)
00297 {
00298   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00299   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00300   if (am_it != shape_actor_map_->end ())
00301   {
00302     PCL_WARN ("[addPolygon] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00303     return (false);
00304   }
00305 
00306   vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (cloud);
00307 
00308   // Create an Actor
00309   vtkSmartPointer<vtkLODActor> actor;
00310   createActorFromVTKDataSet (data, actor);
00311   actor->GetProperty ()->SetRepresentationToWireframe ();
00312   actor->GetProperty ()->SetColor (r, g, b);
00313   actor->GetMapper ()->ScalarVisibilityOff ();
00314   addActorToRenderer (actor, viewport);
00315 
00316   // Save the pointer/ID pair to the global actor map
00317   (*shape_actor_map_)[id] = actor;
00318   return (true);
00319 }
00320 
00322 template <typename PointT> bool
00323 pcl::visualization::PCLVisualizer::addPolygon (
00324     const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00325     const std::string &id, int viewport)
00326 {
00327   return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5, id, viewport));
00328     return (false);
00329 }
00330 
00332 template <typename P1, typename P2> bool
00333 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
00334 {
00335   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00336   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00337   if (am_it != shape_actor_map_->end ())
00338   {
00339     PCL_WARN ("[addLine] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00340     return (false);
00341   }
00342 
00343   vtkSmartPointer<vtkDataSet> data = createLine (pt1.getVector4fMap (), pt2.getVector4fMap ());
00344 
00345   // Create an Actor
00346   vtkSmartPointer<vtkLODActor> actor;
00347   createActorFromVTKDataSet (data, actor);
00348   actor->GetProperty ()->SetRepresentationToWireframe ();
00349   actor->GetProperty ()->SetColor (r, g, b);
00350   actor->GetMapper ()->ScalarVisibilityOff ();
00351   addActorToRenderer (actor, viewport);
00352 
00353   // Save the pointer/ID pair to the global actor map
00354   (*shape_actor_map_)[id] = actor;
00355   return (true);
00356 }
00357 
00359 template <typename P1, typename P2> bool
00360 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
00361 {
00362   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00363   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00364   if (am_it != shape_actor_map_->end ())
00365   {
00366     PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00367     return (false);
00368   }
00369 
00370   // Create an Actor
00371   vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
00372   leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
00373   leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
00374   leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
00375   leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
00376   leader->SetArrowStyleToFilled ();
00377   leader->AutoLabelOn ();
00378 
00379   leader->GetProperty ()->SetColor (r, g, b);
00380   addActorToRenderer (leader, viewport);
00381 
00382   // Save the pointer/ID pair to the global actor map
00383   (*shape_actor_map_)[id] = leader;
00384   return (true);
00385 }
00386 
00388 template <typename P1, typename P2> bool
00389 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, const std::string &id, int viewport)
00390 {
00391   return (!addLine (pt1, pt2, 0.5, 0.5, 0.5, id, viewport));
00392 }
00393 
00395 /*template <typename P1, typename P2> bool
00396   pcl::visualization::PCLVisualizer::addLineToGroup (const P1 &pt1, const P2 &pt2, const std::string &group_id, int viewport)
00397 {
00398   vtkSmartPointer<vtkDataSet> data = createLine<P1, P2> (pt1, pt2);
00399 
00400   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00401   ShapeActorMap::iterator am_it = shape_actor_map_->find (group_id);
00402   if (am_it != shape_actor_map_->end ())
00403   {
00404     // Get the old data
00405     vtkPolyDataMapper* mapper = static_cast<vtkPolyDataMapper*>(am_it->second->GetMapper ());
00406     vtkPolyData* olddata = static_cast<vtkPolyData*>(mapper->GetInput ());
00407     // Add it to the current data
00408     vtkSmartPointer<vtkAppendPolyData> alldata = vtkSmartPointer<vtkAppendPolyData>::New ();
00409     alldata->AddInput (olddata);
00410 
00411     // Convert to vtkPolyData
00412     vtkSmartPointer<vtkPolyData> curdata = (vtkPolyData::SafeDownCast (data));
00413     alldata->AddInput (curdata);
00414 
00415     mapper->SetInput (alldata->GetOutput ());
00416 
00417     am_it->second->SetMapper (mapper);
00418     am_it->second->Modified ();
00419     return (true);
00420   }
00421 
00422   // Create an Actor
00423   vtkSmartPointer<vtkLODActor> actor;
00424   createActorFromVTKDataSet (data, actor);
00425   actor->GetProperty ()->SetRepresentationToWireframe ();
00426   actor->GetProperty ()->SetColor (1, 0, 0);
00427   addActorToRenderer (actor, viewport);
00428 
00429   // Save the pointer/ID pair to the global actor map
00430   (*shape_actor_map_)[group_id] = actor;
00431 
00432 //ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00433 //vtkSmartPointer<vtkLODActor> actor = am_it->second;
00434   //actor->GetProperty ()->SetColor (r, g, b);
00435 //actor->Modified ();
00436   return (true);
00437 }*/
00438 
00440 template <typename PointT> bool
00441 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id, int viewport)
00442 {
00443   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00444   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00445   if (am_it != shape_actor_map_->end ())
00446   {
00447     PCL_WARN ("[addSphere] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00448     return (false);
00449   }
00450 
00451   vtkSmartPointer<vtkDataSet> data = createSphere (center.getVector4fMap (), radius);
00452 
00453   // Create an Actor
00454   vtkSmartPointer<vtkLODActor> actor;
00455   createActorFromVTKDataSet (data, actor);
00456   actor->GetProperty ()->SetRepresentationToWireframe ();
00457   actor->GetProperty ()->SetInterpolationToGouraud ();
00458   actor->GetMapper ()->ScalarVisibilityOff ();
00459   actor->GetProperty ()->SetColor (r, g, b);
00460   addActorToRenderer (actor, viewport);
00461 
00462   // Save the pointer/ID pair to the global actor map
00463   (*shape_actor_map_)[id] = actor;
00464   return (true);
00465 }
00466 
00468 template <typename PointT> bool
00469 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, const std::string &id, int viewport)
00470 {
00471   return (addSphere (center, radius, 0.5, 0.5, 0.5, id, viewport));
00472 }
00473 
00475 template <typename PointT> bool
00476 pcl::visualization::PCLVisualizer::addText3D (
00477     const std::string &text, 
00478     const PointT& position, 
00479     double textScale, 
00480     double r, 
00481     double g, 
00482     double b, 
00483     const std::string &id, 
00484     int viewport)
00485 {
00486   std::string tid;
00487   if (id.empty ())
00488     tid = text;
00489   else
00490     tid = id;
00491 
00492   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00493   ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
00494   if (am_it != shape_actor_map_->end ())
00495   {
00496     pcl::console::print_warn ("[addText3d] A text with id <%s> already exists! Please choose a different id and retry.\n", tid.c_str ());
00497     return (false);
00498   }
00499 
00500   vtkSmartPointer<vtkVectorText> textSource = vtkSmartPointer<vtkVectorText>::New ();
00501   textSource->SetText (text.c_str());
00502   textSource->Update ();
00503 
00504   vtkSmartPointer<vtkPolyDataMapper> textMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
00505   textMapper->SetInputConnection (textSource->GetOutputPort ());
00506 
00507   // Since each follower may follow a different camera, we need different followers
00508   rens_->InitTraversal ();
00509   vtkRenderer* renderer = NULL;
00510   int i = 1;
00511   while ((renderer = rens_->GetNextItem ()) != NULL)
00512   {
00513     // Should we add the actor to all renderers or just to i-nth renderer?
00514     if (viewport == 0 || viewport == i)               
00515     {
00516       vtkSmartPointer<vtkFollower> textActor = vtkSmartPointer<vtkFollower>::New ();
00517       textActor->SetMapper (textMapper);
00518       textActor->SetPosition (position.x, position.y, position.z);
00519       textActor->SetScale (textScale);
00520       textActor->GetProperty ()->SetColor (r, g, b);
00521       textActor->SetCamera (renderer->GetActiveCamera ());
00522 
00523       renderer->AddActor (textActor);
00524       renderer->Render ();
00525 
00526       // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers 
00527       // for multiple viewport
00528       std::string alternate_tid = tid;
00529       alternate_tid.append(i, '*');
00530 
00531       (*shape_actor_map_)[(viewport == 0) ? tid : alternate_tid] = textActor;
00532     }
00533 
00534     ++i;
00535   }
00536 
00537   return (true);
00538 }
00539 
00541 template <typename PointNT> bool
00542 pcl::visualization::PCLVisualizer::addPointCloudNormals (
00543     const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
00544     int level, double scale, const std::string &id, int viewport)
00545 {
00546   return (addPointCloudNormals<PointNT, PointNT>(cloud, cloud, level, scale, id, viewport));
00547 }
00548 
00550 template <typename PointT, typename PointNT> bool
00551 pcl::visualization::PCLVisualizer::addPointCloudNormals (
00552     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
00553     const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
00554     int level, double scale,
00555     const std::string &id, int viewport)
00556 {
00557   if (normals->points.size () != cloud->points.size ())
00558   {
00559     PCL_ERROR ("[addPointCloudNormals] The number of points differs from the number of normals!\n");
00560     return (false);
00561   }
00562   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00563   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00564 
00565   if (am_it != cloud_actor_map_->end ())
00566   {
00567     PCL_WARN ("[addPointCloudNormals] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00568     return (false);
00569   }
00570 
00571   vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
00572   vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
00573 
00574   points->SetDataTypeToFloat ();
00575   vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
00576   data->SetNumberOfComponents (3);
00577   
00578   vtkIdType nr_normals = (cloud->points.size () - 1) / level + 1 ;
00579   float* pts = new float[2 * nr_normals * 3];
00580 
00581   for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
00582   {
00583     PointT p = cloud->points[i];
00584     p.x += normals->points[i].normal[0] * scale; 
00585     p.y += normals->points[i].normal[1] * scale; 
00586     p.z += normals->points[i].normal[2] * scale;
00587     
00588     pts[2 * j * 3 + 0] = cloud->points[i].x;
00589     pts[2 * j * 3 + 1] = cloud->points[i].y;
00590     pts[2 * j * 3 + 2] = cloud->points[i].z;
00591     pts[2 * j * 3 + 3] = p.x;
00592     pts[2 * j * 3 + 4] = p.y;
00593     pts[2 * j * 3 + 5] = p.z;
00594 
00595     lines->InsertNextCell(2);
00596     lines->InsertCellPoint(2*j);
00597     lines->InsertCellPoint(2*j+1);
00598   }
00599 
00600   data->SetArray (&pts[0], 2 * nr_normals * 3, 0);
00601   points->SetData (data);
00602 
00603   vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
00604   polyData->SetPoints(points);
00605   polyData->SetLines(lines);
00606 
00607   vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();      
00608   mapper->SetInput (polyData);
00609   mapper->SetColorModeToMapScalars();
00610   mapper->SetScalarModeToUsePointData();
00611 
00612   // create actor
00613   vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
00614   actor->SetMapper(mapper);
00615   
00616   // Add it to all renderers
00617   addActorToRenderer (actor, viewport);
00618 
00619   // Save the pointer/ID pair to the global actor map
00620   (*cloud_actor_map_)[id].actor = actor;
00621   return (true);
00622 }
00623 
00625 template <typename PointT> bool
00626 pcl::visualization::PCLVisualizer::addCorrespondences (
00627    const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
00628    const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
00629    const std::vector<int> &correspondences,
00630    const std::string &id,
00631    int viewport)
00632 {
00633   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00634   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00635   if (am_it != shape_actor_map_->end ())
00636   {
00637     PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00638     return (false);
00639   }
00640 
00641   vtkSmartPointer<vtkAppendPolyData> polydata = vtkSmartPointer<vtkAppendPolyData>::New ();
00642 
00643   vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00644   line_colors->SetNumberOfComponents (3);
00645   line_colors->SetName ("Colors");
00646   // Use Red by default (can be changed later)
00647   unsigned char rgb[3];
00648   rgb[0] = 1 * 255.0;
00649   rgb[1] = 0 * 255.0;
00650   rgb[2] = 0 * 255.0;
00651 
00652   // Draw lines between the best corresponding points
00653   for (size_t i = 0; i < source_points->size (); ++i)
00654   {
00655     const PointT &p_src = source_points->points[i];
00656     const PointT &p_tgt = target_points->points[correspondences[i]];
00657     
00658     // Add the line
00659     vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
00660     line->SetPoint1 (p_src.x, p_src.y, p_src.z);
00661     line->SetPoint2 (p_tgt.x, p_tgt.y, p_tgt.z);
00662     line->Update ();
00663     polydata->AddInput (line->GetOutput ());
00664     line_colors->InsertNextTupleValue (rgb);
00665   }
00666   polydata->Update ();
00667   vtkSmartPointer<vtkPolyData> line_data = polydata->GetOutput ();
00668   line_data->GetCellData ()->SetScalars (line_colors);
00669 
00670   // Create an Actor
00671   vtkSmartPointer<vtkLODActor> actor;
00672   createActorFromVTKDataSet (line_data, actor);
00673   actor->GetProperty ()->SetRepresentationToWireframe ();
00674   actor->GetProperty ()->SetOpacity (0.5);
00675   addActorToRenderer (actor, viewport);
00676 
00677   // Save the pointer/ID pair to the global actor map
00678   (*shape_actor_map_)[id] = actor;
00679 
00680   return (true);
00681 }
00682 
00684 template <typename PointT> bool
00685 pcl::visualization::PCLVisualizer::addCorrespondences (
00686    const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
00687    const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
00688    const pcl::Correspondences &correspondences,
00689    const std::string &id,
00690    int viewport)
00691 {
00692   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00693   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
00694   if (am_it != shape_actor_map_->end ())
00695   {
00696     PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
00697     return (false);
00698   }
00699 
00700   vtkSmartPointer<vtkAppendPolyData> polydata = vtkSmartPointer<vtkAppendPolyData>::New ();
00701 
00702   vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
00703   line_colors->SetNumberOfComponents (3);
00704   line_colors->SetName ("Colors");
00705   unsigned char rgb[3];
00706   // Use Red by default (can be changed later)
00707   rgb[0] = 1 * 255.0;
00708   rgb[1] = 0 * 255.0;
00709   rgb[2] = 0 * 255.0;
00710 
00711   // Draw lines between the best corresponding points
00712   for (size_t i = 0; i < correspondences.size (); ++i)
00713   {
00714     const PointT &p_src = source_points->points[correspondences[i].index_query];
00715     const PointT &p_tgt = target_points->points[correspondences[i].index_match];
00716     
00717     // Add the line
00718     vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
00719     line->SetPoint1 (p_src.x, p_src.y, p_src.z);
00720     line->SetPoint2 (p_tgt.x, p_tgt.y, p_tgt.z);
00721     line->Update ();
00722     polydata->AddInput (line->GetOutput ());
00723     line_colors->InsertNextTupleValue (rgb);
00724   }
00725   polydata->Update ();
00726   vtkSmartPointer<vtkPolyData> line_data = polydata->GetOutput ();
00727   line_data->GetCellData ()->SetScalars (line_colors);
00728 
00729   // Create an Actor
00730   vtkSmartPointer<vtkLODActor> actor;
00731   createActorFromVTKDataSet (line_data, actor);
00732   actor->GetProperty ()->SetRepresentationToWireframe ();
00733   actor->GetProperty ()->SetOpacity (0.5);
00734   addActorToRenderer (actor, viewport);
00735 
00736   // Save the pointer/ID pair to the global actor map
00737   (*shape_actor_map_)[id] = actor;
00738 
00739   return (true);
00740 }
00741 
00743 template <typename PointT> bool
00744 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
00745     const PointCloudGeometryHandler<PointT> &geometry_handler,
00746     const PointCloudColorHandler<PointT> &color_handler, 
00747     const std::string &id,
00748     int viewport)
00749 {
00750   if (!geometry_handler.isCapable ())
00751   {
00752     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
00753     return (false);
00754   }
00755 
00756   if (!color_handler.isCapable ())
00757   {
00758     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
00759     return (false);
00760   }
00761 
00762   vtkSmartPointer<vtkPolyData> polydata;
00763   vtkSmartPointer<vtkIdTypeArray> initcells;
00764   // Convert the PointCloud to VTK PolyData
00765   convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
00766   // use the given geometry handler
00767   polydata->Update ();
00768 
00769   // Get the colors from the handler
00770   vtkSmartPointer<vtkDataArray> scalars;
00771   color_handler.getColor (scalars);
00772   polydata->GetPointData ()->SetScalars (scalars);
00773 
00774   // Create an Actor
00775   vtkSmartPointer<vtkLODActor> actor;
00776   createActorFromVTKDataSet (polydata, actor);
00777 
00778   // Add it to all renderers
00779   addActorToRenderer (actor, viewport);
00780 
00781   // Save the pointer/ID pair to the global actor map
00782   (*cloud_actor_map_)[id].actor = actor;
00783   (*cloud_actor_map_)[id].cells = initcells;
00784   return (true);
00785 }
00786 
00788 template <typename PointT> bool
00789 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
00790     const PointCloudGeometryHandler<PointT> &geometry_handler,
00791     const ColorHandlerConstPtr &color_handler, 
00792     const std::string &id,
00793     int viewport)
00794 {
00795   if (!geometry_handler.isCapable ())
00796   {
00797     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
00798     return (false);
00799   }
00800 
00801   if (!color_handler->isCapable ())
00802   {
00803     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler->getName ().c_str ());
00804     return (false);
00805   }
00806 
00807   vtkSmartPointer<vtkPolyData> polydata;
00808   vtkSmartPointer<vtkIdTypeArray> initcells;
00809   // Convert the PointCloud to VTK PolyData
00810   convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
00811   // use the given geometry handler
00812   polydata->Update ();
00813 
00814   // Get the colors from the handler
00815   vtkSmartPointer<vtkDataArray> scalars;
00816   color_handler->getColor (scalars);
00817   polydata->GetPointData ()->SetScalars (scalars);
00818 
00819   // Create an Actor
00820   vtkSmartPointer<vtkLODActor> actor;
00821   createActorFromVTKDataSet (polydata, actor);
00822 
00823   // Add it to all renderers
00824   addActorToRenderer (actor, viewport);
00825 
00826   // Save the pointer/ID pair to the global actor map
00827   (*cloud_actor_map_)[id].actor = actor;
00828   (*cloud_actor_map_)[id].cells = initcells;
00829   (*cloud_actor_map_)[id].color_handlers.push_back (color_handler);
00830   return (true);
00831 }
00832 
00834 template <typename PointT> bool
00835 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
00836     const GeometryHandlerConstPtr &geometry_handler,
00837     const PointCloudColorHandler<PointT> &color_handler, 
00838     const std::string &id,
00839     int viewport)
00840 {
00841   if (!geometry_handler->isCapable ())
00842   {
00843     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler->getName ().c_str ());
00844     return (false);
00845   }
00846 
00847   if (!color_handler.isCapable ())
00848   {
00849     PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
00850     return (false);
00851   }
00852 
00853   vtkSmartPointer<vtkPolyData> polydata;
00854   vtkSmartPointer<vtkIdTypeArray> initcells;
00855   // Convert the PointCloud to VTK PolyData
00856   convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
00857   // use the given geometry handler
00858   polydata->Update ();
00859 
00860   // Get the colors from the handler
00861   vtkSmartPointer<vtkDataArray> scalars;
00862   color_handler.getColor (scalars);
00863   polydata->GetPointData ()->SetScalars (scalars);
00864 
00865   // Create an Actor
00866   vtkSmartPointer<vtkLODActor> actor;
00867   createActorFromVTKDataSet (polydata, actor);
00868 
00869   // Add it to all renderers
00870   addActorToRenderer (actor, viewport);
00871 
00872   // Save the pointer/ID pair to the global actor map
00873   (*cloud_actor_map_)[id].actor = actor;
00874   (*cloud_actor_map_)[id].cells = initcells;
00875   (*cloud_actor_map_)[id].geometry_handlers.push_back (geometry_handler);
00876   return (true);
00877 }
00878 
00880 template <typename PointT> bool
00881 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00882                                                      const std::string &id)
00883 {
00884   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00885   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00886 
00887   if (am_it == cloud_actor_map_->end ())
00888     return (false);
00889 
00890   vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
00891   // Convert the PointCloud to VTK PolyData
00892   convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
00893   polydata->Update ();
00894 
00895   // Set scalars to blank, since there is no way we can update them here. 
00896   vtkSmartPointer<vtkDataArray> scalars;
00897   polydata->GetPointData ()->SetScalars (scalars);
00898   polydata->Update ();
00899   am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
00900 
00901   // Update the mapper
00902   reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
00903   return (true);
00904 }
00905 
00907 template <typename PointT> bool
00908 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00909                                                      const PointCloudGeometryHandler<PointT> &geometry_handler,
00910                                                      const std::string &id)
00911 {
00912   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00913   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00914 
00915   if (am_it == cloud_actor_map_->end ())
00916     return (false);
00917 
00918   vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
00919   if (!polydata)
00920     return (false);
00921   // Convert the PointCloud to VTK PolyData
00922   convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
00923 
00924   // Set scalars to blank, since there is no way we can update them here. 
00925   vtkSmartPointer<vtkDataArray> scalars;
00926   polydata->GetPointData ()->SetScalars (scalars);
00927   polydata->Update ();
00928   am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
00929 
00930   // Update the mapper
00931   reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
00932   return (true);
00933 }
00934 
00935 
00937 template <typename PointT> bool
00938 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 
00939                                                      const PointCloudColorHandler<PointT> &color_handler,
00940                                                      const std::string &id)
00941 {
00942   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
00943   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
00944 
00945   if (am_it == cloud_actor_map_->end ())
00946     return (false);
00947 
00948   // Get the current poly data
00949   vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
00950   if (!polydata)
00951     return (false);
00952   vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
00953   vtkSmartPointer<vtkPoints> points      = polydata->GetPoints ();
00954   // Copy the new point array in
00955   vtkIdType nr_points = cloud->points.size ();
00956   points->SetNumberOfPoints (nr_points);
00957   
00958   // Get a pointer to the beginning of the data array
00959   float *data = ((vtkFloatArray*)points->GetData ())->GetPointer (0);
00960 
00961   // If the dataset is dense (no NaNs)
00962   if (cloud->is_dense)
00963   {
00964     for (vtkIdType i = 0; i < nr_points; ++i)
00965       memcpy (&data[i * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
00966   }
00967   else
00968   {
00969     vtkIdType j = 0;    // true point index
00970     for (vtkIdType i = 0; i < nr_points; ++i)
00971     {
00972       // Check if the point is invalid
00973       if (!pcl_isfinite (cloud->points[i].x) || 
00974           !pcl_isfinite (cloud->points[i].y) || 
00975           !pcl_isfinite (cloud->points[i].z))
00976         continue;
00977 
00978       memcpy (&data[j * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
00979       j++;
00980     }
00981     nr_points = j;
00982     points->SetNumberOfPoints (nr_points);
00983   }
00984 
00985   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
00986   updateCells (cells, am_it->second.cells, nr_points);
00987 
00988   // Set the cells and the vertices
00989   vertices->SetCells (nr_points, cells);
00990 
00991   // Get the colors from the handler
00992   vtkSmartPointer<vtkDataArray> scalars;
00993   color_handler.getColor (scalars);
00994   polydata->GetPointData ()->SetScalars (scalars);
00995   polydata->Update ();
00996   
00997   am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
00998 
00999   // Update the mapper
01000   reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
01001   return (true);
01002 }
01003 
01005 template <typename PointT> bool
01006 pcl::visualization::PCLVisualizer::addPolygonMesh (
01007     const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
01008     const std::vector<pcl::Vertices> &vertices,
01009     const std::string &id,
01010     int viewport)
01011 {
01012   ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
01013   if (am_it != shape_actor_map_->end ())
01014   {
01015     pcl::console::print_warn (
01016                                 "[addPolygonMesh] A shape with id <%s> already exists! Please choose a different id and retry.\n",
01017                                 id.c_str ());
01018     return (false);
01019   }
01020 
01021   // Create points from polyMesh.cloud
01022   vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
01023   vtkIdType nr_points = cloud->points.size ();
01024   points->SetNumberOfPoints (nr_points);
01025 
01026   // Get a pointer to the beginning of the data array
01027   float *data = ((vtkFloatArray*)points->GetData ())->GetPointer (0);
01028 
01029   // If the dataset is dense (no NaNs)
01030   if (cloud->is_dense)
01031   {
01032     for (vtkIdType i = 0; i < nr_points; ++i)
01033       memcpy (&data[i * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
01034   }
01035   else
01036   {
01037     vtkIdType j = 0;    // true point index
01038     for (vtkIdType i = 0; i < nr_points; ++i)
01039     {
01040       // Check if the point is invalid
01041       if (!pcl_isfinite (cloud->points[i].x) || 
01042           !pcl_isfinite (cloud->points[i].y) || 
01043           !pcl_isfinite (cloud->points[i].z))
01044         continue;
01045 
01046       memcpy (&data[j * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
01047       j++;
01048     }
01049     nr_points = j;
01050     points->SetNumberOfPoints (nr_points);
01051   }
01052 
01053   vtkSmartPointer<vtkLODActor> actor;
01054   if (vertices.size () > 1) 
01055   {
01056     //create polys from polyMesh.polygons
01057     vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New ();
01058     
01059     for (size_t i = 0; i < vertices.size (); ++i) 
01060     {
01061       size_t n_points = vertices[i].vertices.size ();
01062       cell_array->InsertNextCell (n_points);
01063       for (size_t j = 0; j < n_points; j++) 
01064         cell_array->InsertCellPoint (vertices[i].vertices[j]);
01065     }
01066 
01067     vtkSmartPointer<vtkPolyData> polydata;
01068     allocVtkPolyData (polydata);
01069     polydata->SetStrips (cell_array);
01070     polydata->SetPoints (points);
01071 
01072     createActorFromVTKDataSet (polydata, actor);
01073   } 
01074   else 
01075   {
01076     vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
01077     size_t n_points = vertices[0].vertices.size ();
01078     polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
01079 
01080     for (size_t j = 0; j < (n_points - 1); ++j) 
01081       polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
01082 
01083     vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
01084     allocVtkUnstructuredGrid (poly_grid);
01085     poly_grid->Allocate (1, 1);
01086     poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
01087     poly_grid->SetPoints (points);
01088     poly_grid->Update ();
01089 
01090     createActorFromVTKDataSet (poly_grid, actor);
01091   }
01092 
01093   actor->GetProperty ()->SetRepresentationToWireframe ();
01094   addActorToRenderer (actor, viewport);
01095 
01096   // Save the pointer/ID pair to the global actor map
01097   (*shape_actor_map_)[id] = actor;
01098   return (true);
01099 }
01100 
01101 
01103 /* Optimized function: need to do something with the signature as it colides with the general T one
01104 bool
01105 pcl::visualization::PCLVisualizer::updatePointCloud (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &cloud, 
01106                                                      const PointCloudColorHandlerRGBField<pcl::PointXYZRGB> &color_handler,
01107                                                      const std::string &id)
01108 {
01109   win_->SetAbortRender (1);
01110   // Check to see if this ID entry already exists (has it been already added to the visualizer?)
01111   CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
01112 
01113   if (am_it == cloud_actor_map_->end ())
01114     return (false);
01115 
01116   // Get the current poly data
01117   vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
01118   if (!polydata)
01119     return (false);
01120   vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
01121   vtkSmartPointer<vtkPoints> points      = polydata->GetPoints ();
01122 //  vtkUnsignedCharArray* scalars = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
01123   vtkSmartPointer<vtkUnsignedCharArray> scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
01124   // Copy the new point array in
01125   vtkIdType nr_points = cloud->points.size ();
01126   points->SetNumberOfPoints (nr_points);
01127   scalars->SetNumberOfComponents (3);
01128   scalars->SetNumberOfTuples (nr_points);
01129   polydata->GetPointData ()->SetScalars (scalars);
01130   unsigned char* colors = scalars->GetPointer (0);
01131  
01132   // Get a pointer to the beginning of the data array
01133   float *data = ((vtkFloatArray*)points->GetData ())->GetPointer (0);
01134 
01135   // If the dataset is dense (no NaNs)
01136   if (cloud->is_dense)
01137   {
01138     for (vtkIdType i = 0; i < nr_points; ++i)
01139     {
01140       memcpy (&data[i * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
01141       int idx = i * 3;
01142       colors[idx    ] = cloud->points[i].r;
01143       colors[idx + 1] = cloud->points[i].g;
01144       colors[idx + 2] = cloud->points[i].b;
01145     }
01146   }
01147   else
01148   {
01149     vtkIdType j = 0;    // true point index
01150     for (vtkIdType i = 0; i < nr_points; ++i)
01151     {
01152       // Check if the point is invalid
01153       if (!pcl_isfinite (cloud->points[i].x) || 
01154           !pcl_isfinite (cloud->points[i].y) || 
01155           !pcl_isfinite (cloud->points[i].z))
01156         continue;
01157 
01158       memcpy (&data[j * 3], &cloud->points[i].x, 12);    // sizeof (float) * 3
01159       int idx = j * 3;
01160       colors[idx    ] = cloud->points[i].r;
01161       colors[idx + 1] = cloud->points[i].g;
01162       colors[idx + 2] = cloud->points[i].b;
01163       j++;
01164     }
01165     nr_points = j;
01166     points->SetNumberOfPoints (nr_points);
01167     scalars->SetNumberOfTuples (nr_points);
01168   }
01169 
01170   vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
01171   updateCells (cells, am_it->second.cells, nr_points);
01172 
01173   // Set the cells and the vertices
01174   vertices->SetCells (nr_points, cells);
01175 
01176   // Update the mapper
01177   reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
01178   return (true);
01179 }*/
01180 
01181 
01182 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines