Point Cloud Library (PCL)  1.9.1-dev
pcl_visualizer.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_PCL_VISUALIZER_IMPL_H_
39 #define PCL_PCL_VISUALIZER_IMPL_H_
40 
41 #include <vtkVersion.h>
42 #include <vtkSmartPointer.h>
43 #include <vtkCellArray.h>
44 #include <vtkLeaderActor2D.h>
45 #include <vtkVectorText.h>
46 #include <vtkAlgorithmOutput.h>
47 #include <vtkFollower.h>
48 #include <vtkMath.h>
49 #include <vtkSphereSource.h>
50 #include <vtkProperty2D.h>
51 #include <vtkDataSetSurfaceFilter.h>
52 #include <vtkPointData.h>
53 #include <vtkPolyDataMapper.h>
54 #include <vtkProperty.h>
55 #include <vtkMapper.h>
56 #include <vtkCellData.h>
57 #include <vtkDataSetMapper.h>
58 #include <vtkRenderer.h>
59 #include <vtkRendererCollection.h>
60 #include <vtkAppendPolyData.h>
61 #include <vtkTextProperty.h>
62 #include <vtkLODActor.h>
63 #include <vtkLineSource.h>
64 
66 
67 // Support for VTK 7.1 upwards
68 #ifdef vtkGenericDataArray_h
69 #define SetTupleValue SetTypedTuple
70 #define InsertNextTupleValue InsertNextTypedTuple
71 #define GetTupleValue GetTypedTuple
72 #endif
73 
74 //////////////////////////////////////////////////////////////////////////////////////////////
75 template <typename PointT> bool
77  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
78  const std::string &id, int viewport)
79 {
80  // Convert the PointCloud to VTK PolyData
81  PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
82  return (addPointCloud<PointT> (cloud, geometry_handler, id, viewport));
83 }
84 
85 //////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename PointT> bool
88  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
89  const PointCloudGeometryHandler<PointT> &geometry_handler,
90  const std::string &id, int viewport)
91 {
92  if (contains (id))
93  {
94  PCL_WARN ("[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
95  return (false);
96  }
97 
98  if (pcl::traits::has_color<PointT>())
99  {
100  PointCloudColorHandlerRGBField<PointT> color_handler_rgb_field (cloud);
101  return (fromHandlersToScreen (geometry_handler, color_handler_rgb_field, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
102  }
103  PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
104  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
105 }
106 
107 //////////////////////////////////////////////////////////////////////////////////////////////
108 template <typename PointT> bool
110  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
111  const GeometryHandlerConstPtr &geometry_handler,
112  const std::string &id, int viewport)
113 {
114  if (contains (id))
115  {
116  // Here we're just pushing the handlers onto the queue. If needed, something fancier could
117  // be done such as checking if a specific handler already exists, etc.
118  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
119  am_it->second.geometry_handlers.push_back (geometry_handler);
120  return (true);
121  }
122 
123  //PointCloudColorHandlerRandom<PointT> color_handler (cloud);
124  PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
125  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
126 }
127 
128 //////////////////////////////////////////////////////////////////////////////////////////////
129 template <typename PointT> bool
131  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
132  const PointCloudColorHandler<PointT> &color_handler,
133  const std::string &id, int viewport)
134 {
135  if (contains (id))
136  {
137  PCL_WARN ("[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
138 
139  // Here we're just pushing the handlers onto the queue. If needed, something fancier could
140  // be done such as checking if a specific handler already exists, etc.
141  //cloud_actor_map_[id].color_handlers.push_back (color_handler);
142  //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_));
143  return (false);
144  }
145  // Convert the PointCloud to VTK PolyData
146  PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
147  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
148 }
149 
150 //////////////////////////////////////////////////////////////////////////////////////////////
151 template <typename PointT> bool
153  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
154  const ColorHandlerConstPtr &color_handler,
155  const std::string &id, int viewport)
156 {
157  // Check to see if this entry already exists (has it been already added to the visualizer?)
158  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
159  if (am_it != cloud_actor_map_->end ())
160  {
161  // Here we're just pushing the handlers onto the queue. If needed, something fancier could
162  // be done such as checking if a specific handler already exists, etc.
163  am_it->second.color_handlers.push_back (color_handler);
164  return (true);
165  }
166 
167  PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
168  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
169 }
170 
171 //////////////////////////////////////////////////////////////////////////////////////////////
172 template <typename PointT> bool
174  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
175  const GeometryHandlerConstPtr &geometry_handler,
176  const ColorHandlerConstPtr &color_handler,
177  const std::string &id, int viewport)
178 {
179  // Check to see if this entry already exists (has it been already added to the visualizer?)
180  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
181  if (am_it != cloud_actor_map_->end ())
182  {
183  // Here we're just pushing the handlers onto the queue. If needed, something fancier could
184  // be done such as checking if a specific handler already exists, etc.
185  am_it->second.geometry_handlers.push_back (geometry_handler);
186  am_it->second.color_handlers.push_back (color_handler);
187  return (true);
188  }
189  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
190 }
191 
192 //////////////////////////////////////////////////////////////////////////////////////////////
193 template <typename PointT> bool
195  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
196  const PointCloudColorHandler<PointT> &color_handler,
197  const PointCloudGeometryHandler<PointT> &geometry_handler,
198  const std::string &id, int viewport)
199 {
200  if (contains (id))
201  {
202  PCL_WARN ("[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
203  // Here we're just pushing the handlers onto the queue. If needed, something fancier could
204  // be done such as checking if a specific handler already exists, etc.
205  //cloud_actor_map_[id].geometry_handlers.push_back (geometry_handler);
206  //cloud_actor_map_[id].color_handlers.push_back (color_handler);
207  //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_));
208  return (false);
209  }
210  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
211 }
212 
213 //////////////////////////////////////////////////////////////////////////////////////////////
214 template <typename PointT> void
215 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
216  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
219 {
221  if (!polydata)
222  {
223  allocVtkPolyData (polydata);
225  polydata->SetVerts (vertices);
226  }
227 
228  // Create the supporting structures
229  vertices = polydata->GetVerts ();
230  if (!vertices)
232 
233  vtkIdType nr_points = cloud->points.size ();
234  // Create the point set
235  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
236  if (!points)
237  {
239  points->SetDataTypeToFloat ();
240  polydata->SetPoints (points);
241  }
242  points->SetNumberOfPoints (nr_points);
243 
244  // Get a pointer to the beginning of the data array
245  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
246 
247  // Set the points
248  vtkIdType ptr = 0;
249  if (cloud->is_dense)
250  {
251  for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
252  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
253  }
254  else
255  {
256  vtkIdType j = 0; // true point index
257  for (vtkIdType i = 0; i < nr_points; ++i)
258  {
259  // Check if the point is invalid
260  if (!std::isfinite (cloud->points[i].x) ||
261  !std::isfinite (cloud->points[i].y) ||
262  !std::isfinite (cloud->points[i].z))
263  continue;
264 
265  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
266  j++;
267  }
268  nr_points = j;
269  points->SetNumberOfPoints (nr_points);
270  }
271 
272  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
273  updateCells (cells, initcells, nr_points);
274 
275  // Set the cells and the vertices
276  vertices->SetCells (nr_points, cells);
277 }
278 
279 //////////////////////////////////////////////////////////////////////////////////////////////
280 template <typename PointT> void
281 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
285 {
287  if (!polydata)
288  {
289  allocVtkPolyData (polydata);
291  polydata->SetVerts (vertices);
292  }
293 
294  // Use the handler to obtain the geometry
296  geometry_handler.getGeometry (points);
297  polydata->SetPoints (points);
298 
299  vtkIdType nr_points = points->GetNumberOfPoints ();
300 
301  // Create the supporting structures
302  vertices = polydata->GetVerts ();
303  if (!vertices)
305 
306  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
307  updateCells (cells, initcells, nr_points);
308  // Set the cells and the vertices
309  vertices->SetCells (nr_points, cells);
310 }
311 
312 ////////////////////////////////////////////////////////////////////////////////////////////
313 template <typename PointT> bool
315  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
316  double r, double g, double b, const std::string &id, int viewport)
317 {
318  vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (cloud);
319  if (!data)
320  return (false);
321 
322  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
323  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
324  if (am_it != shape_actor_map_->end ())
325  {
327 
328  // Add old data
329  all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
330 
331  // Add new data
333  surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
334  vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
335  all_data->AddInputData (poly_data);
336 
337  // Create an Actor
339  createActorFromVTKDataSet (all_data->GetOutput (), actor);
340  actor->GetProperty ()->SetRepresentationToWireframe ();
341  actor->GetProperty ()->SetColor (r, g, b);
342  actor->GetMapper ()->ScalarVisibilityOff ();
343  removeActorFromRenderer (am_it->second, viewport);
344  addActorToRenderer (actor, viewport);
345 
346  // Save the pointer/ID pair to the global actor map
347  (*shape_actor_map_)[id] = actor;
348  }
349  else
350  {
351  // Create an Actor
353  createActorFromVTKDataSet (data, actor);
354  actor->GetProperty ()->SetRepresentationToWireframe ();
355  actor->GetProperty ()->SetColor (r, g, b);
356  actor->GetMapper ()->ScalarVisibilityOff ();
357  addActorToRenderer (actor, viewport);
358 
359  // Save the pointer/ID pair to the global actor map
360  (*shape_actor_map_)[id] = actor;
361  }
362 
363  return (true);
364 }
365 
366 ////////////////////////////////////////////////////////////////////////////////////////////
367 template <typename PointT> bool
369  const pcl::PlanarPolygon<PointT> &polygon,
370  double r, double g, double b, const std::string &id, int viewport)
371 {
372  vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (polygon);
373  if (!data)
374  return (false);
375 
376  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
377  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
378  if (am_it != shape_actor_map_->end ())
379  {
381 
382  // Add old data
383  all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
384 
385  // Add new data
387  surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
388  vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
389  all_data->AddInputData (poly_data);
390 
391  // Create an Actor
393  createActorFromVTKDataSet (all_data->GetOutput (), actor);
394  actor->GetProperty ()->SetRepresentationToWireframe ();
395  actor->GetProperty ()->SetColor (r, g, b);
396  actor->GetMapper ()->ScalarVisibilityOn ();
397  actor->GetProperty ()->BackfaceCullingOff ();
398  removeActorFromRenderer (am_it->second, viewport);
399  addActorToRenderer (actor, viewport);
400 
401  // Save the pointer/ID pair to the global actor map
402  (*shape_actor_map_)[id] = actor;
403  }
404  else
405  {
406  // Create an Actor
408  createActorFromVTKDataSet (data, actor);
409  actor->GetProperty ()->SetRepresentationToWireframe ();
410  actor->GetProperty ()->SetColor (r, g, b);
411  actor->GetMapper ()->ScalarVisibilityOn ();
412  actor->GetProperty ()->BackfaceCullingOff ();
413  addActorToRenderer (actor, viewport);
414 
415  // Save the pointer/ID pair to the global actor map
416  (*shape_actor_map_)[id] = actor;
417  }
418  return (true);
419 }
420 
421 ////////////////////////////////////////////////////////////////////////////////////////////
422 template <typename PointT> bool
424  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
425  const std::string &id, int viewport)
426 {
427  return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5, id, viewport));
428 }
429 
430 ////////////////////////////////////////////////////////////////////////////////////////////
431 template <typename P1, typename P2> bool
432 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
433 {
434  if (contains (id))
435  {
436  PCL_WARN ("[addLine] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
437  return (false);
438  }
439 
440  vtkSmartPointer<vtkDataSet> data = createLine (pt1.getVector4fMap (), pt2.getVector4fMap ());
441 
442  // Create an Actor
444  createActorFromVTKDataSet (data, actor);
445  actor->GetProperty ()->SetRepresentationToWireframe ();
446  actor->GetProperty ()->SetColor (r, g, b);
447  actor->GetMapper ()->ScalarVisibilityOff ();
448  addActorToRenderer (actor, viewport);
449 
450  // Save the pointer/ID pair to the global actor map
451  (*shape_actor_map_)[id] = actor;
452  return (true);
453 }
454 
455 ////////////////////////////////////////////////////////////////////////////////////////////
456 template <typename P1, typename P2> bool
457 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
458 {
459  if (contains (id))
460  {
461  PCL_WARN ("[addArrow] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
462  return (false);
463  }
464 
465  // Create an Actor
467  leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
468  leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
469  leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
470  leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
471  leader->SetArrowStyleToFilled ();
472  leader->AutoLabelOn ();
473 
474  leader->GetProperty ()->SetColor (r, g, b);
475  addActorToRenderer (leader, viewport);
476 
477  // Save the pointer/ID pair to the global actor map
478  (*shape_actor_map_)[id] = leader;
479  return (true);
480 }
481 
482 ////////////////////////////////////////////////////////////////////////////////////////////
483 template <typename P1, typename P2> bool
484 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)
485 {
486  if (contains (id))
487  {
488  PCL_WARN ("[addArrow] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
489  return (false);
490  }
491 
492  // Create an Actor
494  leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
495  leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
496  leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
497  leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
498  leader->SetArrowStyleToFilled ();
499  leader->SetArrowPlacementToPoint1 ();
500  if (display_length)
501  leader->AutoLabelOn ();
502  else
503  leader->AutoLabelOff ();
504 
505  leader->GetProperty ()->SetColor (r, g, b);
506  addActorToRenderer (leader, viewport);
507 
508  // Save the pointer/ID pair to the global actor map
509  (*shape_actor_map_)[id] = leader;
510  return (true);
511 }
512 ////////////////////////////////////////////////////////////////////////////////////////////
513 template <typename P1, typename P2> bool
514 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2,
515  double r_line, double g_line, double b_line,
516  double r_text, double g_text, double b_text,
517  const std::string &id, int viewport)
518 {
519  if (contains (id))
520  {
521  PCL_WARN ("[addArrow] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
522  return (false);
523  }
524 
525  // Create an Actor
527  leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
528  leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
529  leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
530  leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
531  leader->SetArrowStyleToFilled ();
532  leader->AutoLabelOn ();
533 
534  leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
535 
536  leader->GetProperty ()->SetColor (r_line, g_line, b_line);
537  addActorToRenderer (leader, viewport);
538 
539  // Save the pointer/ID pair to the global actor map
540  (*shape_actor_map_)[id] = leader;
541  return (true);
542 }
543 
544 ////////////////////////////////////////////////////////////////////////////////////////////
545 template <typename P1, typename P2> bool
546 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, const std::string &id, int viewport)
547 {
548  return (!addLine (pt1, pt2, 0.5, 0.5, 0.5, id, viewport));
549 }
550 
551 ////////////////////////////////////////////////////////////////////////////////////////////
552 template <typename PointT> bool
553 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id, int viewport)
554 {
555  if (contains (id))
556  {
557  PCL_WARN ("[addSphere] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
558  return (false);
559  }
560 
562  data->SetRadius (radius);
563  data->SetCenter (double (center.x), double (center.y), double (center.z));
564  data->SetPhiResolution (10);
565  data->SetThetaResolution (10);
566  data->LatLongTessellationOff ();
567  data->Update ();
568 
569  // Setup actor and mapper
571  mapper->SetInputConnection (data->GetOutputPort ());
572 
573  // Create an Actor
575  actor->SetMapper (mapper);
576  //createActorFromVTKDataSet (data, actor);
577  actor->GetProperty ()->SetRepresentationToSurface ();
578  actor->GetProperty ()->SetInterpolationToFlat ();
579  actor->GetProperty ()->SetColor (r, g, b);
580 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
581  actor->GetMapper ()->ImmediateModeRenderingOn ();
582 #endif
583  actor->GetMapper ()->StaticOn ();
584  actor->GetMapper ()->ScalarVisibilityOff ();
585  actor->GetMapper ()->Update ();
586  addActorToRenderer (actor, viewport);
587 
588  // Save the pointer/ID pair to the global actor map
589  (*shape_actor_map_)[id] = actor;
590  return (true);
591 }
592 
593 ////////////////////////////////////////////////////////////////////////////////////////////
594 template <typename PointT> bool
595 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, const std::string &id, int viewport)
596 {
597  return (addSphere (center, radius, 0.5, 0.5, 0.5, id, viewport));
598 }
599 
600 ////////////////////////////////////////////////////////////////////////////////////////////
601 template<typename PointT> bool
602 pcl::visualization::PCLVisualizer::updateSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id)
603 {
604  if (!contains (id))
605  {
606  return (false);
607  }
608 
609  //////////////////////////////////////////////////////////////////////////
610  // Get the actor pointer
611  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
612  vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
613  if (!actor)
614  return (false);
615  vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
616  vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
617  if (!src)
618  return (false);
619 
620  src->SetCenter (double (center.x), double (center.y), double (center.z));
621  src->SetRadius (radius);
622  src->Update ();
623  actor->GetProperty ()->SetColor (r, g, b);
624  actor->Modified ();
625 
626  return (true);
627 }
628 
629 //////////////////////////////////////////////////
630 template <typename PointT> bool
632  const std::string &text,
633  const PointT& position,
634  double textScale,
635  double r,
636  double g,
637  double b,
638  const std::string &id,
639  int viewport)
640 {
641  std::string tid;
642  if (id.empty ())
643  tid = text;
644  else
645  tid = id;
646 
647  if (viewport < 0)
648  return false;
649 
650  // If there is no custom viewport and the viewport number is not 0, exit
651  if (rens_->GetNumberOfItems () <= viewport)
652  {
653  PCL_ERROR ("[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
654  viewport,
655  tid.c_str ());
656  return false;
657  }
658 
659  // check all or an individual viewport for a similar id
660  rens_->InitTraversal ();
661  for (size_t i = viewport; rens_->GetNextItem (); ++i)
662  {
663  const std::string uid = tid + std::string (i, '*');
664  if (contains (uid))
665  {
666  PCL_ERROR ( "[addText3D] The id <%s> already exists in viewport [%d]! "
667  "Please choose a different id and retry.\n",
668  tid.c_str (),
669  i);
670  return false;
671  }
672 
673  if (viewport > 0)
674  break;
675  }
676 
678  textSource->SetText (text.c_str());
679  textSource->Update ();
680 
682  textMapper->SetInputConnection (textSource->GetOutputPort ());
683 
684  // Since each follower may follow a different camera, we need different followers
685  rens_->InitTraversal ();
686  vtkRenderer* renderer;
687  int i = 0;
688  while ((renderer = rens_->GetNextItem ()))
689  {
690  // Should we add the actor to all renderers or just to i-nth renderer?
691  if (viewport == 0 || viewport == i)
692  {
694  textActor->SetMapper (textMapper);
695  textActor->SetPosition (position.x, position.y, position.z);
696  textActor->SetScale (textScale);
697  textActor->GetProperty ()->SetColor (r, g, b);
698  textActor->SetCamera (renderer->GetActiveCamera ());
699 
700  renderer->AddActor (textActor);
701  renderer->Render ();
702 
703  // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers
704  // for multiple viewport
705  const std::string uid = tid + std::string (i, '*');
706  (*shape_actor_map_)[uid] = textActor;
707  }
708 
709  ++i;
710  }
711 
712  return (true);
713 }
714 
715 //////////////////////////////////////////////////
716 template <typename PointT> bool
718  const std::string &text,
719  const PointT& position,
720  double orientation[3],
721  double textScale,
722  double r,
723  double g,
724  double b,
725  const std::string &id,
726  int viewport)
727 {
728  std::string tid;
729  if (id.empty ())
730  tid = text;
731  else
732  tid = id;
733 
734  if (viewport < 0)
735  return false;
736 
737  // If there is no custom viewport and the viewport number is not 0, exit
738  if (rens_->GetNumberOfItems () <= viewport)
739  {
740  PCL_ERROR ("[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
741  viewport,
742  tid.c_str ());
743  return false;
744  }
745 
746  // check all or an individual viewport for a similar id
747  rens_->InitTraversal ();
748  for (size_t i = viewport; rens_->GetNextItem (); ++i)
749  {
750  const std::string uid = tid + std::string (i, '*');
751  if (contains (uid))
752  {
753  PCL_ERROR ( "[addText3D] The id <%s> already exists in viewport [%d]! "
754  "Please choose a different id and retry.\n",
755  tid.c_str (),
756  i);
757  return false;
758  }
759 
760  if (viewport > 0)
761  break;
762  }
763 
765  textSource->SetText (text.c_str());
766  textSource->Update ();
767 
769  textMapper->SetInputConnection (textSource->GetOutputPort ());
770 
772  textActor->SetMapper (textMapper);
773  textActor->SetPosition (position.x, position.y, position.z);
774  textActor->SetScale (textScale);
775  textActor->GetProperty ()->SetColor (r, g, b);
776  textActor->SetOrientation (orientation);
777 
778  // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers
779  rens_->InitTraversal ();
780  int i = 0;
781  for ( vtkRenderer* renderer = rens_->GetNextItem ();
782  renderer;
783  renderer = rens_->GetNextItem (), ++i)
784  {
785  if (viewport == 0 || viewport == i)
786  {
787  renderer->AddActor (textActor);
788  const std::string uid = tid + std::string (i, '*');
789  (*shape_actor_map_)[uid] = textActor;
790  }
791  }
792 
793  return (true);
794 }
795 
796 //////////////////////////////////////////////////////////////////////////////////////////////
797 template <typename PointNT> bool
799  const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
800  int level, float scale, const std::string &id, int viewport)
801 {
802  return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale, id, viewport));
803 }
804 
805 //////////////////////////////////////////////////////////////////////////////////////////////
806 template <typename PointT, typename PointNT> bool
808  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
809  const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
810  int level, float scale,
811  const std::string &id, int viewport)
812 {
813  if (normals->points.size () != cloud->points.size ())
814  {
815  PCL_ERROR ("[addPointCloudNormals] The number of points differs from the number of normals!\n");
816  return (false);
817  }
818 
819  if (normals->empty ())
820  {
821  PCL_WARN ("[addPointCloudNormals] An empty normal cloud is given! Nothing to display.\n");
822  return (false);
823  }
824 
825  if (contains (id))
826  {
827  PCL_WARN ("[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
828  return (false);
829  }
830 
833 
834  points->SetDataTypeToFloat ();
836  data->SetNumberOfComponents (3);
837 
838 
839  vtkIdType nr_normals = 0;
840  float* pts = nullptr;
841 
842  // If the cloud is organized, then distribute the normal step in both directions
843  if (cloud->isOrganized () && normals->isOrganized ())
844  {
845  vtkIdType point_step = static_cast<vtkIdType> (sqrt (double (level)));
846  nr_normals = (static_cast<vtkIdType> ((cloud->width - 1)/ point_step) + 1) *
847  (static_cast<vtkIdType> ((cloud->height - 1) / point_step) + 1);
848  pts = new float[2 * nr_normals * 3];
849 
850  vtkIdType cell_count = 0;
851  for (vtkIdType y = 0; y < normals->height; y += point_step)
852  for (vtkIdType x = 0; x < normals->width; x += point_step)
853  {
854  PointT p = (*cloud)(x, y);
855  p.x += (*normals)(x, y).normal[0] * scale;
856  p.y += (*normals)(x, y).normal[1] * scale;
857  p.z += (*normals)(x, y).normal[2] * scale;
858 
859  pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
860  pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
861  pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
862  pts[2 * cell_count * 3 + 3] = p.x;
863  pts[2 * cell_count * 3 + 4] = p.y;
864  pts[2 * cell_count * 3 + 5] = p.z;
865 
866  lines->InsertNextCell (2);
867  lines->InsertCellPoint (2 * cell_count);
868  lines->InsertCellPoint (2 * cell_count + 1);
869  cell_count ++;
870  }
871  }
872  else
873  {
874  nr_normals = (cloud->points.size () - 1) / level + 1 ;
875  pts = new float[2 * nr_normals * 3];
876 
877  for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
878  {
879  PointT p = cloud->points[i];
880  p.x += normals->points[i].normal[0] * scale;
881  p.y += normals->points[i].normal[1] * scale;
882  p.z += normals->points[i].normal[2] * scale;
883 
884  pts[2 * j * 3 + 0] = cloud->points[i].x;
885  pts[2 * j * 3 + 1] = cloud->points[i].y;
886  pts[2 * j * 3 + 2] = cloud->points[i].z;
887  pts[2 * j * 3 + 3] = p.x;
888  pts[2 * j * 3 + 4] = p.y;
889  pts[2 * j * 3 + 5] = p.z;
890 
891  lines->InsertNextCell (2);
892  lines->InsertCellPoint (2 * j);
893  lines->InsertCellPoint (2 * j + 1);
894  }
895  }
896 
897  data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
898  points->SetData (data);
899 
901  polyData->SetPoints (points);
902  polyData->SetLines (lines);
903 
905  mapper->SetInputData (polyData);
906  mapper->SetColorModeToMapScalars();
907  mapper->SetScalarModeToUsePointData();
908 
909  // create actor
911  actor->SetMapper (mapper);
912 
913  // Use cloud view point info
915  convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
916  actor->SetUserMatrix (transformation);
917 
918  // Add it to all renderers
919  addActorToRenderer (actor, viewport);
920 
921  // Save the pointer/ID pair to the global actor map
922  (*cloud_actor_map_)[id].actor = actor;
923  return (true);
924 }
925 
926 //////////////////////////////////////////////////////////////////////////////////////////////
927 template <typename PointNT> bool
929  const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
931  int level, float scale,
932  const std::string &id, int viewport)
933 {
934  return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale, id, viewport));
935 }
936 
937 //////////////////////////////////////////////////////////////////////////////////////////////
938 template <typename PointT, typename PointNT> bool
940  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
941  const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
943  int level, float scale,
944  const std::string &id, int viewport)
945 {
946  if (pcs->points.size () != cloud->points.size () || normals->points.size () != cloud->points.size ())
947  {
948  pcl::console::print_error ("[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
949  return (false);
950  }
951 
952  if (contains (id))
953  {
954  PCL_WARN ("[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
955  return (false);
956  }
957 
960 
961  // Setup two colors - one for each line
962  unsigned char green[3] = {0, 255, 0};
963  unsigned char blue[3] = {0, 0, 255};
964 
965  // Setup the colors array
967  line_1_colors->SetNumberOfComponents (3);
968  line_1_colors->SetName ("Colors");
970  line_2_colors->SetNumberOfComponents (3);
971  line_2_colors->SetName ("Colors");
972 
973  // Create the first sets of lines
974  for (size_t i = 0; i < cloud->points.size (); i+=level)
975  {
976  PointT p = cloud->points[i];
977  p.x += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[0]) * scale;
978  p.y += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[1]) * scale;
979  p.z += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[2]) * scale;
980 
982  line_1->SetPoint1 (cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
983  line_1->SetPoint2 (p.x, p.y, p.z);
984  line_1->Update ();
985  polydata_1->AddInputData (line_1->GetOutput ());
986  line_1_colors->InsertNextTupleValue (green);
987  }
988  polydata_1->Update ();
989  vtkSmartPointer<vtkPolyData> line_1_data = polydata_1->GetOutput ();
990  line_1_data->GetCellData ()->SetScalars (line_1_colors);
991 
992  // Create the second sets of lines
993  for (size_t i = 0; i < cloud->points.size (); i += level)
994  {
995  Eigen::Vector3f pc (pcs->points[i].principal_curvature[0],
996  pcs->points[i].principal_curvature[1],
997  pcs->points[i].principal_curvature[2]);
998  Eigen::Vector3f normal (normals->points[i].normal[0],
999  normals->points[i].normal[1],
1000  normals->points[i].normal[2]);
1001  Eigen::Vector3f pc_c = pc.cross (normal);
1002 
1003  PointT p = cloud->points[i];
1004  p.x += (pcs->points[i].pc2 * pc_c[0]) * scale;
1005  p.y += (pcs->points[i].pc2 * pc_c[1]) * scale;
1006  p.z += (pcs->points[i].pc2 * pc_c[2]) * scale;
1007 
1009  line_2->SetPoint1 (cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
1010  line_2->SetPoint2 (p.x, p.y, p.z);
1011  line_2->Update ();
1012  polydata_2->AddInputData (line_2->GetOutput ());
1013 
1014  line_2_colors->InsertNextTupleValue (blue);
1015  }
1016  polydata_2->Update ();
1017  vtkSmartPointer<vtkPolyData> line_2_data = polydata_2->GetOutput ();
1018  line_2_data->GetCellData ()->SetScalars (line_2_colors);
1019 
1020  // Assemble the two sets of lines
1022  alldata->AddInputData (line_1_data);
1023  alldata->AddInputData (line_2_data);
1024 
1025  // Create an Actor
1027  createActorFromVTKDataSet (alldata->GetOutput (), actor);
1028  actor->GetMapper ()->SetScalarModeToUseCellData ();
1029 
1030  // Add it to all renderers
1031  addActorToRenderer (actor, viewport);
1032 
1033  // Save the pointer/ID pair to the global actor map
1034  CloudActor act;
1035  act.actor = actor;
1036  (*cloud_actor_map_)[id] = act;
1037  return (true);
1038 }
1039 
1040 //////////////////////////////////////////////////////////////////////////////////////////////
1041 template <typename PointT, typename GradientT> bool
1043  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1044  const typename pcl::PointCloud<GradientT>::ConstPtr &gradients,
1045  int level, double scale,
1046  const std::string &id, int viewport)
1047 {
1048  if (gradients->points.size () != cloud->points.size ())
1049  {
1050  PCL_ERROR ("[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1051  return (false);
1052  }
1053  if (contains (id))
1054  {
1055  PCL_WARN ("[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1056  return (false);
1057  }
1058 
1061 
1062  points->SetDataTypeToFloat ();
1064  data->SetNumberOfComponents (3);
1065 
1066  vtkIdType nr_gradients = (cloud->points.size () - 1) / level + 1 ;
1067  float* pts = new float[2 * nr_gradients * 3];
1068 
1069  for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1070  {
1071  PointT p = cloud->points[i];
1072  p.x += gradients->points[i].gradient[0] * scale;
1073  p.y += gradients->points[i].gradient[1] * scale;
1074  p.z += gradients->points[i].gradient[2] * scale;
1075 
1076  pts[2 * j * 3 + 0] = cloud->points[i].x;
1077  pts[2 * j * 3 + 1] = cloud->points[i].y;
1078  pts[2 * j * 3 + 2] = cloud->points[i].z;
1079  pts[2 * j * 3 + 3] = p.x;
1080  pts[2 * j * 3 + 4] = p.y;
1081  pts[2 * j * 3 + 5] = p.z;
1082 
1083  lines->InsertNextCell(2);
1084  lines->InsertCellPoint(2*j);
1085  lines->InsertCellPoint(2*j+1);
1086  }
1087 
1088  data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1089  points->SetData (data);
1090 
1092  polyData->SetPoints(points);
1093  polyData->SetLines(lines);
1094 
1096  mapper->SetInputData (polyData);
1097  mapper->SetColorModeToMapScalars();
1098  mapper->SetScalarModeToUsePointData();
1099 
1100  // create actor
1102  actor->SetMapper (mapper);
1103 
1104  // Add it to all renderers
1105  addActorToRenderer (actor, viewport);
1106 
1107  // Save the pointer/ID pair to the global actor map
1108  (*cloud_actor_map_)[id].actor = actor;
1109  return (true);
1110 }
1111 
1112 //////////////////////////////////////////////////////////////////////////////////////////////
1113 template <typename PointT> bool
1115  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1116  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1117  const std::vector<int> &correspondences,
1118  const std::string &id,
1119  int viewport)
1120 {
1121  pcl::Correspondences corrs;
1122  corrs.resize (correspondences.size ());
1123 
1124  size_t index = 0;
1125  for (auto &corr : corrs)
1126  {
1127  corr.index_query = index;
1128  corr.index_match = correspondences[index];
1129  index++;
1130  }
1131 
1132  return (addCorrespondences<PointT> (source_points, target_points, corrs, id, viewport));
1133 }
1134 
1135 //////////////////////////////////////////////////////////////////////////////////////////////
1136 template <typename PointT> bool
1138  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1139  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1140  const pcl::Correspondences &correspondences,
1141  int nth,
1142  const std::string &id,
1143  int viewport,
1144  bool overwrite)
1145 {
1146  if (correspondences.empty ())
1147  {
1148  PCL_DEBUG ("[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1149  return (false);
1150  }
1151 
1152  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1153  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
1154  if (am_it != shape_actor_map_->end () && !overwrite)
1155  {
1156  PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1157  return (false);
1158  }
1159  if (am_it == shape_actor_map_->end () && overwrite)
1160  {
1161  overwrite = false; // Correspondences doesn't exist, add them instead of updating them
1162  }
1163 
1164  int n_corr = int (correspondences.size () / nth);
1166 
1167  // Prepare colors
1169  line_colors->SetNumberOfComponents (3);
1170  line_colors->SetName ("Colors");
1171  line_colors->SetNumberOfTuples (n_corr);
1172 
1173  // Prepare coordinates
1175  line_points->SetNumberOfPoints (2 * n_corr);
1176 
1178  line_cells_id->SetNumberOfComponents (3);
1179  line_cells_id->SetNumberOfTuples (n_corr);
1180  vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1182 
1184  line_tcoords->SetNumberOfComponents (1);
1185  line_tcoords->SetNumberOfTuples (n_corr * 2);
1186  line_tcoords->SetName ("Texture Coordinates");
1187 
1188  double tc[3] = {0.0, 0.0, 0.0};
1189 
1190  Eigen::Affine3f source_transformation;
1191  source_transformation.linear () = source_points->sensor_orientation_.matrix ();
1192  source_transformation.translation () = source_points->sensor_origin_.head (3);
1193  Eigen::Affine3f target_transformation;
1194  target_transformation.linear () = target_points->sensor_orientation_.matrix ();
1195  target_transformation.translation () = target_points->sensor_origin_.head (3);
1196 
1197  int j = 0;
1198  // Draw lines between the best corresponding points
1199  for (size_t i = 0; i < correspondences.size (); i += nth, ++j)
1200  {
1201  if (correspondences[i].index_match == -1)
1202  {
1203  PCL_WARN ("[addCorrespondences] No valid index_match for correspondence %d\n", i);
1204  continue;
1205  }
1206 
1207  PointT p_src (source_points->points[correspondences[i].index_query]);
1208  PointT p_tgt (target_points->points[correspondences[i].index_match]);
1209 
1210  p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1211  p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1212 
1213  int id1 = j * 2 + 0, id2 = j * 2 + 1;
1214  // Set the points
1215  line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1216  line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1217  // Set the cell ID
1218  *line_cell_id++ = 2;
1219  *line_cell_id++ = id1;
1220  *line_cell_id++ = id2;
1221  // Set the texture coords
1222  tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1223  tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1224 
1225  float rgb[3];
1226  rgb[0] = vtkMath::Random (32, 255); // min / max
1227  rgb[1] = vtkMath::Random (32, 255);
1228  rgb[2] = vtkMath::Random (32, 255);
1229  line_colors->InsertTuple (i, rgb);
1230  }
1231  line_colors->SetNumberOfTuples (j);
1232  line_cells_id->SetNumberOfTuples (j);
1233  line_cells->SetCells (n_corr, line_cells_id);
1234  line_points->SetNumberOfPoints (j*2);
1235  line_tcoords->SetNumberOfTuples (j*2);
1236 
1237  // Fill in the lines
1238  line_data->SetPoints (line_points);
1239  line_data->SetLines (line_cells);
1240  line_data->GetPointData ()->SetTCoords (line_tcoords);
1241  line_data->GetCellData ()->SetScalars (line_colors);
1242 
1243  // Create an Actor
1244  if (!overwrite)
1245  {
1247  createActorFromVTKDataSet (line_data, actor);
1248  actor->GetProperty ()->SetRepresentationToWireframe ();
1249  actor->GetProperty ()->SetOpacity (0.5);
1250  addActorToRenderer (actor, viewport);
1251 
1252  // Save the pointer/ID pair to the global actor map
1253  (*shape_actor_map_)[id] = actor;
1254  }
1255  else
1256  {
1257  vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
1258  if (!actor)
1259  return (false);
1260  // Update the mapper
1261  reinterpret_cast<vtkPolyDataMapper*> (actor->GetMapper ())->SetInputData (line_data);
1262  }
1263 
1264  return (true);
1265 }
1266 
1267 //////////////////////////////////////////////////////////////////////////////////////////////
1268 template <typename PointT> bool
1270  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1271  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1272  const pcl::Correspondences &correspondences,
1273  int nth,
1274  const std::string &id,
1275  int viewport)
1276 {
1277  return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth, id, viewport, true));
1278 }
1279 
1280 //////////////////////////////////////////////////////////////////////////////////////////////
1281 template <typename PointT> bool
1282 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1283  const PointCloudGeometryHandler<PointT> &geometry_handler,
1284  const PointCloudColorHandler<PointT> &color_handler,
1285  const std::string &id,
1286  int viewport,
1287  const Eigen::Vector4f& sensor_origin,
1288  const Eigen::Quaternion<float>& sensor_orientation)
1289 {
1290  if (!geometry_handler.isCapable ())
1291  {
1292  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
1293  return (false);
1294  }
1295 
1296  if (!color_handler.isCapable ())
1297  {
1298  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
1299  return (false);
1300  }
1301 
1304  // Convert the PointCloud to VTK PolyData
1305  convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1306  // use the given geometry handler
1307 
1308  // Get the colors from the handler
1309  bool has_colors = false;
1310  double minmax[2];
1311  if (auto scalars = color_handler.getColor ())
1312  {
1313  polydata->GetPointData ()->SetScalars (scalars);
1314  scalars->GetRange (minmax);
1315  has_colors = true;
1316  }
1317 
1318  // Create an Actor
1320  createActorFromVTKDataSet (polydata, actor);
1321  if (has_colors)
1322  actor->GetMapper ()->SetScalarRange (minmax);
1323 
1324  // Add it to all renderers
1325  addActorToRenderer (actor, viewport);
1326 
1327  // Save the pointer/ID pair to the global actor map
1328  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1329  cloud_actor.actor = actor;
1330  cloud_actor.cells = initcells;
1331 
1332  // Save the viewpoint transformation matrix to the global actor map
1334  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1335  cloud_actor.viewpoint_transformation_ = transformation;
1336  cloud_actor.actor->SetUserMatrix (transformation);
1337  cloud_actor.actor->Modified ();
1338 
1339  return (true);
1340 }
1341 
1342 //////////////////////////////////////////////////////////////////////////////////////////////
1343 template <typename PointT> bool
1344 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1345  const PointCloudGeometryHandler<PointT> &geometry_handler,
1346  const ColorHandlerConstPtr &color_handler,
1347  const std::string &id,
1348  int viewport,
1349  const Eigen::Vector4f& sensor_origin,
1350  const Eigen::Quaternion<float>& sensor_orientation)
1351 {
1352  if (!geometry_handler.isCapable ())
1353  {
1354  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
1355  return (false);
1356  }
1357 
1358  if (!color_handler->isCapable ())
1359  {
1360  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler->getName ().c_str ());
1361  return (false);
1362  }
1363 
1366  // Convert the PointCloud to VTK PolyData
1367  convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1368  // use the given geometry handler
1369 
1370  // Get the colors from the handler
1371  bool has_colors = false;
1372  double minmax[2];
1373  if (auto scalars = color_handler->getColor ())
1374  {
1375  polydata->GetPointData ()->SetScalars (scalars);
1376  scalars->GetRange (minmax);
1377  has_colors = true;
1378  }
1379 
1380  // Create an Actor
1382  createActorFromVTKDataSet (polydata, actor);
1383  if (has_colors)
1384  actor->GetMapper ()->SetScalarRange (minmax);
1385 
1386  // Add it to all renderers
1387  addActorToRenderer (actor, viewport);
1388 
1389  // Save the pointer/ID pair to the global actor map
1390  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1391  cloud_actor.actor = actor;
1392  cloud_actor.cells = initcells;
1393  cloud_actor.color_handlers.push_back (color_handler);
1394 
1395  // Save the viewpoint transformation matrix to the global actor map
1397  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1398  cloud_actor.viewpoint_transformation_ = transformation;
1399  cloud_actor.actor->SetUserMatrix (transformation);
1400  cloud_actor.actor->Modified ();
1401 
1402  return (true);
1403 }
1404 
1405 //////////////////////////////////////////////////////////////////////////////////////////////
1406 template <typename PointT> bool
1407 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1408  const GeometryHandlerConstPtr &geometry_handler,
1409  const PointCloudColorHandler<PointT> &color_handler,
1410  const std::string &id,
1411  int viewport,
1412  const Eigen::Vector4f& sensor_origin,
1413  const Eigen::Quaternion<float>& sensor_orientation)
1414 {
1415  if (!geometry_handler->isCapable ())
1416  {
1417  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler->getName ().c_str ());
1418  return (false);
1419  }
1420 
1421  if (!color_handler.isCapable ())
1422  {
1423  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
1424  return (false);
1425  }
1426 
1429  // Convert the PointCloud to VTK PolyData
1430  convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1431  // use the given geometry handler
1432 
1433  // Get the colors from the handler
1434  bool has_colors = false;
1435  double minmax[2];
1436  if (auto scalars = color_handler.getColor ())
1437  {
1438  polydata->GetPointData ()->SetScalars (scalars);
1439  scalars->GetRange (minmax);
1440  has_colors = true;
1441  }
1442 
1443  // Create an Actor
1445  createActorFromVTKDataSet (polydata, actor);
1446  if (has_colors)
1447  actor->GetMapper ()->SetScalarRange (minmax);
1448 
1449  // Add it to all renderers
1450  addActorToRenderer (actor, viewport);
1451 
1452  // Save the pointer/ID pair to the global actor map
1453  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1454  cloud_actor.actor = actor;
1455  cloud_actor.cells = initcells;
1456  cloud_actor.geometry_handlers.push_back (geometry_handler);
1457 
1458  // Save the viewpoint transformation matrix to the global actor map
1460  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1461  cloud_actor.viewpoint_transformation_ = transformation;
1462  cloud_actor.actor->SetUserMatrix (transformation);
1463  cloud_actor.actor->Modified ();
1464 
1465  return (true);
1466 }
1467 
1468 //////////////////////////////////////////////////////////////////////////////////////////////
1469 template <typename PointT> bool
1471  const std::string &id)
1472 {
1473  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1474  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1475 
1476  if (am_it == cloud_actor_map_->end ())
1477  return (false);
1478 
1479  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1480  // Convert the PointCloud to VTK PolyData
1481  convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1482 
1483  // Set scalars to blank, since there is no way we can update them here.
1485  polydata->GetPointData ()->SetScalars (scalars);
1486  double minmax[2];
1487  minmax[0] = std::numeric_limits<double>::min ();
1488  minmax[1] = std::numeric_limits<double>::max ();
1489 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1490  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1491 #endif
1492  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1493 
1494  // Update the mapper
1495  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1496  return (true);
1497 }
1498 
1499 /////////////////////////////////////////////////////////////////////////////////////////////
1500 template <typename PointT> bool
1502  const PointCloudGeometryHandler<PointT> &geometry_handler,
1503  const std::string &id)
1504 {
1505  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1506  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1507 
1508  if (am_it == cloud_actor_map_->end ())
1509  return (false);
1510 
1511  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1512  if (!polydata)
1513  return (false);
1514  // Convert the PointCloud to VTK PolyData
1515  convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1516 
1517  // Set scalars to blank, since there is no way we can update them here.
1519  polydata->GetPointData ()->SetScalars (scalars);
1520  double minmax[2];
1521  minmax[0] = std::numeric_limits<double>::min ();
1522  minmax[1] = std::numeric_limits<double>::max ();
1523 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1524  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1525 #endif
1526  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1527 
1528  // Update the mapper
1529  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1530  return (true);
1531 }
1532 
1533 
1534 /////////////////////////////////////////////////////////////////////////////////////////////
1535 template <typename PointT> bool
1537  const PointCloudColorHandler<PointT> &color_handler,
1538  const std::string &id)
1539 {
1540  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1541  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1542 
1543  if (am_it == cloud_actor_map_->end ())
1544  return (false);
1545 
1546  // Get the current poly data
1547  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1548  if (!polydata)
1549  return (false);
1550  vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
1551  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
1552  // Copy the new point array in
1553  vtkIdType nr_points = cloud->points.size ();
1554  points->SetNumberOfPoints (nr_points);
1555 
1556  // Get a pointer to the beginning of the data array
1557  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
1558 
1559  vtkIdType pts = 0;
1560  // If the dataset is dense (no NaNs)
1561  if (cloud->is_dense)
1562  {
1563  for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
1564  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[pts]);
1565  }
1566  else
1567  {
1568  vtkIdType j = 0; // true point index
1569  for (vtkIdType i = 0; i < nr_points; ++i)
1570  {
1571  // Check if the point is invalid
1572  if (!isFinite (cloud->points[i]))
1573  continue;
1574  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[pts]);
1575  pts += 3;
1576  j++;
1577  }
1578  nr_points = j;
1579  points->SetNumberOfPoints (nr_points);
1580  }
1581 
1582  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
1583  updateCells (cells, am_it->second.cells, nr_points);
1584 
1585  // Set the cells and the vertices
1586  vertices->SetCells (nr_points, cells);
1587 
1588  // Get the colors from the handler
1589  bool has_colors = false;
1590  double minmax[2];
1591  if (auto scalars = color_handler.getColor ())
1592  {
1593  // Update the data
1594  polydata->GetPointData ()->SetScalars (scalars);
1595  scalars->GetRange (minmax);
1596  has_colors = true;
1597  }
1598 
1599 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1600  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1601 #endif
1602 
1603  if (has_colors)
1604  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1605 
1606  // Update the mapper
1607  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1608  return (true);
1609 }
1610 
1611 /////////////////////////////////////////////////////////////////////////////////////////////
1612 template <typename PointT> bool
1614  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1615  const std::vector<pcl::Vertices> &vertices,
1616  const std::string &id,
1617  int viewport)
1618 {
1619  if (vertices.empty () || cloud->points.empty ())
1620  return (false);
1621 
1622  if (contains (id))
1623  {
1624  PCL_WARN ("[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1625  return (false);
1626  }
1627 
1628  int rgb_idx = -1;
1629  std::vector<pcl::PCLPointField> fields;
1631  rgb_idx = pcl::getFieldIndex (*cloud, "rgb", fields);
1632  if (rgb_idx == -1)
1633  rgb_idx = pcl::getFieldIndex (*cloud, "rgba", fields);
1634  if (rgb_idx != -1)
1635  {
1637  colors->SetNumberOfComponents (3);
1638  colors->SetName ("Colors");
1639  uint32_t offset = fields[rgb_idx].offset;
1640  for (size_t i = 0; i < cloud->size (); ++i)
1641  {
1642  if (!isFinite (cloud->points[i]))
1643  continue;
1644  const pcl::RGB* const rgb_data = reinterpret_cast<const pcl::RGB*>(reinterpret_cast<const char*> (&cloud->points[i]) + offset);
1645  unsigned char color[3];
1646  color[0] = rgb_data->r;
1647  color[1] = rgb_data->g;
1648  color[2] = rgb_data->b;
1649  colors->InsertNextTupleValue (color);
1650  }
1651  }
1652 
1653  // Create points from polyMesh.cloud
1655  vtkIdType nr_points = cloud->points.size ();
1656  points->SetNumberOfPoints (nr_points);
1658 
1659  // Get a pointer to the beginning of the data array
1660  float *data = static_cast<vtkFloatArray*> (points->GetData ())->GetPointer (0);
1661 
1662  vtkIdType ptr = 0;
1663  std::vector<int> lookup;
1664  // If the dataset is dense (no NaNs)
1665  if (cloud->is_dense)
1666  {
1667  for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1668  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
1669  }
1670  else
1671  {
1672  lookup.resize (nr_points);
1673  vtkIdType j = 0; // true point index
1674  for (vtkIdType i = 0; i < nr_points; ++i)
1675  {
1676  // Check if the point is invalid
1677  if (!isFinite (cloud->points[i]))
1678  continue;
1679 
1680  lookup[i] = static_cast<int> (j);
1681  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
1682  j++;
1683  ptr += 3;
1684  }
1685  nr_points = j;
1686  points->SetNumberOfPoints (nr_points);
1687  }
1688 
1689  // Get the maximum size of a polygon
1690  int max_size_of_polygon = -1;
1691  for (const auto &vertex : vertices)
1692  if (max_size_of_polygon < static_cast<int> (vertex.vertices.size ()))
1693  max_size_of_polygon = static_cast<int> (vertex.vertices.size ());
1694 
1695  if (vertices.size () > 1)
1696  {
1697  // Create polys from polyMesh.polygons
1699  vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
1700  int idx = 0;
1701  if (!lookup.empty ())
1702  {
1703  for (size_t i = 0; i < vertices.size (); ++i, ++idx)
1704  {
1705  size_t n_points = vertices[i].vertices.size ();
1706  *cell++ = n_points;
1707  //cell_array->InsertNextCell (n_points);
1708  for (size_t j = 0; j < n_points; j++, ++idx)
1709  *cell++ = lookup[vertices[i].vertices[j]];
1710  //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
1711  }
1712  }
1713  else
1714  {
1715  for (size_t i = 0; i < vertices.size (); ++i, ++idx)
1716  {
1717  size_t n_points = vertices[i].vertices.size ();
1718  *cell++ = n_points;
1719  //cell_array->InsertNextCell (n_points);
1720  for (size_t j = 0; j < n_points; j++, ++idx)
1721  *cell++ = vertices[i].vertices[j];
1722  //cell_array->InsertCellPoint (vertices[i].vertices[j]);
1723  }
1724  }
1726  allocVtkPolyData (polydata);
1727  cell_array->GetData ()->SetNumberOfValues (idx);
1728  cell_array->Squeeze ();
1729  polydata->SetPolys (cell_array);
1730  polydata->SetPoints (points);
1731 
1732  if (colors)
1733  polydata->GetPointData ()->SetScalars (colors);
1734 
1735  createActorFromVTKDataSet (polydata, actor, false);
1736  }
1737  else
1738  {
1740  size_t n_points = vertices[0].vertices.size ();
1741  polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1742 
1743  if (!lookup.empty ())
1744  {
1745  for (size_t j = 0; j < (n_points - 1); ++j)
1746  polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1747  }
1748  else
1749  {
1750  for (size_t j = 0; j < (n_points - 1); ++j)
1751  polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1752  }
1754  allocVtkUnstructuredGrid (poly_grid);
1755  poly_grid->Allocate (1, 1);
1756  poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1757  poly_grid->SetPoints (points);
1758  if (colors)
1759  poly_grid->GetPointData ()->SetScalars (colors);
1760 
1761  createActorFromVTKDataSet (poly_grid, actor, false);
1762  }
1763  addActorToRenderer (actor, viewport);
1764  actor->GetProperty ()->SetRepresentationToSurface ();
1765  // Backface culling renders the visualization slower, but guarantees that we see all triangles
1766  actor->GetProperty ()->BackfaceCullingOff ();
1767  actor->GetProperty ()->SetInterpolationToFlat ();
1768  actor->GetProperty ()->EdgeVisibilityOff ();
1769  actor->GetProperty ()->ShadingOff ();
1770 
1771  // Save the pointer/ID pair to the global actor map
1772  (*cloud_actor_map_)[id].actor = actor;
1773 
1774  // Save the viewpoint transformation matrix to the global actor map
1776  convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
1777  (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1778 
1779  return (true);
1780 }
1781 
1782 /////////////////////////////////////////////////////////////////////////////////////////////
1783 template <typename PointT> bool
1785  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1786  const std::vector<pcl::Vertices> &verts,
1787  const std::string &id)
1788 {
1789  if (verts.empty ())
1790  {
1791  pcl::console::print_error ("[addPolygonMesh] No vertices given!\n");
1792  return (false);
1793  }
1794 
1795  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1796  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1797  if (am_it == cloud_actor_map_->end ())
1798  return (false);
1799 
1800  // Get the current poly data
1801  vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1802  if (!polydata)
1803  return (false);
1804  vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
1805  if (!cells)
1806  return (false);
1807  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
1808  // Copy the new point array in
1809  vtkIdType nr_points = cloud->points.size ();
1810  points->SetNumberOfPoints (nr_points);
1811 
1812  // Get a pointer to the beginning of the data array
1813  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
1814 
1815  int ptr = 0;
1816  std::vector<int> lookup;
1817  // If the dataset is dense (no NaNs)
1818  if (cloud->is_dense)
1819  {
1820  for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1821  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
1822  }
1823  else
1824  {
1825  lookup.resize (nr_points);
1826  vtkIdType j = 0; // true point index
1827  for (vtkIdType i = 0; i < nr_points; ++i)
1828  {
1829  // Check if the point is invalid
1830  if (!isFinite (cloud->points[i]))
1831  continue;
1832 
1833  lookup [i] = static_cast<int> (j);
1834  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
1835  j++;
1836  ptr += 3;
1837  }
1838  nr_points = j;
1839  points->SetNumberOfPoints (nr_points);
1840  }
1841 
1842  // Update colors
1843  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1844  if (!colors)
1845  return (false);
1846  int rgb_idx = -1;
1847  std::vector<pcl::PCLPointField> fields;
1848  rgb_idx = pcl::getFieldIndex (*cloud, "rgb", fields);
1849  if (rgb_idx == -1)
1850  rgb_idx = pcl::getFieldIndex (*cloud, "rgba", fields);
1851  if (rgb_idx != -1 && colors)
1852  {
1853  int j = 0;
1854  uint32_t offset = fields[rgb_idx].offset;
1855  for (size_t i = 0; i < cloud->size (); ++i)
1856  {
1857  if (!isFinite (cloud->points[i]))
1858  continue;
1859  const pcl::RGB* const rgb_data = reinterpret_cast<const pcl::RGB*>(reinterpret_cast<const char*> (&cloud->points[i]) + offset);
1860  unsigned char color[3];
1861  color[0] = rgb_data->r;
1862  color[1] = rgb_data->g;
1863  color[2] = rgb_data->b;
1864  colors->SetTupleValue (j++, color);
1865  }
1866  }
1867 
1868  // Get the maximum size of a polygon
1869  int max_size_of_polygon = -1;
1870  for (const auto &vertex : verts)
1871  if (max_size_of_polygon < static_cast<int> (vertex.vertices.size ()))
1872  max_size_of_polygon = static_cast<int> (vertex.vertices.size ());
1873 
1874  // Update the cells
1876  vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
1877  int idx = 0;
1878  if (!lookup.empty ())
1879  {
1880  for (size_t i = 0; i < verts.size (); ++i, ++idx)
1881  {
1882  size_t n_points = verts[i].vertices.size ();
1883  *cell++ = n_points;
1884  for (size_t j = 0; j < n_points; j++, cell++, ++idx)
1885  *cell = lookup[verts[i].vertices[j]];
1886  }
1887  }
1888  else
1889  {
1890  for (size_t i = 0; i < verts.size (); ++i, ++idx)
1891  {
1892  size_t n_points = verts[i].vertices.size ();
1893  *cell++ = n_points;
1894  for (size_t j = 0; j < n_points; j++, cell++, ++idx)
1895  *cell = verts[i].vertices[j];
1896  }
1897  }
1898  cells->GetData ()->SetNumberOfValues (idx);
1899  cells->Squeeze ();
1900  // Set the the vertices
1901  polydata->SetPolys (cells);
1902 
1903  return (true);
1904 }
1905 
1906 #ifdef vtkGenericDataArray_h
1907 #undef SetTupleValue
1908 #undef InsertNextTupleValue
1909 #undef GetTupleValue
1910 #endif
1911 
1912 #endif
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:53
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:58
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:423
bool addPointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a Point Cloud (templated) to screen.
virtual void getGeometry(vtkSmartPointer< vtkPoints > &points) const =0
Obtain the actual point geometry for the input dataset in VTK format.
static void convertToVtkMatrix(const Eigen::Matrix4f &m, vtkSmartPointer< vtkMatrix4x4 > &vtk_matrix)
Convert Eigen::Matrix4f to vtkMatrix4x4.
std::vector< ColorHandlerConstPtr > color_handlers
A vector of color handlers that can be used for rendering the data.
Definition: actor_map.h:85
bool addPointCloudIntensityGradients(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const typename pcl::PointCloud< GradientT >::ConstPtr &gradients, int level=100, double scale=1e-6, const std::string &id="cloud", int viewport=0)
Add the estimated surface intensity gradients of a Point Cloud to screen.
bool addPolygon(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, double r, double g, double b, const std::string &id="polygon", int viewport=0)
Add a polygon (polyline) that represents the input point cloud (connects all points in order) ...
bool addPointCloudPrincipalCurvatures(const typename pcl::PointCloud< PointNT >::ConstPtr &cloud, const typename pcl::PointCloud< pcl::PrincipalCurvatures >::ConstPtr &pcs, int level=100, float scale=1.0f, const std::string &id="cloud", int viewport=0)
Add the estimated principal curvatures of a Point Cloud to screen.
Base Handler class for PointCloud colors.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:428
bool addSphere(const PointT &center, double radius, const std::string &id="sphere", int viewport=0)
Add a sphere shape from a point and a radius.
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:344
bool isCapable() const
Checl if this handler is capable of handling the input data or not.
bool addCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const std::vector< int > &correspondences, const std::string &id="correspondences", int viewport=0)
Add the specified correspondences to the display.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:434
A structure representing RGB color information.
ColorHandler::ConstPtr ColorHandlerConstPtr
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:426
bool updateCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const pcl::Correspondences &correspondences, int nth, const std::string &id="correspondences", int viewport=0)
Update the specified correspondences to the display.
vtkSmartPointer< vtkIdTypeArray > cells
Internal cell array.
Definition: actor_map.h:97
Define methods or creating 3D shapes from parametric models.
virtual vtkSmartPointer< vtkDataArray > getColor() const
Obtain the actual color for the input dataset as a VTK data array.
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
bool addPolygonMesh(const pcl::PolygonMesh &polymesh, const std::string &id="polygon", int viewport=0)
Add a PolygonMesh object to screen.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:436
vtkSmartPointer< vtkLODActor > actor
The actor holding the data to render.
Definition: actor_map.h:79
bool contains(const std::string &id) const
Check if the cloud, shape, or coordinate with the given id was already added to this visualizer...
bool addArrow(const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id="arrow", int viewport=0)
Add a line arrow segment between two points, and display the distance between them.
bool updatePointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud")
Updates the XYZ data for an existing cloud object id on screen.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:442
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
Definition: point_cloud.h:431
bool isCapable() const
Check if this handler is capable of handling the input data or not.
Base handler class for PointCloud geometry.
std::vector< GeometryHandlerConstPtr > geometry_handlers
A vector of geometry handlers that can be used for rendering the data.
Definition: actor_map.h:82
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
virtual std::string getName() const =0
Abstract getName method.
GeometryHandler::ConstPtr GeometryHandlerConstPtr
bool addLine(const P1 &pt1, const P2 &pt2, const std::string &id="line", int viewport=0)
Add a line segment from two points.
bool addText3D(const std::string &text, const PointT &position, double textScale=1.0, double r=1.0, double g=1.0, double b=1.0, const std::string &id="", int viewport=0)
Add a 3d text to the scene.
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual std::string getName() const =0
Abstract getName method.
bool updateSphere(const PointT &center, double radius, double r, double g, double b, const std::string &id="sphere")
Update an existing sphere shape from a point and a radius.
vtkSmartPointer< vtkMatrix4x4 > viewpoint_transformation_
The viewpoint transformation matrix.
Definition: actor_map.h:94
bool addPointCloudNormals(const typename pcl::PointCloud< PointNT >::ConstPtr &cloud, int level=100, float scale=0.02f, const std::string &id="cloud", int viewport=0)
Add the estimated surface normals of a Point Cloud to screen.
PCL_EXPORTS vtkSmartPointer< vtkDataSet > createLine(const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
Create a line shape from two points.
bool updatePolygonMesh(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::vector< pcl::Vertices > &vertices, const std::string &id="polygon")
Update a PolygonMesh object on screen.
PCL_EXPORTS void print_error(const char *format,...)
Print an error message on stream with colors.
size_t size() const
Definition: point_cloud.h:461
bool empty() const
Definition: point_cloud.h:463