Point Cloud Library (PCL)
1.3.1
|
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 ¢er, 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 ¢er, 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