Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) 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 */ 00037 00038 #ifndef PCL_PCL_VISUALIZER_IMPL_H_ 00039 #define PCL_PCL_VISUALIZER_IMPL_H_ 00040 00041 #include <vtkSmartPointer.h> 00042 #include <vtkCellArray.h> 00043 #include <vtkLeaderActor2D.h> 00044 #include <vtkVectorText.h> 00045 #include <vtkAlgorithmOutput.h> 00046 #include <vtkFollower.h> 00047 #include <vtkSphereSource.h> 00048 #include <vtkProperty2D.h> 00049 #include <vtkDataSetSurfaceFilter.h> 00050 #include <vtkPointData.h> 00051 #include <vtkPolyDataMapper.h> 00052 #include <vtkProperty.h> 00053 #include <vtkMapper.h> 00054 #include <vtkCellData.h> 00055 #include <vtkDataSetMapper.h> 00056 #include <vtkRenderer.h> 00057 #include <vtkRendererCollection.h> 00058 #include <vtkAppendPolyData.h> 00059 #include <vtkTextProperty.h> 00060 #include <vtkLODActor.h> 00061 00062 #include <pcl/visualization/common/shapes.h> 00063 00064 ////////////////////////////////////////////////////////////////////////////////////////////// 00065 template <typename PointT> bool 00066 pcl::visualization::PCLVisualizer::addPointCloud ( 00067 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00068 const std::string &id, int viewport) 00069 { 00070 // Convert the PointCloud to VTK PolyData 00071 PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud); 00072 return (addPointCloud<PointT> (cloud, geometry_handler, id, viewport)); 00073 } 00074 00075 ////////////////////////////////////////////////////////////////////////////////////////////// 00076 template <typename PointT> bool 00077 pcl::visualization::PCLVisualizer::addPointCloud ( 00078 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00079 const PointCloudGeometryHandler<PointT> &geometry_handler, 00080 const std::string &id, int viewport) 00081 { 00082 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00083 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 00084 00085 if (am_it != cloud_actor_map_->end ()) 00086 { 00087 PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00088 return (false); 00089 } 00090 00091 //PointCloudColorHandlerRandom<PointT> color_handler (cloud); 00092 PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255); 00093 return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_)); 00094 } 00095 00096 ////////////////////////////////////////////////////////////////////////////////////////////// 00097 template <typename PointT> bool 00098 pcl::visualization::PCLVisualizer::addPointCloud ( 00099 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00100 const GeometryHandlerConstPtr &geometry_handler, 00101 const std::string &id, int viewport) 00102 { 00103 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00104 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 00105 00106 if (am_it != cloud_actor_map_->end ()) 00107 { 00108 // Here we're just pushing the handlers onto the queue. If needed, something fancier could 00109 // be done such as checking if a specific handler already exists, etc. 00110 am_it->second.geometry_handlers.push_back (geometry_handler); 00111 return (true); 00112 } 00113 00114 //PointCloudColorHandlerRandom<PointT> color_handler (cloud); 00115 PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255); 00116 return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_)); 00117 } 00118 00119 ////////////////////////////////////////////////////////////////////////////////////////////// 00120 template <typename PointT> bool 00121 pcl::visualization::PCLVisualizer::addPointCloud ( 00122 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00123 const PointCloudColorHandler<PointT> &color_handler, 00124 const std::string &id, int viewport) 00125 { 00126 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00127 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 00128 00129 if (am_it != cloud_actor_map_->end ()) 00130 { 00131 PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00132 00133 // Here we're just pushing the handlers onto the queue. If needed, something fancier could 00134 // be done such as checking if a specific handler already exists, etc. 00135 //cloud_actor_map_[id].color_handlers.push_back (color_handler); 00136 //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_)); 00137 return (false); 00138 } 00139 // Convert the PointCloud to VTK PolyData 00140 PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud); 00141 return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_)); 00142 } 00143 00144 ////////////////////////////////////////////////////////////////////////////////////////////// 00145 template <typename PointT> bool 00146 pcl::visualization::PCLVisualizer::addPointCloud ( 00147 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00148 const ColorHandlerConstPtr &color_handler, 00149 const std::string &id, int viewport) 00150 { 00151 // Check to see if this entry already exists (has it been already added to the visualizer?) 00152 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 00153 if (am_it != cloud_actor_map_->end ()) 00154 { 00155 // Here we're just pushing the handlers onto the queue. If needed, something fancier could 00156 // be done such as checking if a specific handler already exists, etc. 00157 am_it->second.color_handlers.push_back (color_handler); 00158 return (true); 00159 } 00160 00161 PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud); 00162 return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_)); 00163 } 00164 00165 ////////////////////////////////////////////////////////////////////////////////////////////// 00166 template <typename PointT> bool 00167 pcl::visualization::PCLVisualizer::addPointCloud ( 00168 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00169 const GeometryHandlerConstPtr &geometry_handler, 00170 const ColorHandlerConstPtr &color_handler, 00171 const std::string &id, int viewport) 00172 { 00173 // Check to see if this entry already exists (has it been already added to the visualizer?) 00174 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 00175 if (am_it != cloud_actor_map_->end ()) 00176 { 00177 // Here we're just pushing the handlers onto the queue. If needed, something fancier could 00178 // be done such as checking if a specific handler already exists, etc. 00179 am_it->second.geometry_handlers.push_back (geometry_handler); 00180 am_it->second.color_handlers.push_back (color_handler); 00181 return (true); 00182 } 00183 return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_)); 00184 } 00185 00186 ////////////////////////////////////////////////////////////////////////////////////////////// 00187 template <typename PointT> bool 00188 pcl::visualization::PCLVisualizer::addPointCloud ( 00189 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00190 const PointCloudColorHandler<PointT> &color_handler, 00191 const PointCloudGeometryHandler<PointT> &geometry_handler, 00192 const std::string &id, int viewport) 00193 { 00194 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00195 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 00196 00197 if (am_it != cloud_actor_map_->end ()) 00198 { 00199 PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00200 // Here we're just pushing the handlers onto the queue. If needed, something fancier could 00201 // be done such as checking if a specific handler already exists, etc. 00202 //cloud_actor_map_[id].geometry_handlers.push_back (geometry_handler); 00203 //cloud_actor_map_[id].color_handlers.push_back (color_handler); 00204 //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_)); 00205 return (false); 00206 } 00207 return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_)); 00208 } 00209 00210 ////////////////////////////////////////////////////////////////////////////////////////////// 00211 template <typename PointT> void 00212 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData ( 00213 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00214 vtkSmartPointer<vtkPolyData> &polydata, 00215 vtkSmartPointer<vtkIdTypeArray> &initcells) 00216 { 00217 vtkSmartPointer<vtkCellArray> vertices; 00218 if (!polydata) 00219 { 00220 allocVtkPolyData (polydata); 00221 vertices = vtkSmartPointer<vtkCellArray>::New (); 00222 polydata->SetVerts (vertices); 00223 } 00224 00225 // Create the supporting structures 00226 vertices = polydata->GetVerts (); 00227 if (!vertices) 00228 vertices = vtkSmartPointer<vtkCellArray>::New (); 00229 00230 vtkIdType nr_points = cloud->points.size (); 00231 // Create the point set 00232 vtkSmartPointer<vtkPoints> points = polydata->GetPoints (); 00233 if (!points) 00234 { 00235 points = vtkSmartPointer<vtkPoints>::New (); 00236 points->SetDataTypeToFloat (); 00237 polydata->SetPoints (points); 00238 } 00239 points->SetNumberOfPoints (nr_points); 00240 00241 // Get a pointer to the beginning of the data array 00242 float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0); 00243 00244 // Set the points 00245 if (cloud->is_dense) 00246 { 00247 for (vtkIdType i = 0; i < nr_points; ++i) 00248 memcpy (&data[i * 3], &cloud->points[i].x, 12); // sizeof (float) * 3 00249 } 00250 else 00251 { 00252 vtkIdType j = 0; // true point index 00253 for (vtkIdType i = 0; i < nr_points; ++i) 00254 { 00255 // Check if the point is invalid 00256 if (!pcl_isfinite (cloud->points[i].x) || 00257 !pcl_isfinite (cloud->points[i].y) || 00258 !pcl_isfinite (cloud->points[i].z)) 00259 continue; 00260 00261 memcpy (&data[j * 3], &cloud->points[i].x, 12); // sizeof (float) * 3 00262 j++; 00263 } 00264 nr_points = j; 00265 points->SetNumberOfPoints (nr_points); 00266 } 00267 00268 vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData (); 00269 updateCells (cells, initcells, nr_points); 00270 00271 // Set the cells and the vertices 00272 vertices->SetCells (nr_points, cells); 00273 } 00274 00275 ////////////////////////////////////////////////////////////////////////////////////////////// 00276 template <typename PointT> void 00277 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData ( 00278 const pcl::visualization::PointCloudGeometryHandler<PointT> &geometry_handler, 00279 vtkSmartPointer<vtkPolyData> &polydata, 00280 vtkSmartPointer<vtkIdTypeArray> &initcells) 00281 { 00282 vtkSmartPointer<vtkCellArray> vertices; 00283 if (!polydata) 00284 { 00285 allocVtkPolyData (polydata); 00286 vertices = vtkSmartPointer<vtkCellArray>::New (); 00287 polydata->SetVerts (vertices); 00288 } 00289 00290 // Use the handler to obtain the geometry 00291 vtkSmartPointer<vtkPoints> points; 00292 geometry_handler.getGeometry (points); 00293 polydata->SetPoints (points); 00294 00295 vtkIdType nr_points = points->GetNumberOfPoints (); 00296 00297 // Create the supporting structures 00298 vertices = polydata->GetVerts (); 00299 if (!vertices) 00300 vertices = vtkSmartPointer<vtkCellArray>::New (); 00301 00302 vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData (); 00303 updateCells (cells, initcells, nr_points); 00304 // Set the cells and the vertices 00305 vertices->SetCells (nr_points, cells); 00306 } 00307 00308 //////////////////////////////////////////////////////////////////////////////////////////// 00309 template <typename PointT> bool 00310 pcl::visualization::PCLVisualizer::addPolygon ( 00311 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00312 double r, double g, double b, const std::string &id, int viewport) 00313 { 00314 vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (cloud); 00315 if (!data) 00316 return (false); 00317 00318 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00319 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 00320 if (am_it != shape_actor_map_->end ()) 00321 { 00322 vtkSmartPointer<vtkAppendPolyData> all_data = vtkSmartPointer<vtkAppendPolyData>::New (); 00323 00324 // Add old data 00325 all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ()); 00326 00327 // Add new data 00328 vtkSmartPointer<vtkDataSetSurfaceFilter> surface_filter = vtkSmartPointer<vtkDataSetSurfaceFilter>::New (); 00329 surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data)); 00330 vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput (); 00331 all_data->AddInput (poly_data); 00332 00333 // Create an Actor 00334 vtkSmartPointer<vtkActor> actor; 00335 createActorFromVTKDataSet (all_data->GetOutput (), actor); 00336 actor->GetProperty ()->SetRepresentationToWireframe (); 00337 actor->GetProperty ()->SetColor (r, g, b); 00338 actor->GetMapper ()->ScalarVisibilityOff (); 00339 removeActorFromRenderer (am_it->second, viewport); 00340 addActorToRenderer (actor, viewport); 00341 00342 // Save the pointer/ID pair to the global actor map 00343 (*shape_actor_map_)[id] = actor; 00344 } 00345 else 00346 { 00347 // Create an Actor 00348 vtkSmartPointer<vtkActor> actor; 00349 createActorFromVTKDataSet (data, actor); 00350 actor->GetProperty ()->SetRepresentationToWireframe (); 00351 actor->GetProperty ()->SetColor (r, g, b); 00352 actor->GetMapper ()->ScalarVisibilityOff (); 00353 addActorToRenderer (actor, viewport); 00354 00355 // Save the pointer/ID pair to the global actor map 00356 (*shape_actor_map_)[id] = actor; 00357 } 00358 00359 return (true); 00360 } 00361 00362 //////////////////////////////////////////////////////////////////////////////////////////// 00363 template <typename PointT> bool 00364 pcl::visualization::PCLVisualizer::addPolygon ( 00365 const pcl::PlanarPolygon<PointT> &polygon, 00366 double r, double g, double b, const std::string &id, int viewport) 00367 { 00368 vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (polygon); 00369 if (!data) 00370 return (false); 00371 00372 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00373 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 00374 if (am_it != shape_actor_map_->end ()) 00375 { 00376 vtkSmartPointer<vtkAppendPolyData> all_data = vtkSmartPointer<vtkAppendPolyData>::New (); 00377 00378 // Add old data 00379 all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ()); 00380 00381 // Add new data 00382 vtkSmartPointer<vtkDataSetSurfaceFilter> surface_filter = vtkSmartPointer<vtkDataSetSurfaceFilter>::New (); 00383 surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data)); 00384 vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput (); 00385 all_data->AddInput (poly_data); 00386 00387 // Create an Actor 00388 vtkSmartPointer<vtkActor> actor; 00389 createActorFromVTKDataSet (all_data->GetOutput (), actor); 00390 actor->GetProperty ()->SetRepresentationToWireframe (); 00391 actor->GetProperty ()->SetColor (r, g, b); 00392 actor->GetMapper ()->ScalarVisibilityOn (); 00393 actor->GetProperty ()->BackfaceCullingOff (); 00394 removeActorFromRenderer (am_it->second, viewport); 00395 addActorToRenderer (actor, viewport); 00396 00397 // Save the pointer/ID pair to the global actor map 00398 (*shape_actor_map_)[id] = actor; 00399 } 00400 else 00401 { 00402 // Create an Actor 00403 vtkSmartPointer<vtkActor> actor; 00404 createActorFromVTKDataSet (data, actor); 00405 actor->GetProperty ()->SetRepresentationToWireframe (); 00406 actor->GetProperty ()->SetColor (r, g, b); 00407 actor->GetMapper ()->ScalarVisibilityOn (); 00408 actor->GetProperty ()->BackfaceCullingOff (); 00409 addActorToRenderer (actor, viewport); 00410 00411 // Save the pointer/ID pair to the global actor map 00412 (*shape_actor_map_)[id] = actor; 00413 } 00414 return (true); 00415 } 00416 00417 //////////////////////////////////////////////////////////////////////////////////////////// 00418 template <typename PointT> bool 00419 pcl::visualization::PCLVisualizer::addPolygon ( 00420 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00421 const std::string &id, int viewport) 00422 { 00423 return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5, id, viewport)); 00424 } 00425 00426 //////////////////////////////////////////////////////////////////////////////////////////// 00427 template <typename P1, typename P2> bool 00428 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport) 00429 { 00430 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00431 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 00432 if (am_it != shape_actor_map_->end ()) 00433 { 00434 PCL_WARN ("[addLine] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00435 return (false); 00436 } 00437 00438 vtkSmartPointer<vtkDataSet> data = createLine (pt1.getVector4fMap (), pt2.getVector4fMap ()); 00439 00440 // Create an Actor 00441 vtkSmartPointer<vtkLODActor> actor; 00442 createActorFromVTKDataSet (data, actor); 00443 actor->GetProperty ()->SetRepresentationToWireframe (); 00444 actor->GetProperty ()->SetColor (r, g, b); 00445 actor->GetMapper ()->ScalarVisibilityOff (); 00446 addActorToRenderer (actor, viewport); 00447 00448 // Save the pointer/ID pair to the global actor map 00449 (*shape_actor_map_)[id] = actor; 00450 return (true); 00451 } 00452 00453 //////////////////////////////////////////////////////////////////////////////////////////// 00454 template <typename P1, typename P2> bool 00455 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport) 00456 { 00457 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00458 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 00459 if (am_it != shape_actor_map_->end ()) 00460 { 00461 PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00462 return (false); 00463 } 00464 00465 // Create an Actor 00466 vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New (); 00467 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld (); 00468 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z); 00469 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld (); 00470 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z); 00471 leader->SetArrowStyleToFilled (); 00472 leader->AutoLabelOn (); 00473 00474 leader->GetProperty ()->SetColor (r, g, b); 00475 addActorToRenderer (leader, viewport); 00476 00477 // Save the pointer/ID pair to the global actor map 00478 (*shape_actor_map_)[id] = leader; 00479 return (true); 00480 } 00481 00482 //////////////////////////////////////////////////////////////////////////////////////////// 00483 template <typename P1, typename P2> bool 00484 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, bool display_length, const std::string &id, int viewport) 00485 { 00486 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00487 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 00488 if (am_it != shape_actor_map_->end ()) 00489 { 00490 PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00491 return (false); 00492 } 00493 00494 // Create an Actor 00495 vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New (); 00496 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld (); 00497 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z); 00498 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld (); 00499 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z); 00500 leader->SetArrowStyleToFilled (); 00501 leader->SetArrowPlacementToPoint1 (); 00502 if (display_length) 00503 leader->AutoLabelOn (); 00504 else 00505 leader->AutoLabelOff (); 00506 00507 leader->GetProperty ()->SetColor (r, g, b); 00508 addActorToRenderer (leader, viewport); 00509 00510 // Save the pointer/ID pair to the global actor map 00511 (*shape_actor_map_)[id] = leader; 00512 return (true); 00513 } 00514 //////////////////////////////////////////////////////////////////////////////////////////// 00515 template <typename P1, typename P2> bool 00516 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, 00517 double r_line, double g_line, double b_line, 00518 double r_text, double g_text, double b_text, 00519 const std::string &id, int viewport) 00520 { 00521 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00522 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 00523 if (am_it != shape_actor_map_->end ()) 00524 { 00525 PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00526 return (false); 00527 } 00528 00529 // Create an Actor 00530 vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New (); 00531 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld (); 00532 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z); 00533 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld (); 00534 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z); 00535 leader->SetArrowStyleToFilled (); 00536 leader->AutoLabelOn (); 00537 00538 leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text); 00539 00540 leader->GetProperty ()->SetColor (r_line, g_line, b_line); 00541 addActorToRenderer (leader, viewport); 00542 00543 // Save the pointer/ID pair to the global actor map 00544 (*shape_actor_map_)[id] = leader; 00545 return (true); 00546 } 00547 00548 //////////////////////////////////////////////////////////////////////////////////////////// 00549 template <typename P1, typename P2> bool 00550 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, const std::string &id, int viewport) 00551 { 00552 return (!addLine (pt1, pt2, 0.5, 0.5, 0.5, id, viewport)); 00553 } 00554 00555 //////////////////////////////////////////////////////////////////////////////////////////// 00556 template <typename PointT> bool 00557 pcl::visualization::PCLVisualizer::addSphere (const PointT ¢er, double radius, double r, double g, double b, const std::string &id, int viewport) 00558 { 00559 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00560 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 00561 if (am_it != shape_actor_map_->end ()) 00562 { 00563 PCL_WARN ("[addSphere] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00564 return (false); 00565 } 00566 00567 vtkSmartPointer<vtkSphereSource> data = vtkSmartPointer<vtkSphereSource>::New (); 00568 data->SetRadius (radius); 00569 data->SetCenter (double (center.x), double (center.y), double (center.z)); 00570 data->SetPhiResolution (10); 00571 data->SetThetaResolution (10); 00572 data->LatLongTessellationOff (); 00573 data->Update (); 00574 00575 // Setup actor and mapper 00576 vtkSmartPointer <vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New (); 00577 mapper->SetInputConnection (data->GetOutputPort ()); 00578 00579 // Create an Actor 00580 vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New (); 00581 actor->SetMapper (mapper); 00582 //createActorFromVTKDataSet (data, actor); 00583 actor->GetProperty ()->SetRepresentationToSurface (); 00584 actor->GetProperty ()->SetInterpolationToFlat (); 00585 actor->GetProperty ()->SetColor (r, g, b); 00586 actor->GetMapper ()->ImmediateModeRenderingOn (); 00587 actor->GetMapper ()->StaticOn (); 00588 actor->GetMapper ()->ScalarVisibilityOff (); 00589 actor->GetMapper ()->Update (); 00590 addActorToRenderer (actor, viewport); 00591 00592 // Save the pointer/ID pair to the global actor map 00593 (*shape_actor_map_)[id] = actor; 00594 return (true); 00595 } 00596 00597 //////////////////////////////////////////////////////////////////////////////////////////// 00598 template <typename PointT> bool 00599 pcl::visualization::PCLVisualizer::addSphere (const PointT ¢er, double radius, const std::string &id, int viewport) 00600 { 00601 return (addSphere (center, radius, 0.5, 0.5, 0.5, id, viewport)); 00602 } 00603 00604 //////////////////////////////////////////////////////////////////////////////////////////// 00605 template<typename PointT> bool 00606 pcl::visualization::PCLVisualizer::updateSphere (const PointT ¢er, double radius, double r, double g, double b, const std::string &id) 00607 { 00608 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00609 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 00610 if (am_it == shape_actor_map_->end ()) 00611 return (false); 00612 00613 ////////////////////////////////////////////////////////////////////////// 00614 // Get the actor pointer 00615 vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second); 00616 vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer (); 00617 vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo); 00618 00619 src->SetCenter (double (center.x), double (center.y), double (center.z)); 00620 src->SetRadius (radius); 00621 src->Update (); 00622 actor->GetProperty ()->SetColor (r, g, b); 00623 actor->Modified (); 00624 00625 return (true); 00626 } 00627 00628 ////////////////////////////////////////////////// 00629 template <typename PointT> bool 00630 pcl::visualization::PCLVisualizer::addText3D ( 00631 const std::string &text, 00632 const PointT& position, 00633 double textScale, 00634 double r, 00635 double g, 00636 double b, 00637 const std::string &id, 00638 int viewport) 00639 { 00640 std::string tid; 00641 if (id.empty ()) 00642 tid = text; 00643 else 00644 tid = id; 00645 00646 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00647 ShapeActorMap::iterator am_it = shape_actor_map_->find (tid); 00648 if (am_it != shape_actor_map_->end ()) 00649 { 00650 pcl::console::print_warn (stderr, "[addText3d] A text with id <%s> already exists! Please choose a different id and retry.\n", tid.c_str ()); 00651 return (false); 00652 } 00653 00654 vtkSmartPointer<vtkVectorText> textSource = vtkSmartPointer<vtkVectorText>::New (); 00655 textSource->SetText (text.c_str()); 00656 textSource->Update (); 00657 00658 vtkSmartPointer<vtkPolyDataMapper> textMapper = vtkSmartPointer<vtkPolyDataMapper>::New (); 00659 textMapper->SetInputConnection (textSource->GetOutputPort ()); 00660 00661 // Since each follower may follow a different camera, we need different followers 00662 rens_->InitTraversal (); 00663 vtkRenderer* renderer = NULL; 00664 int i = 1; 00665 while ((renderer = rens_->GetNextItem ()) != NULL) 00666 { 00667 // Should we add the actor to all renderers or just to i-nth renderer? 00668 if (viewport == 0 || viewport == i) 00669 { 00670 vtkSmartPointer<vtkFollower> textActor = vtkSmartPointer<vtkFollower>::New (); 00671 textActor->SetMapper (textMapper); 00672 textActor->SetPosition (position.x, position.y, position.z); 00673 textActor->SetScale (textScale); 00674 textActor->GetProperty ()->SetColor (r, g, b); 00675 textActor->SetCamera (renderer->GetActiveCamera ()); 00676 00677 renderer->AddActor (textActor); 00678 renderer->Render (); 00679 00680 // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers 00681 // for multiple viewport 00682 std::string alternate_tid = tid; 00683 alternate_tid.append(i, '*'); 00684 00685 (*shape_actor_map_)[(viewport == 0) ? tid : alternate_tid] = textActor; 00686 } 00687 00688 ++i; 00689 } 00690 00691 return (true); 00692 } 00693 00694 ////////////////////////////////////////////////////////////////////////////////////////////// 00695 template <typename PointNT> bool 00696 pcl::visualization::PCLVisualizer::addPointCloudNormals ( 00697 const typename pcl::PointCloud<PointNT>::ConstPtr &cloud, 00698 int level, float scale, const std::string &id, int viewport) 00699 { 00700 return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale, id, viewport)); 00701 } 00702 00703 ////////////////////////////////////////////////////////////////////////////////////////////// 00704 template <typename PointT, typename PointNT> bool 00705 pcl::visualization::PCLVisualizer::addPointCloudNormals ( 00706 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00707 const typename pcl::PointCloud<PointNT>::ConstPtr &normals, 00708 int level, float scale, 00709 const std::string &id, int viewport) 00710 { 00711 if (normals->points.size () != cloud->points.size ()) 00712 { 00713 PCL_ERROR ("[addPointCloudNormals] The number of points differs from the number of normals!\n"); 00714 return (false); 00715 } 00716 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00717 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 00718 00719 if (am_it != cloud_actor_map_->end ()) 00720 { 00721 PCL_WARN ("[addPointCloudNormals] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00722 return (false); 00723 } 00724 00725 vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New(); 00726 vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New(); 00727 00728 points->SetDataTypeToFloat (); 00729 vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New (); 00730 data->SetNumberOfComponents (3); 00731 00732 00733 vtkIdType nr_normals = 0; 00734 float* pts = 0; 00735 00736 // If the cloud is organized, then distribute the normal step in both directions 00737 if (cloud->isOrganized () && normals->isOrganized ()) 00738 { 00739 vtkIdType point_step = static_cast<vtkIdType> (sqrt (double (level))); 00740 nr_normals = (static_cast<vtkIdType> ((cloud->width - 1)/ point_step) + 1) * 00741 (static_cast<vtkIdType> ((cloud->height - 1) / point_step) + 1); 00742 pts = new float[2 * nr_normals * 3]; 00743 00744 vtkIdType cell_count = 0; 00745 for (vtkIdType y = 0; y < normals->height; y += point_step) 00746 for (vtkIdType x = 0; x < normals->width; x += point_step) 00747 { 00748 PointT p = (*cloud)(x, y); 00749 p.x += (*normals)(x, y).normal[0] * scale; 00750 p.y += (*normals)(x, y).normal[1] * scale; 00751 p.z += (*normals)(x, y).normal[2] * scale; 00752 00753 pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x; 00754 pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y; 00755 pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z; 00756 pts[2 * cell_count * 3 + 3] = p.x; 00757 pts[2 * cell_count * 3 + 4] = p.y; 00758 pts[2 * cell_count * 3 + 5] = p.z; 00759 00760 lines->InsertNextCell (2); 00761 lines->InsertCellPoint (2 * cell_count); 00762 lines->InsertCellPoint (2 * cell_count + 1); 00763 cell_count ++; 00764 } 00765 } 00766 else 00767 { 00768 nr_normals = (cloud->points.size () - 1) / level + 1 ; 00769 pts = new float[2 * nr_normals * 3]; 00770 00771 for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level) 00772 { 00773 PointT p = cloud->points[i]; 00774 p.x += normals->points[i].normal[0] * scale; 00775 p.y += normals->points[i].normal[1] * scale; 00776 p.z += normals->points[i].normal[2] * scale; 00777 00778 pts[2 * j * 3 + 0] = cloud->points[i].x; 00779 pts[2 * j * 3 + 1] = cloud->points[i].y; 00780 pts[2 * j * 3 + 2] = cloud->points[i].z; 00781 pts[2 * j * 3 + 3] = p.x; 00782 pts[2 * j * 3 + 4] = p.y; 00783 pts[2 * j * 3 + 5] = p.z; 00784 00785 lines->InsertNextCell (2); 00786 lines->InsertCellPoint (2 * j); 00787 lines->InsertCellPoint (2 * j + 1); 00788 } 00789 } 00790 00791 data->SetArray (&pts[0], 2 * nr_normals * 3, 0); 00792 points->SetData (data); 00793 00794 vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New(); 00795 polyData->SetPoints (points); 00796 polyData->SetLines (lines); 00797 00798 vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New (); 00799 mapper->SetInput (polyData); 00800 mapper->SetColorModeToMapScalars(); 00801 mapper->SetScalarModeToUsePointData(); 00802 00803 // create actor 00804 vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New (); 00805 actor->SetMapper (mapper); 00806 00807 // Add it to all renderers 00808 addActorToRenderer (actor, viewport); 00809 00810 // Save the pointer/ID pair to the global actor map 00811 (*cloud_actor_map_)[id].actor = actor; 00812 return (true); 00813 } 00814 00815 ////////////////////////////////////////////////////////////////////////////////////////////// 00816 template <typename PointT, typename GradientT> bool 00817 pcl::visualization::PCLVisualizer::addPointCloudIntensityGradients ( 00818 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 00819 const typename pcl::PointCloud<GradientT>::ConstPtr &gradients, 00820 int level, double scale, 00821 const std::string &id, int viewport) 00822 { 00823 if (gradients->points.size () != cloud->points.size ()) 00824 { 00825 PCL_ERROR ("[addPointCloudGradients] The number of points differs from the number of gradients!\n"); 00826 return (false); 00827 } 00828 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00829 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 00830 00831 if (am_it != cloud_actor_map_->end ()) 00832 { 00833 PCL_WARN ("[addPointCloudGradients] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00834 return (false); 00835 } 00836 00837 vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New(); 00838 vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New(); 00839 00840 points->SetDataTypeToFloat (); 00841 vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New (); 00842 data->SetNumberOfComponents (3); 00843 00844 vtkIdType nr_gradients = (cloud->points.size () - 1) / level + 1 ; 00845 float* pts = new float[2 * nr_gradients * 3]; 00846 00847 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level) 00848 { 00849 PointT p = cloud->points[i]; 00850 p.x += gradients->points[i].gradient[0] * scale; 00851 p.y += gradients->points[i].gradient[1] * scale; 00852 p.z += gradients->points[i].gradient[2] * scale; 00853 00854 pts[2 * j * 3 + 0] = cloud->points[i].x; 00855 pts[2 * j * 3 + 1] = cloud->points[i].y; 00856 pts[2 * j * 3 + 2] = cloud->points[i].z; 00857 pts[2 * j * 3 + 3] = p.x; 00858 pts[2 * j * 3 + 4] = p.y; 00859 pts[2 * j * 3 + 5] = p.z; 00860 00861 lines->InsertNextCell(2); 00862 lines->InsertCellPoint(2*j); 00863 lines->InsertCellPoint(2*j+1); 00864 } 00865 00866 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0); 00867 points->SetData (data); 00868 00869 vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New(); 00870 polyData->SetPoints(points); 00871 polyData->SetLines(lines); 00872 00873 vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New (); 00874 mapper->SetInput (polyData); 00875 mapper->SetColorModeToMapScalars(); 00876 mapper->SetScalarModeToUsePointData(); 00877 00878 // create actor 00879 vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New (); 00880 actor->SetMapper (mapper); 00881 00882 // Add it to all renderers 00883 addActorToRenderer (actor, viewport); 00884 00885 // Save the pointer/ID pair to the global actor map 00886 (*cloud_actor_map_)[id].actor = actor; 00887 return (true); 00888 } 00889 00890 ////////////////////////////////////////////////////////////////////////////////////////////// 00891 template <typename PointT> bool 00892 pcl::visualization::PCLVisualizer::addCorrespondences ( 00893 const typename pcl::PointCloud<PointT>::ConstPtr &source_points, 00894 const typename pcl::PointCloud<PointT>::ConstPtr &target_points, 00895 const std::vector<int> &correspondences, 00896 const std::string &id, 00897 int viewport) 00898 { 00899 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 00900 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 00901 if (am_it != shape_actor_map_->end ()) 00902 { 00903 PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 00904 return (false); 00905 } 00906 00907 int n_corr = int (correspondences.size ()); 00908 vtkSmartPointer<vtkPolyData> line_data = vtkSmartPointer<vtkPolyData>::New (); 00909 00910 // Prepare colors 00911 vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New (); 00912 line_colors->SetNumberOfComponents (3); 00913 line_colors->SetName ("Colors"); 00914 line_colors->SetNumberOfTuples (n_corr); 00915 unsigned char* colors = line_colors->GetPointer (0); 00916 memset (colors, 0, line_colors->GetNumberOfTuples () * line_colors->GetNumberOfComponents ()); 00917 pcl::RGB rgb; 00918 // Will use random colors or RED by default 00919 rgb.r = 255; rgb.g = rgb.b = 0; 00920 00921 // Prepare coordinates 00922 vtkSmartPointer<vtkPoints> line_points = vtkSmartPointer<vtkPoints>::New (); 00923 line_points->SetNumberOfPoints (2 * n_corr); 00924 00925 vtkSmartPointer<vtkIdTypeArray> line_cells_id = vtkSmartPointer<vtkIdTypeArray>::New (); 00926 line_cells_id->SetNumberOfComponents (3); 00927 line_cells_id->SetNumberOfTuples (n_corr); 00928 vtkIdType *line_cell_id = line_cells_id->GetPointer (0); 00929 vtkSmartPointer<vtkCellArray> line_cells = vtkSmartPointer<vtkCellArray>::New (); 00930 00931 vtkSmartPointer<vtkFloatArray> line_tcoords = vtkSmartPointer<vtkFloatArray>::New (); 00932 line_tcoords->SetNumberOfComponents (1); 00933 line_tcoords->SetNumberOfTuples (n_corr * 2); 00934 line_tcoords->SetName ("Texture Coordinates"); 00935 00936 double tc[3] = {0.0, 0.0, 0.0}; 00937 00938 int j = 0, idx = 0; 00939 // Draw lines between the best corresponding points 00940 for (size_t i = 0; i < n_corr; ++i) 00941 { 00942 const PointT &p_src = source_points->points[i]; 00943 const PointT &p_tgt = target_points->points[correspondences[i]]; 00944 00945 int id1 = j * 2 + 0, id2 = j * 2 + 1; 00946 // Set the points 00947 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z); 00948 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z); 00949 // Set the cell ID 00950 *line_cell_id++ = 2; 00951 *line_cell_id++ = id1; 00952 *line_cell_id++ = id2; 00953 // Set the texture coords 00954 tc[0] = 0.; line_tcoords->SetTuple (id1, tc); 00955 tc[0] = 1.; line_tcoords->SetTuple (id2, tc); 00956 00957 getRandomColors (rgb); 00958 colors[idx+0] = rgb.r; 00959 colors[idx+1] = rgb.g; 00960 colors[idx+2] = rgb.b; 00961 } 00962 line_colors->SetNumberOfTuples (j); 00963 line_cells_id->SetNumberOfTuples (j); 00964 line_cells->SetCells (n_corr, line_cells_id); 00965 line_points->SetNumberOfPoints (j*2); 00966 line_tcoords->SetNumberOfTuples (j*2); 00967 00968 // Fill in the lines 00969 line_data->SetPoints (line_points); 00970 line_data->SetLines (line_cells); 00971 line_data->GetPointData ()->SetTCoords (line_tcoords); 00972 line_data->GetCellData ()->SetScalars (line_colors); 00973 line_data->Update (); 00974 00975 // Create an Actor 00976 vtkSmartPointer<vtkLODActor> actor; 00977 createActorFromVTKDataSet (line_data, actor); 00978 actor->GetProperty ()->SetRepresentationToWireframe (); 00979 actor->GetProperty ()->SetOpacity (0.5); 00980 addActorToRenderer (actor, viewport); 00981 00982 // Save the pointer/ID pair to the global actor map 00983 (*shape_actor_map_)[id] = actor; 00984 00985 return (true); 00986 } 00987 00988 ////////////////////////////////////////////////////////////////////////////////////////////// 00989 template <typename PointT> bool 00990 pcl::visualization::PCLVisualizer::addCorrespondences ( 00991 const typename pcl::PointCloud<PointT>::ConstPtr &source_points, 00992 const typename pcl::PointCloud<PointT>::ConstPtr &target_points, 00993 const pcl::Correspondences &correspondences, 00994 int nth, 00995 const std::string &id, 00996 int viewport) 00997 { 00998 if (correspondences.empty ()) 00999 { 01000 PCL_DEBUG ("[addCorrespondences] An empty set of correspondences given! Nothing to display.\n"); 01001 return (false); 01002 } 01003 01004 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 01005 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 01006 if (am_it != shape_actor_map_->end ()) 01007 { 01008 PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ()); 01009 return (false); 01010 } 01011 01012 int n_corr = int (correspondences.size () / nth + 1); 01013 vtkSmartPointer<vtkPolyData> line_data = vtkSmartPointer<vtkPolyData>::New (); 01014 01015 // Prepare colors 01016 vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New (); 01017 line_colors->SetNumberOfComponents (3); 01018 line_colors->SetName ("Colors"); 01019 line_colors->SetNumberOfTuples (n_corr); 01020 unsigned char* colors = line_colors->GetPointer (0); 01021 memset (colors, 0, line_colors->GetNumberOfTuples () * line_colors->GetNumberOfComponents ()); 01022 pcl::RGB rgb; 01023 // Will use random colors or RED by default 01024 rgb.r = 255; rgb.g = rgb.b = 0; 01025 01026 // Prepare coordinates 01027 vtkSmartPointer<vtkPoints> line_points = vtkSmartPointer<vtkPoints>::New (); 01028 line_points->SetNumberOfPoints (2 * n_corr); 01029 01030 vtkSmartPointer<vtkIdTypeArray> line_cells_id = vtkSmartPointer<vtkIdTypeArray>::New (); 01031 line_cells_id->SetNumberOfComponents (3); 01032 line_cells_id->SetNumberOfTuples (n_corr); 01033 vtkIdType *line_cell_id = line_cells_id->GetPointer (0); 01034 vtkSmartPointer<vtkCellArray> line_cells = vtkSmartPointer<vtkCellArray>::New (); 01035 01036 vtkSmartPointer<vtkFloatArray> line_tcoords = vtkSmartPointer<vtkFloatArray>::New (); 01037 line_tcoords->SetNumberOfComponents (1); 01038 line_tcoords->SetNumberOfTuples (n_corr * 2); 01039 line_tcoords->SetName ("Texture Coordinates"); 01040 01041 double tc[3] = {0.0, 0.0, 0.0}; 01042 01043 int j = 0, idx = 0; 01044 // Draw lines between the best corresponding points 01045 for (size_t i = 0; i < correspondences.size (); i += nth, idx = j * 3, ++j) 01046 { 01047 const PointT &p_src = source_points->points[correspondences[i].index_query]; 01048 const PointT &p_tgt = target_points->points[correspondences[i].index_match]; 01049 01050 int id1 = j * 2 + 0, id2 = j * 2 + 1; 01051 // Set the points 01052 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z); 01053 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z); 01054 // Set the cell ID 01055 *line_cell_id++ = 2; 01056 *line_cell_id++ = id1; 01057 *line_cell_id++ = id2; 01058 // Set the texture coords 01059 tc[0] = 0.; line_tcoords->SetTuple (id1, tc); 01060 tc[0] = 1.; line_tcoords->SetTuple (id2, tc); 01061 01062 getRandomColors (rgb); 01063 colors[idx+0] = rgb.r; 01064 colors[idx+1] = rgb.g; 01065 colors[idx+2] = rgb.b; 01066 } 01067 line_colors->SetNumberOfTuples (j); 01068 line_cells_id->SetNumberOfTuples (j); 01069 line_cells->SetCells (n_corr, line_cells_id); 01070 line_points->SetNumberOfPoints (j*2); 01071 line_tcoords->SetNumberOfTuples (j*2); 01072 01073 // Fill in the lines 01074 line_data->SetPoints (line_points); 01075 line_data->SetLines (line_cells); 01076 line_data->GetPointData ()->SetTCoords (line_tcoords); 01077 line_data->GetCellData ()->SetScalars (line_colors); 01078 line_data->Update (); 01079 01080 // Create an Actor 01081 vtkSmartPointer<vtkLODActor> actor; 01082 createActorFromVTKDataSet (line_data, actor); 01083 actor->GetProperty ()->SetRepresentationToWireframe (); 01084 actor->GetProperty ()->SetOpacity (0.5); 01085 addActorToRenderer (actor, viewport); 01086 01087 // Save the pointer/ID pair to the global actor map 01088 (*shape_actor_map_)[id] = actor; 01089 01090 return (true); 01091 } 01092 01093 ////////////////////////////////////////////////////////////////////////////////////////////// 01094 template <typename PointT> bool 01095 pcl::visualization::PCLVisualizer::updateCorrespondences ( 01096 const typename pcl::PointCloud<PointT>::ConstPtr &source_points, 01097 const typename pcl::PointCloud<PointT>::ConstPtr &target_points, 01098 const pcl::Correspondences &correspondences, 01099 int nth, 01100 const std::string &id) 01101 { 01102 if (correspondences.empty ()) 01103 { 01104 PCL_DEBUG ("[updateCorrespondences] An empty set of correspondences given! Nothing to display.\n"); 01105 return (false); 01106 } 01107 01108 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 01109 ShapeActorMap::iterator am_it = shape_actor_map_->find (id); 01110 if (am_it == shape_actor_map_->end ()) 01111 return (false); 01112 01113 vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second); 01114 vtkSmartPointer<vtkPolyData> line_data = reinterpret_cast<vtkPolyDataMapper*>(actor->GetMapper ())->GetInput (); 01115 01116 int n_corr = int (correspondences.size () / nth + 1); 01117 01118 // Prepare colors 01119 vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New (); 01120 line_colors->SetNumberOfComponents (3); 01121 line_colors->SetName ("Colors"); 01122 line_colors->SetNumberOfTuples (n_corr); 01123 unsigned char* colors = line_colors->GetPointer (0); 01124 memset (colors, 0, line_colors->GetNumberOfTuples () * line_colors->GetNumberOfComponents ()); 01125 pcl::RGB rgb; 01126 // Will use random colors or RED by default 01127 rgb.r = 255.0; rgb.g = rgb.b = 0.0; 01128 01129 // Prepare coordinates 01130 vtkSmartPointer<vtkPoints> line_points = vtkSmartPointer<vtkPoints>::New (); 01131 line_points->SetNumberOfPoints (2 * n_corr); 01132 01133 vtkSmartPointer<vtkIdTypeArray> line_cells_id = vtkSmartPointer<vtkIdTypeArray>::New (); 01134 line_cells_id->SetNumberOfComponents (3); 01135 line_cells_id->SetNumberOfTuples (n_corr); 01136 vtkIdType *line_cell_id = line_cells_id->GetPointer (0); 01137 vtkSmartPointer<vtkCellArray> line_cells = line_data->GetLines (); 01138 01139 vtkSmartPointer<vtkFloatArray> line_tcoords = vtkSmartPointer<vtkFloatArray>::New (); 01140 line_tcoords->SetNumberOfComponents (1); 01141 line_tcoords->SetNumberOfTuples (n_corr * 2); 01142 line_tcoords->SetName ("Texture Coordinates"); 01143 01144 double tc[3] = {0.0, 0.0, 0.0}; 01145 01146 int j = 0, idx = 0; 01147 // Draw lines between the best corresponding points 01148 for (size_t i = 0; i < correspondences.size (); i += nth, idx = j * 3, ++j) 01149 { 01150 const PointT &p_src = source_points->points[correspondences[i].index_query]; 01151 const PointT &p_tgt = target_points->points[correspondences[i].index_match]; 01152 01153 int id1 = j * 2 + 0, id2 = j * 2 + 1; 01154 // Set the points 01155 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z); 01156 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z); 01157 // Set the cell ID 01158 *line_cell_id++ = 2; 01159 *line_cell_id++ = id1; 01160 *line_cell_id++ = id2; 01161 // Set the texture coords 01162 tc[0] = 0.; line_tcoords->SetTuple (id1, tc); 01163 tc[0] = 1.; line_tcoords->SetTuple (id2, tc); 01164 01165 getRandomColors (rgb); 01166 colors[idx+0] = rgb.r; 01167 colors[idx+1] = rgb.g; 01168 colors[idx+2] = rgb.b; 01169 } 01170 line_colors->SetNumberOfTuples (j); 01171 line_cells_id->SetNumberOfTuples (j); 01172 line_cells->SetCells (n_corr, line_cells_id); 01173 line_points->SetNumberOfPoints (j*2); 01174 line_tcoords->SetNumberOfTuples (j*2); 01175 01176 // Fill in the lines 01177 line_data->SetPoints (line_points); 01178 line_data->SetLines (line_cells); 01179 line_data->GetPointData ()->SetTCoords (line_tcoords); 01180 line_data->GetCellData ()->SetScalars (line_colors); 01181 line_data->Update (); 01182 01183 // Update the mapper 01184 reinterpret_cast<vtkPolyDataMapper*>(actor->GetMapper ())->SetInput (line_data); 01185 01186 return (true); 01187 } 01188 01189 ////////////////////////////////////////////////////////////////////////////////////////////// 01190 template <typename PointT> bool 01191 pcl::visualization::PCLVisualizer::fromHandlersToScreen ( 01192 const PointCloudGeometryHandler<PointT> &geometry_handler, 01193 const PointCloudColorHandler<PointT> &color_handler, 01194 const std::string &id, 01195 int viewport, 01196 const Eigen::Vector4f& sensor_origin, 01197 const Eigen::Quaternion<float>& sensor_orientation) 01198 { 01199 if (!geometry_handler.isCapable ()) 01200 { 01201 PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ()); 01202 return (false); 01203 } 01204 01205 if (!color_handler.isCapable ()) 01206 { 01207 PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ()); 01208 return (false); 01209 } 01210 01211 vtkSmartPointer<vtkPolyData> polydata; 01212 vtkSmartPointer<vtkIdTypeArray> initcells; 01213 // Convert the PointCloud to VTK PolyData 01214 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells); 01215 // use the given geometry handler 01216 polydata->Update (); 01217 01218 // Get the colors from the handler 01219 bool has_colors = false; 01220 double minmax[2]; 01221 vtkSmartPointer<vtkDataArray> scalars; 01222 if (color_handler.getColor (scalars)) 01223 { 01224 polydata->GetPointData ()->SetScalars (scalars); 01225 scalars->GetRange (minmax); 01226 has_colors = true; 01227 } 01228 01229 // Create an Actor 01230 vtkSmartPointer<vtkLODActor> actor; 01231 createActorFromVTKDataSet (polydata, actor); 01232 if (has_colors) 01233 actor->GetMapper ()->SetScalarRange (minmax); 01234 01235 // Add it to all renderers 01236 addActorToRenderer (actor, viewport); 01237 01238 // Save the pointer/ID pair to the global actor map 01239 CloudActor& cloud_actor = (*cloud_actor_map_)[id]; 01240 cloud_actor.actor = actor; 01241 cloud_actor.cells = initcells; 01242 01243 // Save the viewpoint transformation matrix to the global actor map 01244 vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New(); 01245 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation); 01246 cloud_actor.viewpoint_transformation_ = transformation; 01247 cloud_actor.actor->SetUserMatrix (transformation); 01248 cloud_actor.actor->Modified (); 01249 01250 return (true); 01251 } 01252 01253 ////////////////////////////////////////////////////////////////////////////////////////////// 01254 template <typename PointT> bool 01255 pcl::visualization::PCLVisualizer::fromHandlersToScreen ( 01256 const PointCloudGeometryHandler<PointT> &geometry_handler, 01257 const ColorHandlerConstPtr &color_handler, 01258 const std::string &id, 01259 int viewport, 01260 const Eigen::Vector4f& sensor_origin, 01261 const Eigen::Quaternion<float>& sensor_orientation) 01262 { 01263 if (!geometry_handler.isCapable ()) 01264 { 01265 PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ()); 01266 return (false); 01267 } 01268 01269 if (!color_handler->isCapable ()) 01270 { 01271 PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler->getName ().c_str ()); 01272 return (false); 01273 } 01274 01275 vtkSmartPointer<vtkPolyData> polydata; 01276 vtkSmartPointer<vtkIdTypeArray> initcells; 01277 // Convert the PointCloud to VTK PolyData 01278 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells); 01279 // use the given geometry handler 01280 polydata->Update (); 01281 01282 // Get the colors from the handler 01283 bool has_colors = false; 01284 double minmax[2]; 01285 vtkSmartPointer<vtkDataArray> scalars; 01286 if (color_handler->getColor (scalars)) 01287 { 01288 polydata->GetPointData ()->SetScalars (scalars); 01289 scalars->GetRange (minmax); 01290 has_colors = true; 01291 } 01292 01293 // Create an Actor 01294 vtkSmartPointer<vtkLODActor> actor; 01295 createActorFromVTKDataSet (polydata, actor); 01296 if (has_colors) 01297 actor->GetMapper ()->SetScalarRange (minmax); 01298 01299 // Add it to all renderers 01300 addActorToRenderer (actor, viewport); 01301 01302 // Save the pointer/ID pair to the global actor map 01303 CloudActor& cloud_actor = (*cloud_actor_map_)[id]; 01304 cloud_actor.actor = actor; 01305 cloud_actor.cells = initcells; 01306 cloud_actor.color_handlers.push_back (color_handler); 01307 01308 // Save the viewpoint transformation matrix to the global actor map 01309 vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New(); 01310 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation); 01311 cloud_actor.viewpoint_transformation_ = transformation; 01312 cloud_actor.actor->SetUserMatrix (transformation); 01313 cloud_actor.actor->Modified (); 01314 01315 return (true); 01316 } 01317 01318 ////////////////////////////////////////////////////////////////////////////////////////////// 01319 template <typename PointT> bool 01320 pcl::visualization::PCLVisualizer::fromHandlersToScreen ( 01321 const GeometryHandlerConstPtr &geometry_handler, 01322 const PointCloudColorHandler<PointT> &color_handler, 01323 const std::string &id, 01324 int viewport, 01325 const Eigen::Vector4f& sensor_origin, 01326 const Eigen::Quaternion<float>& sensor_orientation) 01327 { 01328 if (!geometry_handler->isCapable ()) 01329 { 01330 PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler->getName ().c_str ()); 01331 return (false); 01332 } 01333 01334 if (!color_handler.isCapable ()) 01335 { 01336 PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ()); 01337 return (false); 01338 } 01339 01340 vtkSmartPointer<vtkPolyData> polydata; 01341 vtkSmartPointer<vtkIdTypeArray> initcells; 01342 // Convert the PointCloud to VTK PolyData 01343 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells); 01344 // use the given geometry handler 01345 polydata->Update (); 01346 01347 // Get the colors from the handler 01348 bool has_colors = false; 01349 double minmax[2]; 01350 vtkSmartPointer<vtkDataArray> scalars; 01351 if (color_handler.getColor (scalars)) 01352 { 01353 polydata->GetPointData ()->SetScalars (scalars); 01354 scalars->GetRange (minmax); 01355 has_colors = true; 01356 } 01357 01358 // Create an Actor 01359 vtkSmartPointer<vtkLODActor> actor; 01360 createActorFromVTKDataSet (polydata, actor); 01361 if (has_colors) 01362 actor->GetMapper ()->SetScalarRange (minmax); 01363 01364 // Add it to all renderers 01365 addActorToRenderer (actor, viewport); 01366 01367 // Save the pointer/ID pair to the global actor map 01368 CloudActor& cloud_actor = (*cloud_actor_map_)[id]; 01369 cloud_actor.actor = actor; 01370 cloud_actor.cells = initcells; 01371 cloud_actor.geometry_handlers.push_back (geometry_handler); 01372 01373 // Save the viewpoint transformation matrix to the global actor map 01374 vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New (); 01375 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation); 01376 cloud_actor.viewpoint_transformation_ = transformation; 01377 cloud_actor.actor->SetUserMatrix (transformation); 01378 cloud_actor.actor->Modified (); 01379 01380 return (true); 01381 } 01382 01383 ////////////////////////////////////////////////////////////////////////////////////////////// 01384 template <typename PointT> bool 01385 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 01386 const std::string &id) 01387 { 01388 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 01389 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 01390 01391 if (am_it == cloud_actor_map_->end ()) 01392 return (false); 01393 01394 vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput (); 01395 // Convert the PointCloud to VTK PolyData 01396 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells); 01397 polydata->Update (); 01398 01399 // Set scalars to blank, since there is no way we can update them here. 01400 vtkSmartPointer<vtkDataArray> scalars; 01401 polydata->GetPointData ()->SetScalars (scalars); 01402 polydata->Update (); 01403 double minmax[2]; 01404 minmax[0] = std::numeric_limits<double>::min (); 01405 minmax[1] = std::numeric_limits<double>::max (); 01406 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff (); 01407 am_it->second.actor->GetMapper ()->SetScalarRange (minmax); 01408 01409 // Update the mapper 01410 reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata); 01411 return (true); 01412 } 01413 01414 ///////////////////////////////////////////////////////////////////////////////////////////// 01415 template <typename PointT> bool 01416 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &, 01417 const PointCloudGeometryHandler<PointT> &geometry_handler, 01418 const std::string &id) 01419 { 01420 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 01421 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 01422 01423 if (am_it == cloud_actor_map_->end ()) 01424 return (false); 01425 01426 vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput (); 01427 if (!polydata) 01428 return (false); 01429 // Convert the PointCloud to VTK PolyData 01430 convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells); 01431 01432 // Set scalars to blank, since there is no way we can update them here. 01433 vtkSmartPointer<vtkDataArray> scalars; 01434 polydata->GetPointData ()->SetScalars (scalars); 01435 polydata->Update (); 01436 double minmax[2]; 01437 minmax[0] = std::numeric_limits<double>::min (); 01438 minmax[1] = std::numeric_limits<double>::max (); 01439 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff (); 01440 am_it->second.actor->GetMapper ()->SetScalarRange (minmax); 01441 01442 // Update the mapper 01443 reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata); 01444 return (true); 01445 } 01446 01447 01448 ///////////////////////////////////////////////////////////////////////////////////////////// 01449 template <typename PointT> bool 01450 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 01451 const PointCloudColorHandler<PointT> &color_handler, 01452 const std::string &id) 01453 { 01454 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 01455 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 01456 01457 if (am_it == cloud_actor_map_->end ()) 01458 return (false); 01459 01460 // Get the current poly data 01461 vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput (); 01462 if (!polydata) 01463 return (false); 01464 vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts (); 01465 vtkSmartPointer<vtkPoints> points = polydata->GetPoints (); 01466 // Copy the new point array in 01467 vtkIdType nr_points = cloud->points.size (); 01468 points->SetNumberOfPoints (nr_points); 01469 01470 // Get a pointer to the beginning of the data array 01471 float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0); 01472 01473 int pts = 0; 01474 // If the dataset is dense (no NaNs) 01475 if (cloud->is_dense) 01476 { 01477 for (vtkIdType i = 0; i < nr_points; ++i, pts += 3) 01478 memcpy (&data[pts], &cloud->points[i].x, 12); // sizeof (float) * 3 01479 } 01480 else 01481 { 01482 vtkIdType j = 0; // true point index 01483 for (vtkIdType i = 0; i < nr_points; ++i) 01484 { 01485 // Check if the point is invalid 01486 if (!isFinite (cloud->points[i])) 01487 continue; 01488 01489 memcpy (&data[pts], &cloud->points[i].x, 12); // sizeof (float) * 3 01490 pts += 3; 01491 j++; 01492 } 01493 nr_points = j; 01494 points->SetNumberOfPoints (nr_points); 01495 } 01496 01497 vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData (); 01498 updateCells (cells, am_it->second.cells, nr_points); 01499 01500 // Set the cells and the vertices 01501 vertices->SetCells (nr_points, cells); 01502 01503 // Get the colors from the handler 01504 vtkSmartPointer<vtkDataArray> scalars; 01505 color_handler.getColor (scalars); 01506 double minmax[2]; 01507 scalars->GetRange (minmax); 01508 // Update the data 01509 polydata->GetPointData ()->SetScalars (scalars); 01510 polydata->Update (); 01511 01512 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff (); 01513 am_it->second.actor->GetMapper ()->SetScalarRange (minmax); 01514 01515 // Update the mapper 01516 reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata); 01517 return (true); 01518 } 01519 01520 ///////////////////////////////////////////////////////////////////////////////////////////// 01521 template <typename PointT> bool 01522 pcl::visualization::PCLVisualizer::addPolygonMesh ( 01523 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 01524 const std::vector<pcl::Vertices> &vertices, 01525 const std::string &id, 01526 int viewport) 01527 { 01528 if (vertices.empty () || cloud->points.empty ()) 01529 return (false); 01530 01531 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 01532 if (am_it != cloud_actor_map_->end ()) 01533 { 01534 pcl::console::print_warn (stderr, 01535 "[addPolygonMesh] A shape with id <%s> already exists! Please choose a different id and retry.\n", 01536 id.c_str ()); 01537 return (false); 01538 } 01539 01540 int rgb_idx = -1; 01541 std::vector<pcl::PCLPointField> fields; 01542 vtkSmartPointer<vtkUnsignedCharArray> colors; 01543 rgb_idx = pcl::getFieldIndex (*cloud, "rgb", fields); 01544 if (rgb_idx == -1) 01545 rgb_idx = pcl::getFieldIndex (*cloud, "rgba", fields); 01546 if (rgb_idx != -1) 01547 { 01548 colors = vtkSmartPointer<vtkUnsignedCharArray>::New (); 01549 colors->SetNumberOfComponents (3); 01550 colors->SetName ("Colors"); 01551 01552 pcl::RGB rgb_data; 01553 for (size_t i = 0; i < cloud->size (); ++i) 01554 { 01555 if (!isFinite (cloud->points[i])) 01556 continue; 01557 memcpy (&rgb_data, reinterpret_cast<const char*> (&cloud->points[i]) + fields[rgb_idx].offset, sizeof (pcl::RGB)); 01558 unsigned char color[3]; 01559 color[0] = rgb_data.r; 01560 color[1] = rgb_data.g; 01561 color[2] = rgb_data.b; 01562 colors->InsertNextTupleValue (color); 01563 } 01564 } 01565 01566 // Create points from polyMesh.cloud 01567 vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New (); 01568 vtkIdType nr_points = cloud->points.size (); 01569 points->SetNumberOfPoints (nr_points); 01570 vtkSmartPointer<vtkLODActor> actor; 01571 01572 // Get a pointer to the beginning of the data array 01573 float *data = static_cast<vtkFloatArray*> (points->GetData ())->GetPointer (0); 01574 01575 int ptr = 0; 01576 std::vector<int> lookup; 01577 // If the dataset is dense (no NaNs) 01578 if (cloud->is_dense) 01579 { 01580 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3) 01581 memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3); 01582 } 01583 else 01584 { 01585 lookup.resize (nr_points); 01586 vtkIdType j = 0; // true point index 01587 for (vtkIdType i = 0; i < nr_points; ++i) 01588 { 01589 // Check if the point is invalid 01590 if (!isFinite (cloud->points[i])) 01591 continue; 01592 01593 lookup[i] = static_cast<int> (j); 01594 memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3); 01595 j++; 01596 ptr += 3; 01597 } 01598 nr_points = j; 01599 points->SetNumberOfPoints (nr_points); 01600 } 01601 01602 // Get the maximum size of a polygon 01603 int max_size_of_polygon = -1; 01604 for (size_t i = 0; i < vertices.size (); ++i) 01605 if (max_size_of_polygon < static_cast<int> (vertices[i].vertices.size ())) 01606 max_size_of_polygon = static_cast<int> (vertices[i].vertices.size ()); 01607 01608 if (vertices.size () > 1) 01609 { 01610 // Create polys from polyMesh.polygons 01611 vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New (); 01612 vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1)); 01613 int idx = 0; 01614 if (lookup.size () > 0) 01615 { 01616 for (size_t i = 0; i < vertices.size (); ++i, ++idx) 01617 { 01618 size_t n_points = vertices[i].vertices.size (); 01619 *cell++ = n_points; 01620 //cell_array->InsertNextCell (n_points); 01621 for (size_t j = 0; j < n_points; j++, ++idx) 01622 *cell++ = lookup[vertices[i].vertices[j]]; 01623 //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]); 01624 } 01625 } 01626 else 01627 { 01628 for (size_t i = 0; i < vertices.size (); ++i, ++idx) 01629 { 01630 size_t n_points = vertices[i].vertices.size (); 01631 *cell++ = n_points; 01632 //cell_array->InsertNextCell (n_points); 01633 for (size_t j = 0; j < n_points; j++, ++idx) 01634 *cell++ = vertices[i].vertices[j]; 01635 //cell_array->InsertCellPoint (vertices[i].vertices[j]); 01636 } 01637 } 01638 vtkSmartPointer<vtkPolyData> polydata; 01639 allocVtkPolyData (polydata); 01640 cell_array->GetData ()->SetNumberOfValues (idx); 01641 cell_array->Squeeze (); 01642 polydata->SetStrips (cell_array); 01643 polydata->SetPoints (points); 01644 01645 if (colors) 01646 polydata->GetPointData ()->SetScalars (colors); 01647 01648 createActorFromVTKDataSet (polydata, actor, false); 01649 } 01650 else 01651 { 01652 vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New (); 01653 size_t n_points = vertices[0].vertices.size (); 01654 polygon->GetPointIds ()->SetNumberOfIds (n_points - 1); 01655 01656 if (lookup.size () > 0) 01657 { 01658 for (size_t j = 0; j < (n_points - 1); ++j) 01659 polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]); 01660 } 01661 else 01662 { 01663 for (size_t j = 0; j < (n_points - 1); ++j) 01664 polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]); 01665 } 01666 vtkSmartPointer<vtkUnstructuredGrid> poly_grid; 01667 allocVtkUnstructuredGrid (poly_grid); 01668 poly_grid->Allocate (1, 1); 01669 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ()); 01670 poly_grid->SetPoints (points); 01671 poly_grid->Update (); 01672 if (colors) 01673 poly_grid->GetPointData ()->SetScalars (colors); 01674 01675 createActorFromVTKDataSet (poly_grid, actor, false); 01676 } 01677 addActorToRenderer (actor, viewport); 01678 actor->GetProperty ()->SetRepresentationToSurface (); 01679 // Backface culling renders the visualization slower, but guarantees that we see all triangles 01680 actor->GetProperty ()->BackfaceCullingOff (); 01681 actor->GetProperty ()->SetInterpolationToFlat (); 01682 actor->GetProperty ()->EdgeVisibilityOff (); 01683 actor->GetProperty ()->ShadingOff (); 01684 01685 // Save the pointer/ID pair to the global actor map 01686 (*cloud_actor_map_)[id].actor = actor; 01687 01688 // Save the viewpoint transformation matrix to the global actor map 01689 vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New(); 01690 convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation); 01691 (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation; 01692 01693 return (true); 01694 } 01695 01696 ///////////////////////////////////////////////////////////////////////////////////////////// 01697 template <typename PointT> bool 01698 pcl::visualization::PCLVisualizer::updatePolygonMesh ( 01699 const typename pcl::PointCloud<PointT>::ConstPtr &cloud, 01700 const std::vector<pcl::Vertices> &verts, 01701 const std::string &id) 01702 { 01703 if (verts.empty ()) 01704 { 01705 pcl::console::print_error ("[addPolygonMesh] No vertices given!\n"); 01706 return (false); 01707 } 01708 01709 // Check to see if this ID entry already exists (has it been already added to the visualizer?) 01710 CloudActorMap::iterator am_it = cloud_actor_map_->find (id); 01711 if (am_it == cloud_actor_map_->end ()) 01712 return (false); 01713 01714 // Get the current poly data 01715 vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput (); 01716 if (!polydata) 01717 return (false); 01718 vtkSmartPointer<vtkCellArray> cells = polydata->GetStrips (); 01719 if (!cells) 01720 return (false); 01721 vtkSmartPointer<vtkPoints> points = polydata->GetPoints (); 01722 // Copy the new point array in 01723 vtkIdType nr_points = cloud->points.size (); 01724 points->SetNumberOfPoints (nr_points); 01725 01726 // Get a pointer to the beginning of the data array 01727 float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0); 01728 01729 int ptr = 0; 01730 std::vector<int> lookup; 01731 // If the dataset is dense (no NaNs) 01732 if (cloud->is_dense) 01733 { 01734 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3) 01735 memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3); 01736 } 01737 else 01738 { 01739 lookup.resize (nr_points); 01740 vtkIdType j = 0; // true point index 01741 for (vtkIdType i = 0; i < nr_points; ++i) 01742 { 01743 // Check if the point is invalid 01744 if (!isFinite (cloud->points[i])) 01745 continue; 01746 01747 lookup [i] = static_cast<int> (j); 01748 memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3); 01749 j++; 01750 ptr += 3; 01751 } 01752 nr_points = j; 01753 points->SetNumberOfPoints (nr_points); 01754 } 01755 01756 // Update colors 01757 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ()); 01758 int rgb_idx = -1; 01759 std::vector<pcl::PCLPointField> fields; 01760 rgb_idx = pcl::getFieldIndex (*cloud, "rgb", fields); 01761 if (rgb_idx == -1) 01762 rgb_idx = pcl::getFieldIndex (*cloud, "rgba", fields); 01763 if (rgb_idx != -1 && colors) 01764 { 01765 pcl::RGB rgb_data; 01766 int j = 0; 01767 for (size_t i = 0; i < cloud->size (); ++i) 01768 { 01769 if (!isFinite (cloud->points[i])) 01770 continue; 01771 memcpy (&rgb_data, 01772 reinterpret_cast<const char*> (&cloud->points[i]) + fields[rgb_idx].offset, 01773 sizeof (pcl::RGB)); 01774 unsigned char color[3]; 01775 color[0] = rgb_data.r; 01776 color[1] = rgb_data.g; 01777 color[2] = rgb_data.b; 01778 colors->SetTupleValue (j++, color); 01779 } 01780 } 01781 01782 // Get the maximum size of a polygon 01783 int max_size_of_polygon = -1; 01784 for (size_t i = 0; i < verts.size (); ++i) 01785 if (max_size_of_polygon < static_cast<int> (verts[i].vertices.size ())) 01786 max_size_of_polygon = static_cast<int> (verts[i].vertices.size ()); 01787 01788 // Update the cells 01789 cells = vtkSmartPointer<vtkCellArray>::New (); 01790 vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1)); 01791 int idx = 0; 01792 if (lookup.size () > 0) 01793 { 01794 for (size_t i = 0; i < verts.size (); ++i, ++idx) 01795 { 01796 size_t n_points = verts[i].vertices.size (); 01797 *cell++ = n_points; 01798 for (size_t j = 0; j < n_points; j++, cell++, ++idx) 01799 *cell = lookup[verts[i].vertices[j]]; 01800 } 01801 } 01802 else 01803 { 01804 for (size_t i = 0; i < verts.size (); ++i, ++idx) 01805 { 01806 size_t n_points = verts[i].vertices.size (); 01807 *cell++ = n_points; 01808 for (size_t j = 0; j < n_points; j++, cell++, ++idx) 01809 *cell = verts[i].vertices[j]; 01810 } 01811 } 01812 cells->GetData ()->SetNumberOfValues (idx); 01813 cells->Squeeze (); 01814 // Set the the vertices 01815 polydata->SetStrips (cells); 01816 polydata->Update (); 01817 01818 return (true); 01819 } 01820 01821 #endif