Point Cloud Library (PCL)  1.11.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 
65 #include <pcl/visualization/common/shapes.h>
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
76 pcl::visualization::PCLVisualizer::addPointCloud (
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
87 pcl::visualization::PCLVisualizer::addPointCloud (
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
109 pcl::visualization::PCLVisualizer::addPointCloud (
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
130 pcl::visualization::PCLVisualizer::addPointCloud (
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
152 pcl::visualization::PCLVisualizer::addPointCloud (
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
173 pcl::visualization::PCLVisualizer::addPointCloud (
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
194 pcl::visualization::PCLVisualizer::addPointCloud (
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,
217  vtkSmartPointer<vtkPolyData> &polydata,
218  vtkSmartPointer<vtkIdTypeArray> &initcells)
219 {
220  vtkSmartPointer<vtkCellArray> vertices;
221  if (!polydata)
222  {
223  allocVtkPolyData (polydata);
224  vertices = vtkSmartPointer<vtkCellArray>::New ();
225  polydata->SetVerts (vertices);
226  }
227 
228  // Create the supporting structures
229  vertices = polydata->GetVerts ();
230  if (!vertices)
231  vertices = vtkSmartPointer<vtkCellArray>::New ();
232 
233  vtkIdType nr_points = cloud->size ();
234  // Create the point set
235  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
236  if (!points)
237  {
238  points = vtkSmartPointer<vtkPoints>::New ();
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)[i].x, &(*cloud)[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)[i].x) ||
261  !std::isfinite ((*cloud)[i].y) ||
262  !std::isfinite ((*cloud)[i].z))
263  continue;
264 
265  std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
266  j++;
267  ptr += 3;
268  }
269  nr_points = j;
270  points->SetNumberOfPoints (nr_points);
271  }
272 
273  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
274  updateCells (cells, initcells, nr_points);
275 
276  // Set the cells and the vertices
277  vertices->SetCells (nr_points, cells);
278 }
279 
280 //////////////////////////////////////////////////////////////////////////////////////////////
281 template <typename PointT> void
282 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
283  const pcl::visualization::PointCloudGeometryHandler<PointT> &geometry_handler,
284  vtkSmartPointer<vtkPolyData> &polydata,
285  vtkSmartPointer<vtkIdTypeArray> &initcells)
286 {
287  vtkSmartPointer<vtkCellArray> vertices;
288  if (!polydata)
289  {
290  allocVtkPolyData (polydata);
291  vertices = vtkSmartPointer<vtkCellArray>::New ();
292  polydata->SetVerts (vertices);
293  }
294 
295  // Use the handler to obtain the geometry
296  vtkSmartPointer<vtkPoints> points;
297  geometry_handler.getGeometry (points);
298  polydata->SetPoints (points);
299 
300  vtkIdType nr_points = points->GetNumberOfPoints ();
301 
302  // Create the supporting structures
303  vertices = polydata->GetVerts ();
304  if (!vertices)
305  vertices = vtkSmartPointer<vtkCellArray>::New ();
306 
307  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
308  updateCells (cells, initcells, nr_points);
309  // Set the cells and the vertices
310  vertices->SetCells (nr_points, cells);
311 }
312 
313 ////////////////////////////////////////////////////////////////////////////////////////////
314 template <typename PointT> bool
315 pcl::visualization::PCLVisualizer::addPolygon (
316  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
317  double r, double g, double b, const std::string &id, int viewport)
318 {
319  vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (cloud);
320  if (!data)
321  return (false);
322 
323  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
324  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
325  if (am_it != shape_actor_map_->end ())
326  {
327  vtkSmartPointer<vtkAppendPolyData> all_data = vtkSmartPointer<vtkAppendPolyData>::New ();
328 
329  // Add old data
330  all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
331 
332  // Add new data
333  vtkSmartPointer<vtkDataSetSurfaceFilter> surface_filter = vtkSmartPointer<vtkDataSetSurfaceFilter>::New ();
334  surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
335  vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
336  all_data->AddInputData (poly_data);
337 
338  // Create an Actor
339  vtkSmartPointer<vtkActor> actor;
340  createActorFromVTKDataSet (all_data->GetOutput (), actor);
341  actor->GetProperty ()->SetRepresentationToWireframe ();
342  actor->GetProperty ()->SetColor (r, g, b);
343  actor->GetMapper ()->ScalarVisibilityOff ();
344  removeActorFromRenderer (am_it->second, viewport);
345  addActorToRenderer (actor, viewport);
346 
347  // Save the pointer/ID pair to the global actor map
348  (*shape_actor_map_)[id] = actor;
349  }
350  else
351  {
352  // Create an Actor
353  vtkSmartPointer<vtkActor> actor;
354  createActorFromVTKDataSet (data, actor);
355  actor->GetProperty ()->SetRepresentationToWireframe ();
356  actor->GetProperty ()->SetColor (r, g, b);
357  actor->GetMapper ()->ScalarVisibilityOff ();
358  addActorToRenderer (actor, viewport);
359 
360  // Save the pointer/ID pair to the global actor map
361  (*shape_actor_map_)[id] = actor;
362  }
363 
364  return (true);
365 }
366 
367 ////////////////////////////////////////////////////////////////////////////////////////////
368 template <typename PointT> bool
369 pcl::visualization::PCLVisualizer::addPolygon (
370  const pcl::PlanarPolygon<PointT> &polygon,
371  double r, double g, double b, const std::string &id, int viewport)
372 {
373  vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (polygon);
374  if (!data)
375  return (false);
376 
377  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
378  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
379  if (am_it != shape_actor_map_->end ())
380  {
381  vtkSmartPointer<vtkAppendPolyData> all_data = vtkSmartPointer<vtkAppendPolyData>::New ();
382 
383  // Add old data
384  all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
385 
386  // Add new data
387  vtkSmartPointer<vtkDataSetSurfaceFilter> surface_filter = vtkSmartPointer<vtkDataSetSurfaceFilter>::New ();
388  surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
389  vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
390  all_data->AddInputData (poly_data);
391 
392  // Create an Actor
393  vtkSmartPointer<vtkActor> actor;
394  createActorFromVTKDataSet (all_data->GetOutput (), actor);
395  actor->GetProperty ()->SetRepresentationToWireframe ();
396  actor->GetProperty ()->SetColor (r, g, b);
397  actor->GetMapper ()->ScalarVisibilityOn ();
398  actor->GetProperty ()->BackfaceCullingOff ();
399  removeActorFromRenderer (am_it->second, viewport);
400  addActorToRenderer (actor, viewport);
401 
402  // Save the pointer/ID pair to the global actor map
403  (*shape_actor_map_)[id] = actor;
404  }
405  else
406  {
407  // Create an Actor
408  vtkSmartPointer<vtkActor> actor;
409  createActorFromVTKDataSet (data, actor);
410  actor->GetProperty ()->SetRepresentationToWireframe ();
411  actor->GetProperty ()->SetColor (r, g, b);
412  actor->GetMapper ()->ScalarVisibilityOn ();
413  actor->GetProperty ()->BackfaceCullingOff ();
414  addActorToRenderer (actor, viewport);
415 
416  // Save the pointer/ID pair to the global actor map
417  (*shape_actor_map_)[id] = actor;
418  }
419  return (true);
420 }
421 
422 ////////////////////////////////////////////////////////////////////////////////////////////
423 template <typename PointT> bool
424 pcl::visualization::PCLVisualizer::addPolygon (
425  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
426  const std::string &id, int viewport)
427 {
428  return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5, id, viewport));
429 }
430 
431 ////////////////////////////////////////////////////////////////////////////////////////////
432 template <typename P1, typename P2> bool
433 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
434 {
435  if (contains (id))
436  {
437  PCL_WARN ("[addLine] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
438  return (false);
439  }
440 
441  vtkSmartPointer<vtkDataSet> data = createLine (pt1.getVector4fMap (), pt2.getVector4fMap ());
442 
443  // Create an Actor
444  vtkSmartPointer<vtkLODActor> actor;
445  createActorFromVTKDataSet (data, actor);
446  actor->GetProperty ()->SetRepresentationToWireframe ();
447  actor->GetProperty ()->SetColor (r, g, b);
448  actor->GetMapper ()->ScalarVisibilityOff ();
449  addActorToRenderer (actor, viewport);
450 
451  // Save the pointer/ID pair to the global actor map
452  (*shape_actor_map_)[id] = actor;
453  return (true);
454 }
455 
456 ////////////////////////////////////////////////////////////////////////////////////////////
457 template <typename P1, typename P2> bool
458 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
459 {
460  if (contains (id))
461  {
462  PCL_WARN ("[addArrow] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
463  return (false);
464  }
465 
466  // Create an Actor
467  vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
468  leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
469  leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
470  leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
471  leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
472  leader->SetArrowStyleToFilled ();
473  leader->AutoLabelOn ();
474 
475  leader->GetProperty ()->SetColor (r, g, b);
476  addActorToRenderer (leader, viewport);
477 
478  // Save the pointer/ID pair to the global actor map
479  (*shape_actor_map_)[id] = leader;
480  return (true);
481 }
482 
483 ////////////////////////////////////////////////////////////////////////////////////////////
484 template <typename P1, typename P2> bool
485 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)
486 {
487  if (contains (id))
488  {
489  PCL_WARN ("[addArrow] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
490  return (false);
491  }
492 
493  // Create an Actor
494  vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
495  leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
496  leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
497  leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
498  leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
499  leader->SetArrowStyleToFilled ();
500  leader->SetArrowPlacementToPoint1 ();
501  if (display_length)
502  leader->AutoLabelOn ();
503  else
504  leader->AutoLabelOff ();
505 
506  leader->GetProperty ()->SetColor (r, g, b);
507  addActorToRenderer (leader, viewport);
508 
509  // Save the pointer/ID pair to the global actor map
510  (*shape_actor_map_)[id] = leader;
511  return (true);
512 }
513 ////////////////////////////////////////////////////////////////////////////////////////////
514 template <typename P1, typename P2> bool
515 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2,
516  double r_line, double g_line, double b_line,
517  double r_text, double g_text, double b_text,
518  const std::string &id, int viewport)
519 {
520  if (contains (id))
521  {
522  PCL_WARN ("[addArrow] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
523  return (false);
524  }
525 
526  // Create an Actor
527  vtkSmartPointer<vtkLeaderActor2D> leader = vtkSmartPointer<vtkLeaderActor2D>::New ();
528  leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
529  leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
530  leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
531  leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
532  leader->SetArrowStyleToFilled ();
533  leader->AutoLabelOn ();
534 
535  leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
536 
537  leader->GetProperty ()->SetColor (r_line, g_line, b_line);
538  addActorToRenderer (leader, viewport);
539 
540  // Save the pointer/ID pair to the global actor map
541  (*shape_actor_map_)[id] = leader;
542  return (true);
543 }
544 
545 ////////////////////////////////////////////////////////////////////////////////////////////
546 template <typename P1, typename P2> bool
547 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, const std::string &id, int viewport)
548 {
549  return (!addLine (pt1, pt2, 0.5, 0.5, 0.5, id, viewport));
550 }
551 
552 ////////////////////////////////////////////////////////////////////////////////////////////
553 template <typename PointT> bool
554 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id, int viewport)
555 {
556  if (contains (id))
557  {
558  PCL_WARN ("[addSphere] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
559  return (false);
560  }
561 
562  vtkSmartPointer<vtkSphereSource> data = vtkSmartPointer<vtkSphereSource>::New ();
563  data->SetRadius (radius);
564  data->SetCenter (double (center.x), double (center.y), double (center.z));
565  data->SetPhiResolution (10);
566  data->SetThetaResolution (10);
567  data->LatLongTessellationOff ();
568  data->Update ();
569 
570  // Setup actor and mapper
571  vtkSmartPointer <vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
572  mapper->SetInputConnection (data->GetOutputPort ());
573 
574  // Create an Actor
575  vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
576  actor->SetMapper (mapper);
577  //createActorFromVTKDataSet (data, actor);
578  actor->GetProperty ()->SetRepresentationToSurface ();
579  actor->GetProperty ()->SetInterpolationToFlat ();
580  actor->GetProperty ()->SetColor (r, g, b);
581 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
582  actor->GetMapper ()->ImmediateModeRenderingOn ();
583 #endif
584  actor->GetMapper ()->StaticOn ();
585  actor->GetMapper ()->ScalarVisibilityOff ();
586  actor->GetMapper ()->Update ();
587  addActorToRenderer (actor, viewport);
588 
589  // Save the pointer/ID pair to the global actor map
590  (*shape_actor_map_)[id] = actor;
591  return (true);
592 }
593 
594 ////////////////////////////////////////////////////////////////////////////////////////////
595 template <typename PointT> bool
596 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, const std::string &id, int viewport)
597 {
598  return (addSphere (center, radius, 0.5, 0.5, 0.5, id, viewport));
599 }
600 
601 ////////////////////////////////////////////////////////////////////////////////////////////
602 template<typename PointT> bool
603 pcl::visualization::PCLVisualizer::updateSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id)
604 {
605  if (!contains (id))
606  {
607  return (false);
608  }
609 
610  //////////////////////////////////////////////////////////////////////////
611  // Get the actor pointer
612  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
613  vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
614  if (!actor)
615  return (false);
616  vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
617  vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
618  if (!src)
619  return (false);
620 
621  src->SetCenter (double (center.x), double (center.y), double (center.z));
622  src->SetRadius (radius);
623  src->Update ();
624  actor->GetProperty ()->SetColor (r, g, b);
625  actor->Modified ();
626 
627  return (true);
628 }
629 
630 //////////////////////////////////////////////////
631 template <typename PointT> bool
632 pcl::visualization::PCLVisualizer::addText3D (
633  const std::string &text,
634  const PointT& position,
635  double textScale,
636  double r,
637  double g,
638  double b,
639  const std::string &id,
640  int viewport)
641 {
642  std::string tid;
643  if (id.empty ())
644  tid = text;
645  else
646  tid = id;
647 
648  if (viewport < 0)
649  return false;
650 
651  // If there is no custom viewport and the viewport number is not 0, exit
652  if (rens_->GetNumberOfItems () <= viewport)
653  {
654  PCL_ERROR ("[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
655  viewport,
656  tid.c_str ());
657  return false;
658  }
659 
660  // check all or an individual viewport for a similar id
661  rens_->InitTraversal ();
662  for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
663  {
664  const std::string uid = tid + std::string (i, '*');
665  if (contains (uid))
666  {
667  PCL_ERROR ( "[addText3D] The id <%s> already exists in viewport [%d]! "
668  "Please choose a different id and retry.\n",
669  tid.c_str (),
670  i);
671  return false;
672  }
673 
674  if (viewport > 0)
675  break;
676  }
677 
678  vtkSmartPointer<vtkVectorText> textSource = vtkSmartPointer<vtkVectorText>::New ();
679  textSource->SetText (text.c_str());
680  textSource->Update ();
681 
682  vtkSmartPointer<vtkPolyDataMapper> textMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
683  textMapper->SetInputConnection (textSource->GetOutputPort ());
684 
685  // Since each follower may follow a different camera, we need different followers
686  rens_->InitTraversal ();
687  vtkRenderer* renderer;
688  int i = 0;
689  while ((renderer = rens_->GetNextItem ()))
690  {
691  // Should we add the actor to all renderers or just to i-nth renderer?
692  if (viewport == 0 || viewport == i)
693  {
694  vtkSmartPointer<vtkFollower> textActor = vtkSmartPointer<vtkFollower>::New ();
695  textActor->SetMapper (textMapper);
696  textActor->SetPosition (position.x, position.y, position.z);
697  textActor->SetScale (textScale);
698  textActor->GetProperty ()->SetColor (r, g, b);
699  textActor->SetCamera (renderer->GetActiveCamera ());
700 
701  renderer->AddActor (textActor);
702  renderer->Render ();
703 
704  // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers
705  // for multiple viewport
706  const std::string uid = tid + std::string (i, '*');
707  (*shape_actor_map_)[uid] = textActor;
708  }
709 
710  ++i;
711  }
712 
713  return (true);
714 }
715 
716 //////////////////////////////////////////////////
717 template <typename PointT> bool
718 pcl::visualization::PCLVisualizer::addText3D (
719  const std::string &text,
720  const PointT& position,
721  double orientation[3],
722  double textScale,
723  double r,
724  double g,
725  double b,
726  const std::string &id,
727  int viewport)
728 {
729  std::string tid;
730  if (id.empty ())
731  tid = text;
732  else
733  tid = id;
734 
735  if (viewport < 0)
736  return false;
737 
738  // If there is no custom viewport and the viewport number is not 0, exit
739  if (rens_->GetNumberOfItems () <= viewport)
740  {
741  PCL_ERROR ("[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
742  viewport,
743  tid.c_str ());
744  return false;
745  }
746 
747  // check all or an individual viewport for a similar id
748  rens_->InitTraversal ();
749  for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
750  {
751  const std::string uid = tid + std::string (i, '*');
752  if (contains (uid))
753  {
754  PCL_ERROR ( "[addText3D] The id <%s> already exists in viewport [%d]! "
755  "Please choose a different id and retry.\n",
756  tid.c_str (),
757  i);
758  return false;
759  }
760 
761  if (viewport > 0)
762  break;
763  }
764 
765  vtkSmartPointer<vtkVectorText> textSource = vtkSmartPointer<vtkVectorText>::New ();
766  textSource->SetText (text.c_str());
767  textSource->Update ();
768 
769  vtkSmartPointer<vtkPolyDataMapper> textMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
770  textMapper->SetInputConnection (textSource->GetOutputPort ());
771 
772  vtkSmartPointer<vtkActor> textActor = vtkSmartPointer<vtkActor>::New ();
773  textActor->SetMapper (textMapper);
774  textActor->SetPosition (position.x, position.y, position.z);
775  textActor->SetScale (textScale);
776  textActor->GetProperty ()->SetColor (r, g, b);
777  textActor->SetOrientation (orientation);
778 
779  // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers
780  rens_->InitTraversal ();
781  int i = 0;
782  for ( vtkRenderer* renderer = rens_->GetNextItem ();
783  renderer;
784  renderer = rens_->GetNextItem (), ++i)
785  {
786  if (viewport == 0 || viewport == i)
787  {
788  renderer->AddActor (textActor);
789  const std::string uid = tid + std::string (i, '*');
790  (*shape_actor_map_)[uid] = textActor;
791  }
792  }
793 
794  return (true);
795 }
796 
797 //////////////////////////////////////////////////////////////////////////////////////////////
798 template <typename PointNT> bool
799 pcl::visualization::PCLVisualizer::addPointCloudNormals (
800  const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
801  int level, float scale, const std::string &id, int viewport)
802 {
803  return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale, id, viewport));
804 }
805 
806 //////////////////////////////////////////////////////////////////////////////////////////////
807 template <typename PointT, typename PointNT> bool
808 pcl::visualization::PCLVisualizer::addPointCloudNormals (
809  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
810  const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
811  int level, float scale,
812  const std::string &id, int viewport)
813 {
814  if (normals->size () != cloud->size ())
815  {
816  PCL_ERROR ("[addPointCloudNormals] The number of points differs from the number of normals!\n");
817  return (false);
818  }
819 
820  if (normals->empty ())
821  {
822  PCL_WARN ("[addPointCloudNormals] An empty normal cloud is given! Nothing to display.\n");
823  return (false);
824  }
825 
826  if (contains (id))
827  {
828  PCL_WARN ("[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
829  return (false);
830  }
831 
832  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
833  vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
834 
835  points->SetDataTypeToFloat ();
836  vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
837  data->SetNumberOfComponents (3);
838 
839 
840  vtkIdType nr_normals = 0;
841  float* pts = nullptr;
842 
843  // If the cloud is organized, then distribute the normal step in both directions
844  if (cloud->isOrganized () && normals->isOrganized ())
845  {
846  vtkIdType point_step = static_cast<vtkIdType> (sqrt (double (level)));
847  nr_normals = (static_cast<vtkIdType> ((cloud->width - 1)/ point_step) + 1) *
848  (static_cast<vtkIdType> ((cloud->height - 1) / point_step) + 1);
849  pts = new float[2 * nr_normals * 3];
850 
851  vtkIdType cell_count = 0;
852  for (vtkIdType y = 0; y < normals->height; y += point_step)
853  for (vtkIdType x = 0; x < normals->width; x += point_step)
854  {
855  PointT p = (*cloud)(x, y);
856  p.x += (*normals)(x, y).normal[0] * scale;
857  p.y += (*normals)(x, y).normal[1] * scale;
858  p.z += (*normals)(x, y).normal[2] * scale;
859 
860  pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
861  pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
862  pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
863  pts[2 * cell_count * 3 + 3] = p.x;
864  pts[2 * cell_count * 3 + 4] = p.y;
865  pts[2 * cell_count * 3 + 5] = p.z;
866 
867  lines->InsertNextCell (2);
868  lines->InsertCellPoint (2 * cell_count);
869  lines->InsertCellPoint (2 * cell_count + 1);
870  cell_count ++;
871  }
872  }
873  else
874  {
875  nr_normals = (cloud->size () - 1) / level + 1 ;
876  pts = new float[2 * nr_normals * 3];
877 
878  for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
879  {
880  PointT p = (*cloud)[i];
881  p.x += (*normals)[i].normal[0] * scale;
882  p.y += (*normals)[i].normal[1] * scale;
883  p.z += (*normals)[i].normal[2] * scale;
884 
885  pts[2 * j * 3 + 0] = (*cloud)[i].x;
886  pts[2 * j * 3 + 1] = (*cloud)[i].y;
887  pts[2 * j * 3 + 2] = (*cloud)[i].z;
888  pts[2 * j * 3 + 3] = p.x;
889  pts[2 * j * 3 + 4] = p.y;
890  pts[2 * j * 3 + 5] = p.z;
891 
892  lines->InsertNextCell (2);
893  lines->InsertCellPoint (2 * j);
894  lines->InsertCellPoint (2 * j + 1);
895  }
896  }
897 
898  data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
899  points->SetData (data);
900 
901  vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
902  polyData->SetPoints (points);
903  polyData->SetLines (lines);
904 
905  vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
906  mapper->SetInputData (polyData);
907  mapper->SetColorModeToMapScalars();
908  mapper->SetScalarModeToUsePointData();
909 
910  // create actor
911  vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
912  actor->SetMapper (mapper);
913 
914  // Use cloud view point info
915  vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
916  convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
917  actor->SetUserMatrix (transformation);
918 
919  // Add it to all renderers
920  addActorToRenderer (actor, viewport);
921 
922  // Save the pointer/ID pair to the global actor map
923  (*cloud_actor_map_)[id].actor = actor;
924  return (true);
925 }
926 
927 //////////////////////////////////////////////////////////////////////////////////////////////
928 template <typename PointNT> bool
929 pcl::visualization::PCLVisualizer::addPointCloudPrincipalCurvatures (
930  const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
931  const pcl::PointCloud<pcl::PrincipalCurvatures>::ConstPtr &pcs,
932  int level, float scale,
933  const std::string &id, int viewport)
934 {
935  return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale, id, viewport));
936 }
937 
938 //////////////////////////////////////////////////////////////////////////////////////////////
939 template <typename PointT, typename PointNT> bool
940 pcl::visualization::PCLVisualizer::addPointCloudPrincipalCurvatures (
941  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
942  const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
943  const pcl::PointCloud<pcl::PrincipalCurvatures>::ConstPtr &pcs,
944  int level, float scale,
945  const std::string &id, int viewport)
946 {
947  if (pcs->size () != cloud->size () || normals->size () != cloud->size ())
948  {
949  pcl::console::print_error ("[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
950  return (false);
951  }
952 
953  if (contains (id))
954  {
955  PCL_WARN ("[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
956  return (false);
957  }
958 
959  vtkSmartPointer<vtkAppendPolyData> polydata_1 = vtkSmartPointer<vtkAppendPolyData>::New ();
960  vtkSmartPointer<vtkAppendPolyData> polydata_2 = vtkSmartPointer<vtkAppendPolyData>::New ();
961 
962  // Setup two colors - one for each line
963  unsigned char green[3] = {0, 255, 0};
964  unsigned char blue[3] = {0, 0, 255};
965 
966  // Setup the colors array
967  vtkSmartPointer<vtkUnsignedCharArray> line_1_colors =vtkSmartPointer<vtkUnsignedCharArray>::New ();
968  line_1_colors->SetNumberOfComponents (3);
969  line_1_colors->SetName ("Colors");
970  vtkSmartPointer<vtkUnsignedCharArray> line_2_colors =vtkSmartPointer<vtkUnsignedCharArray>::New ();
971  line_2_colors->SetNumberOfComponents (3);
972  line_2_colors->SetName ("Colors");
973 
974  // Create the first sets of lines
975  for (std::size_t i = 0; i < cloud->size (); i+=level)
976  {
977  PointT p = (*cloud)[i];
978  p.x += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[0]) * scale;
979  p.y += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[1]) * scale;
980  p.z += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[2]) * scale;
981 
982  vtkSmartPointer<vtkLineSource> line_1 = vtkSmartPointer<vtkLineSource>::New ();
983  line_1->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
984  line_1->SetPoint2 (p.x, p.y, p.z);
985  line_1->Update ();
986  polydata_1->AddInputData (line_1->GetOutput ());
987  line_1_colors->InsertNextTupleValue (green);
988  }
989  polydata_1->Update ();
990  vtkSmartPointer<vtkPolyData> line_1_data = polydata_1->GetOutput ();
991  line_1_data->GetCellData ()->SetScalars (line_1_colors);
992 
993  // Create the second sets of lines
994  for (std::size_t i = 0; i < cloud->size (); i += level)
995  {
996  Eigen::Vector3f pc ((*pcs)[i].principal_curvature[0],
997  (*pcs)[i].principal_curvature[1],
998  (*pcs)[i].principal_curvature[2]);
999  Eigen::Vector3f normal ((*normals)[i].normal[0],
1000  (*normals)[i].normal[1],
1001  (*normals)[i].normal[2]);
1002  Eigen::Vector3f pc_c = pc.cross (normal);
1003 
1004  PointT p = (*cloud)[i];
1005  p.x += ((*pcs)[i].pc2 * pc_c[0]) * scale;
1006  p.y += ((*pcs)[i].pc2 * pc_c[1]) * scale;
1007  p.z += ((*pcs)[i].pc2 * pc_c[2]) * scale;
1008 
1009  vtkSmartPointer<vtkLineSource> line_2 = vtkSmartPointer<vtkLineSource>::New ();
1010  line_2->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1011  line_2->SetPoint2 (p.x, p.y, p.z);
1012  line_2->Update ();
1013  polydata_2->AddInputData (line_2->GetOutput ());
1014 
1015  line_2_colors->InsertNextTupleValue (blue);
1016  }
1017  polydata_2->Update ();
1018  vtkSmartPointer<vtkPolyData> line_2_data = polydata_2->GetOutput ();
1019  line_2_data->GetCellData ()->SetScalars (line_2_colors);
1020 
1021  // Assemble the two sets of lines
1022  vtkSmartPointer<vtkAppendPolyData> alldata = vtkSmartPointer<vtkAppendPolyData>::New ();
1023  alldata->AddInputData (line_1_data);
1024  alldata->AddInputData (line_2_data);
1025 
1026  // Create an Actor
1027  vtkSmartPointer<vtkLODActor> actor;
1028  createActorFromVTKDataSet (alldata->GetOutput (), actor);
1029  actor->GetMapper ()->SetScalarModeToUseCellData ();
1030 
1031  // Add it to all renderers
1032  addActorToRenderer (actor, viewport);
1033 
1034  // Save the pointer/ID pair to the global actor map
1035  CloudActor act;
1036  act.actor = actor;
1037  (*cloud_actor_map_)[id] = act;
1038  return (true);
1039 }
1040 
1041 //////////////////////////////////////////////////////////////////////////////////////////////
1042 template <typename PointT, typename GradientT> bool
1043 pcl::visualization::PCLVisualizer::addPointCloudIntensityGradients (
1044  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1045  const typename pcl::PointCloud<GradientT>::ConstPtr &gradients,
1046  int level, double scale,
1047  const std::string &id, int viewport)
1048 {
1049  if (gradients->size () != cloud->size ())
1050  {
1051  PCL_ERROR ("[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1052  return (false);
1053  }
1054  if (contains (id))
1055  {
1056  PCL_WARN ("[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1057  return (false);
1058  }
1059 
1060  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
1061  vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
1062 
1063  points->SetDataTypeToFloat ();
1064  vtkSmartPointer<vtkFloatArray> data = vtkSmartPointer<vtkFloatArray>::New ();
1065  data->SetNumberOfComponents (3);
1066 
1067  vtkIdType nr_gradients = (cloud->size () - 1) / level + 1 ;
1068  float* pts = new float[2 * nr_gradients * 3];
1069 
1070  for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1071  {
1072  PointT p = (*cloud)[i];
1073  p.x += (*gradients)[i].gradient[0] * scale;
1074  p.y += (*gradients)[i].gradient[1] * scale;
1075  p.z += (*gradients)[i].gradient[2] * scale;
1076 
1077  pts[2 * j * 3 + 0] = (*cloud)[i].x;
1078  pts[2 * j * 3 + 1] = (*cloud)[i].y;
1079  pts[2 * j * 3 + 2] = (*cloud)[i].z;
1080  pts[2 * j * 3 + 3] = p.x;
1081  pts[2 * j * 3 + 4] = p.y;
1082  pts[2 * j * 3 + 5] = p.z;
1083 
1084  lines->InsertNextCell(2);
1085  lines->InsertCellPoint(2*j);
1086  lines->InsertCellPoint(2*j+1);
1087  }
1088 
1089  data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1090  points->SetData (data);
1091 
1092  vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
1093  polyData->SetPoints(points);
1094  polyData->SetLines(lines);
1095 
1096  vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
1097  mapper->SetInputData (polyData);
1098  mapper->SetColorModeToMapScalars();
1099  mapper->SetScalarModeToUsePointData();
1100 
1101  // create actor
1102  vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
1103  actor->SetMapper (mapper);
1104 
1105  // Add it to all renderers
1106  addActorToRenderer (actor, viewport);
1107 
1108  // Save the pointer/ID pair to the global actor map
1109  (*cloud_actor_map_)[id].actor = actor;
1110  return (true);
1111 }
1112 
1113 //////////////////////////////////////////////////////////////////////////////////////////////
1114 template <typename PointT> bool
1115 pcl::visualization::PCLVisualizer::addCorrespondences (
1116  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1117  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1118  const std::vector<int> &correspondences,
1119  const std::string &id,
1120  int viewport)
1121 {
1122  pcl::Correspondences corrs;
1123  corrs.resize (correspondences.size ());
1124 
1125  std::size_t index = 0;
1126  for (auto &corr : corrs)
1127  {
1128  corr.index_query = index;
1129  corr.index_match = correspondences[index];
1130  index++;
1131  }
1132 
1133  return (addCorrespondences<PointT> (source_points, target_points, corrs, id, viewport));
1134 }
1135 
1136 //////////////////////////////////////////////////////////////////////////////////////////////
1137 template <typename PointT> bool
1138 pcl::visualization::PCLVisualizer::addCorrespondences (
1139  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1140  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1141  const pcl::Correspondences &correspondences,
1142  int nth,
1143  const std::string &id,
1144  int viewport,
1145  bool overwrite)
1146 {
1147  if (correspondences.empty ())
1148  {
1149  PCL_DEBUG ("[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1150  return (false);
1151  }
1152 
1153  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1154  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
1155  if (am_it != shape_actor_map_->end () && !overwrite)
1156  {
1157  PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1158  return (false);
1159  }
1160  if (am_it == shape_actor_map_->end () && overwrite)
1161  {
1162  overwrite = false; // Correspondences doesn't exist, add them instead of updating them
1163  }
1164 
1165  int n_corr = int (correspondences.size () / nth);
1166  vtkSmartPointer<vtkPolyData> line_data = vtkSmartPointer<vtkPolyData>::New ();
1167 
1168  // Prepare colors
1169  vtkSmartPointer<vtkUnsignedCharArray> line_colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
1170  line_colors->SetNumberOfComponents (3);
1171  line_colors->SetName ("Colors");
1172  line_colors->SetNumberOfTuples (n_corr);
1173 
1174  // Prepare coordinates
1175  vtkSmartPointer<vtkPoints> line_points = vtkSmartPointer<vtkPoints>::New ();
1176  line_points->SetNumberOfPoints (2 * n_corr);
1177 
1178  vtkSmartPointer<vtkIdTypeArray> line_cells_id = vtkSmartPointer<vtkIdTypeArray>::New ();
1179  line_cells_id->SetNumberOfComponents (3);
1180  line_cells_id->SetNumberOfTuples (n_corr);
1181  vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1182  vtkSmartPointer<vtkCellArray> line_cells = vtkSmartPointer<vtkCellArray>::New ();
1183 
1184  vtkSmartPointer<vtkFloatArray> line_tcoords = vtkSmartPointer<vtkFloatArray>::New ();
1185  line_tcoords->SetNumberOfComponents (1);
1186  line_tcoords->SetNumberOfTuples (n_corr * 2);
1187  line_tcoords->SetName ("Texture Coordinates");
1188 
1189  double tc[3] = {0.0, 0.0, 0.0};
1190 
1191  Eigen::Affine3f source_transformation;
1192  source_transformation.linear () = source_points->sensor_orientation_.matrix ();
1193  source_transformation.translation () = source_points->sensor_origin_.head (3);
1194  Eigen::Affine3f target_transformation;
1195  target_transformation.linear () = target_points->sensor_orientation_.matrix ();
1196  target_transformation.translation () = target_points->sensor_origin_.head (3);
1197 
1198  int j = 0;
1199  // Draw lines between the best corresponding points
1200  for (std::size_t i = 0; i < correspondences.size (); i += nth, ++j)
1201  {
1202  if (correspondences[i].index_match == -1)
1203  {
1204  PCL_WARN ("[addCorrespondences] No valid index_match for correspondence %d\n", i);
1205  continue;
1206  }
1207 
1208  PointT p_src ((*source_points)[correspondences[i].index_query]);
1209  PointT p_tgt ((*target_points)[correspondences[i].index_match]);
1210 
1211  p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1212  p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1213 
1214  int id1 = j * 2 + 0, id2 = j * 2 + 1;
1215  // Set the points
1216  line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1217  line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1218  // Set the cell ID
1219  *line_cell_id++ = 2;
1220  *line_cell_id++ = id1;
1221  *line_cell_id++ = id2;
1222  // Set the texture coords
1223  tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1224  tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1225 
1226  float rgb[3];
1227  rgb[0] = vtkMath::Random (32, 255); // min / max
1228  rgb[1] = vtkMath::Random (32, 255);
1229  rgb[2] = vtkMath::Random (32, 255);
1230  line_colors->InsertTuple (i, rgb);
1231  }
1232  line_colors->SetNumberOfTuples (j);
1233  line_cells_id->SetNumberOfTuples (j);
1234  line_cells->SetCells (n_corr, line_cells_id);
1235  line_points->SetNumberOfPoints (j*2);
1236  line_tcoords->SetNumberOfTuples (j*2);
1237 
1238  // Fill in the lines
1239  line_data->SetPoints (line_points);
1240  line_data->SetLines (line_cells);
1241  line_data->GetPointData ()->SetTCoords (line_tcoords);
1242  line_data->GetCellData ()->SetScalars (line_colors);
1243 
1244  // Create an Actor
1245  if (!overwrite)
1246  {
1247  vtkSmartPointer<vtkLODActor> actor;
1248  createActorFromVTKDataSet (line_data, actor);
1249  actor->GetProperty ()->SetRepresentationToWireframe ();
1250  actor->GetProperty ()->SetOpacity (0.5);
1251  addActorToRenderer (actor, viewport);
1252 
1253  // Save the pointer/ID pair to the global actor map
1254  (*shape_actor_map_)[id] = actor;
1255  }
1256  else
1257  {
1258  vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
1259  if (!actor)
1260  return (false);
1261  // Update the mapper
1262  reinterpret_cast<vtkPolyDataMapper*> (actor->GetMapper ())->SetInputData (line_data);
1263  }
1264 
1265  return (true);
1266 }
1267 
1268 //////////////////////////////////////////////////////////////////////////////////////////////
1269 template <typename PointT> bool
1270 pcl::visualization::PCLVisualizer::updateCorrespondences (
1271  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1272  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1273  const pcl::Correspondences &correspondences,
1274  int nth,
1275  const std::string &id,
1276  int viewport)
1277 {
1278  return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth, id, viewport, true));
1279 }
1280 
1281 //////////////////////////////////////////////////////////////////////////////////////////////
1282 template <typename PointT> bool
1283 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1284  const PointCloudGeometryHandler<PointT> &geometry_handler,
1285  const PointCloudColorHandler<PointT> &color_handler,
1286  const std::string &id,
1287  int viewport,
1288  const Eigen::Vector4f& sensor_origin,
1289  const Eigen::Quaternion<float>& sensor_orientation)
1290 {
1291  if (!geometry_handler.isCapable ())
1292  {
1293  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
1294  return (false);
1295  }
1296 
1297  if (!color_handler.isCapable ())
1298  {
1299  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
1300  return (false);
1301  }
1302 
1303  vtkSmartPointer<vtkPolyData> polydata;
1304  vtkSmartPointer<vtkIdTypeArray> initcells;
1305  // Convert the PointCloud to VTK PolyData
1306  convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1307  // use the given geometry handler
1308 
1309  // Get the colors from the handler
1310  bool has_colors = false;
1311  double minmax[2];
1312  if (auto scalars = color_handler.getColor ())
1313  {
1314  polydata->GetPointData ()->SetScalars (scalars);
1315  scalars->GetRange (minmax);
1316  has_colors = true;
1317  }
1318 
1319  // Create an Actor
1320  vtkSmartPointer<vtkLODActor> actor;
1321  createActorFromVTKDataSet (polydata, actor);
1322  if (has_colors)
1323  actor->GetMapper ()->SetScalarRange (minmax);
1324 
1325  // Add it to all renderers
1326  addActorToRenderer (actor, viewport);
1327 
1328  // Save the pointer/ID pair to the global actor map
1329  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1330  cloud_actor.actor = actor;
1331  cloud_actor.cells = initcells;
1332 
1333  // Save the viewpoint transformation matrix to the global actor map
1334  vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
1335  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1336  cloud_actor.viewpoint_transformation_ = transformation;
1337  cloud_actor.actor->SetUserMatrix (transformation);
1338  cloud_actor.actor->Modified ();
1339 
1340  return (true);
1341 }
1342 
1343 //////////////////////////////////////////////////////////////////////////////////////////////
1344 template <typename PointT> bool
1345 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1346  const PointCloudGeometryHandler<PointT> &geometry_handler,
1347  const ColorHandlerConstPtr &color_handler,
1348  const std::string &id,
1349  int viewport,
1350  const Eigen::Vector4f& sensor_origin,
1351  const Eigen::Quaternion<float>& sensor_orientation)
1352 {
1353  if (!geometry_handler.isCapable ())
1354  {
1355  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
1356  return (false);
1357  }
1358 
1359  if (!color_handler->isCapable ())
1360  {
1361  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler->getName ().c_str ());
1362  return (false);
1363  }
1364 
1365  vtkSmartPointer<vtkPolyData> polydata;
1366  vtkSmartPointer<vtkIdTypeArray> initcells;
1367  // Convert the PointCloud to VTK PolyData
1368  convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1369  // use the given geometry handler
1370 
1371  // Get the colors from the handler
1372  bool has_colors = false;
1373  double minmax[2];
1374  if (auto scalars = color_handler->getColor ())
1375  {
1376  polydata->GetPointData ()->SetScalars (scalars);
1377  scalars->GetRange (minmax);
1378  has_colors = true;
1379  }
1380 
1381  // Create an Actor
1382  vtkSmartPointer<vtkLODActor> actor;
1383  createActorFromVTKDataSet (polydata, actor);
1384  if (has_colors)
1385  actor->GetMapper ()->SetScalarRange (minmax);
1386 
1387  // Add it to all renderers
1388  addActorToRenderer (actor, viewport);
1389 
1390  // Save the pointer/ID pair to the global actor map
1391  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1392  cloud_actor.actor = actor;
1393  cloud_actor.cells = initcells;
1394  cloud_actor.color_handlers.push_back (color_handler);
1395 
1396  // Save the viewpoint transformation matrix to the global actor map
1397  vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
1398  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1399  cloud_actor.viewpoint_transformation_ = transformation;
1400  cloud_actor.actor->SetUserMatrix (transformation);
1401  cloud_actor.actor->Modified ();
1402 
1403  return (true);
1404 }
1405 
1406 //////////////////////////////////////////////////////////////////////////////////////////////
1407 template <typename PointT> bool
1408 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1409  const GeometryHandlerConstPtr &geometry_handler,
1410  const PointCloudColorHandler<PointT> &color_handler,
1411  const std::string &id,
1412  int viewport,
1413  const Eigen::Vector4f& sensor_origin,
1414  const Eigen::Quaternion<float>& sensor_orientation)
1415 {
1416  if (!geometry_handler->isCapable ())
1417  {
1418  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler->getName ().c_str ());
1419  return (false);
1420  }
1421 
1422  if (!color_handler.isCapable ())
1423  {
1424  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
1425  return (false);
1426  }
1427 
1428  vtkSmartPointer<vtkPolyData> polydata;
1429  vtkSmartPointer<vtkIdTypeArray> initcells;
1430  // Convert the PointCloud to VTK PolyData
1431  convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1432  // use the given geometry handler
1433 
1434  // Get the colors from the handler
1435  bool has_colors = false;
1436  double minmax[2];
1437  if (auto scalars = color_handler.getColor ())
1438  {
1439  polydata->GetPointData ()->SetScalars (scalars);
1440  scalars->GetRange (minmax);
1441  has_colors = true;
1442  }
1443 
1444  // Create an Actor
1445  vtkSmartPointer<vtkLODActor> actor;
1446  createActorFromVTKDataSet (polydata, actor);
1447  if (has_colors)
1448  actor->GetMapper ()->SetScalarRange (minmax);
1449 
1450  // Add it to all renderers
1451  addActorToRenderer (actor, viewport);
1452 
1453  // Save the pointer/ID pair to the global actor map
1454  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1455  cloud_actor.actor = actor;
1456  cloud_actor.cells = initcells;
1457  cloud_actor.geometry_handlers.push_back (geometry_handler);
1458 
1459  // Save the viewpoint transformation matrix to the global actor map
1460  vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New ();
1461  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1462  cloud_actor.viewpoint_transformation_ = transformation;
1463  cloud_actor.actor->SetUserMatrix (transformation);
1464  cloud_actor.actor->Modified ();
1465 
1466  return (true);
1467 }
1468 
1469 //////////////////////////////////////////////////////////////////////////////////////////////
1470 template <typename PointT> bool
1471 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1472  const std::string &id)
1473 {
1474  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1475  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1476 
1477  if (am_it == cloud_actor_map_->end ())
1478  return (false);
1479 
1480  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1481  if (!polydata)
1482  return false;
1483  // Convert the PointCloud to VTK PolyData
1484  convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1485 
1486  // Set scalars to blank, since there is no way we can update them here.
1487  vtkSmartPointer<vtkDataArray> scalars;
1488  polydata->GetPointData ()->SetScalars (scalars);
1489  double minmax[2];
1490  minmax[0] = std::numeric_limits<double>::min ();
1491  minmax[1] = std::numeric_limits<double>::max ();
1492 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1493  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1494 #endif
1495  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1496 
1497  // Update the mapper
1498  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1499  return (true);
1500 }
1501 
1502 /////////////////////////////////////////////////////////////////////////////////////////////
1503 template <typename PointT> bool
1504 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &,
1505  const PointCloudGeometryHandler<PointT> &geometry_handler,
1506  const std::string &id)
1507 {
1508  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1509  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1510 
1511  if (am_it == cloud_actor_map_->end ())
1512  return (false);
1513 
1514  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1515  if (!polydata)
1516  return (false);
1517  // Convert the PointCloud to VTK PolyData
1518  convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1519 
1520  // Set scalars to blank, since there is no way we can update them here.
1521  vtkSmartPointer<vtkDataArray> scalars;
1522  polydata->GetPointData ()->SetScalars (scalars);
1523  double minmax[2];
1524  minmax[0] = std::numeric_limits<double>::min ();
1525  minmax[1] = std::numeric_limits<double>::max ();
1526 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1527  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1528 #endif
1529  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1530 
1531  // Update the mapper
1532  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1533  return (true);
1534 }
1535 
1536 
1537 /////////////////////////////////////////////////////////////////////////////////////////////
1538 template <typename PointT> bool
1539 pcl::visualization::PCLVisualizer::updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1540  const PointCloudColorHandler<PointT> &color_handler,
1541  const std::string &id)
1542 {
1543  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1544  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1545 
1546  if (am_it == cloud_actor_map_->end ())
1547  return (false);
1548 
1549  // Get the current poly data
1550  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1551  if (!polydata)
1552  return (false);
1553  vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
1554  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
1555  // Copy the new point array in
1556  vtkIdType nr_points = cloud->size ();
1557  points->SetNumberOfPoints (nr_points);
1558 
1559  // Get a pointer to the beginning of the data array
1560  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
1561 
1562  vtkIdType pts = 0;
1563  // If the dataset is dense (no NaNs)
1564  if (cloud->is_dense)
1565  {
1566  for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
1567  std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[pts]);
1568  }
1569  else
1570  {
1571  vtkIdType j = 0; // true point index
1572  for (vtkIdType i = 0; i < nr_points; ++i)
1573  {
1574  // Check if the point is invalid
1575  if (!isFinite ((*cloud)[i]))
1576  continue;
1577  std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[pts]);
1578  pts += 3;
1579  j++;
1580  }
1581  nr_points = j;
1582  points->SetNumberOfPoints (nr_points);
1583  }
1584 
1585  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
1586  updateCells (cells, am_it->second.cells, nr_points);
1587 
1588  // Set the cells and the vertices
1589  vertices->SetCells (nr_points, cells);
1590  // Set the cell count explicitly as the array doesn't get modified enough so the above method updates accordingly. See #4001 and #3452
1591  vertices->SetNumberOfCells(nr_points);
1592 
1593  // Get the colors from the handler
1594  bool has_colors = false;
1595  double minmax[2];
1596  if (auto scalars = color_handler.getColor ())
1597  {
1598  // Update the data
1599  polydata->GetPointData ()->SetScalars (scalars);
1600  scalars->GetRange (minmax);
1601  has_colors = true;
1602  }
1603 
1604 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1605  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1606 #endif
1607 
1608  if (has_colors)
1609  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1610 
1611  // Update the mapper
1612  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1613  return (true);
1614 }
1615 
1616 /////////////////////////////////////////////////////////////////////////////////////////////
1617 template <typename PointT> bool
1618 pcl::visualization::PCLVisualizer::addPolygonMesh (
1619  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1620  const std::vector<pcl::Vertices> &vertices,
1621  const std::string &id,
1622  int viewport)
1623 {
1624  if (vertices.empty () || cloud->points.empty ())
1625  return (false);
1626 
1627  if (contains (id))
1628  {
1629  PCL_WARN ("[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1630  return (false);
1631  }
1632 
1633  int rgb_idx = -1;
1634  std::vector<pcl::PCLPointField> fields;
1635  vtkSmartPointer<vtkUnsignedCharArray> colors;
1636  rgb_idx = pcl::getFieldIndex<PointT> ("rgb", fields);
1637  if (rgb_idx == -1)
1638  rgb_idx = pcl::getFieldIndex<PointT> ("rgba", fields);
1639  if (rgb_idx != -1)
1640  {
1641  colors = vtkSmartPointer<vtkUnsignedCharArray>::New ();
1642  colors->SetNumberOfComponents (3);
1643  colors->SetName ("Colors");
1644  std::uint32_t offset = fields[rgb_idx].offset;
1645  for (std::size_t i = 0; i < cloud->size (); ++i)
1646  {
1647  if (!isFinite ((*cloud)[i]))
1648  continue;
1649  const pcl::RGB* const rgb_data = reinterpret_cast<const pcl::RGB*>(reinterpret_cast<const char*> (&(*cloud)[i]) + offset);
1650  unsigned char color[3];
1651  color[0] = rgb_data->r;
1652  color[1] = rgb_data->g;
1653  color[2] = rgb_data->b;
1654  colors->InsertNextTupleValue (color);
1655  }
1656  }
1657 
1658  // Create points from polyMesh.cloud
1659  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
1660  vtkIdType nr_points = cloud->size ();
1661  points->SetNumberOfPoints (nr_points);
1662  vtkSmartPointer<vtkLODActor> actor;
1663 
1664  // Get a pointer to the beginning of the data array
1665  float *data = static_cast<vtkFloatArray*> (points->GetData ())->GetPointer (0);
1666 
1667  vtkIdType ptr = 0;
1668  std::vector<int> lookup;
1669  // If the dataset is dense (no NaNs)
1670  if (cloud->is_dense)
1671  {
1672  for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1673  std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1674  }
1675  else
1676  {
1677  lookup.resize (nr_points);
1678  vtkIdType j = 0; // true point index
1679  for (vtkIdType i = 0; i < nr_points; ++i)
1680  {
1681  // Check if the point is invalid
1682  if (!isFinite ((*cloud)[i]))
1683  continue;
1684 
1685  lookup[i] = static_cast<int> (j);
1686  std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1687  j++;
1688  ptr += 3;
1689  }
1690  nr_points = j;
1691  points->SetNumberOfPoints (nr_points);
1692  }
1693 
1694  // Get the maximum size of a polygon
1695  int max_size_of_polygon = -1;
1696  for (const auto &vertex : vertices)
1697  if (max_size_of_polygon < static_cast<int> (vertex.vertices.size ()))
1698  max_size_of_polygon = static_cast<int> (vertex.vertices.size ());
1699 
1700  if (vertices.size () > 1)
1701  {
1702  // Create polys from polyMesh.polygons
1703  vtkSmartPointer<vtkCellArray> cell_array = vtkSmartPointer<vtkCellArray>::New ();
1704  vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
1705  int idx = 0;
1706  if (!lookup.empty ())
1707  {
1708  for (std::size_t i = 0; i < vertices.size (); ++i, ++idx)
1709  {
1710  std::size_t n_points = vertices[i].vertices.size ();
1711  *cell++ = n_points;
1712  //cell_array->InsertNextCell (n_points);
1713  for (std::size_t j = 0; j < n_points; j++, ++idx)
1714  *cell++ = lookup[vertices[i].vertices[j]];
1715  //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
1716  }
1717  }
1718  else
1719  {
1720  for (std::size_t i = 0; i < vertices.size (); ++i, ++idx)
1721  {
1722  std::size_t n_points = vertices[i].vertices.size ();
1723  *cell++ = n_points;
1724  //cell_array->InsertNextCell (n_points);
1725  for (std::size_t j = 0; j < n_points; j++, ++idx)
1726  *cell++ = vertices[i].vertices[j];
1727  //cell_array->InsertCellPoint (vertices[i].vertices[j]);
1728  }
1729  }
1730  vtkSmartPointer<vtkPolyData> polydata;
1731  allocVtkPolyData (polydata);
1732  cell_array->GetData ()->SetNumberOfValues (idx);
1733  cell_array->Squeeze ();
1734  polydata->SetPolys (cell_array);
1735  polydata->SetPoints (points);
1736 
1737  if (colors)
1738  polydata->GetPointData ()->SetScalars (colors);
1739 
1740  createActorFromVTKDataSet (polydata, actor, false);
1741  }
1742  else
1743  {
1744  vtkSmartPointer<vtkPolygon> polygon = vtkSmartPointer<vtkPolygon>::New ();
1745  std::size_t n_points = vertices[0].vertices.size ();
1746  polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1747 
1748  if (!lookup.empty ())
1749  {
1750  for (std::size_t j = 0; j < (n_points - 1); ++j)
1751  polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1752  }
1753  else
1754  {
1755  for (std::size_t j = 0; j < (n_points - 1); ++j)
1756  polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1757  }
1758  vtkSmartPointer<vtkUnstructuredGrid> poly_grid;
1759  allocVtkUnstructuredGrid (poly_grid);
1760  poly_grid->Allocate (1, 1);
1761  poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1762  poly_grid->SetPoints (points);
1763  if (colors)
1764  poly_grid->GetPointData ()->SetScalars (colors);
1765 
1766  createActorFromVTKDataSet (poly_grid, actor, false);
1767  }
1768  addActorToRenderer (actor, viewport);
1769  actor->GetProperty ()->SetRepresentationToSurface ();
1770  // Backface culling renders the visualization slower, but guarantees that we see all triangles
1771  actor->GetProperty ()->BackfaceCullingOff ();
1772  actor->GetProperty ()->SetInterpolationToFlat ();
1773  actor->GetProperty ()->EdgeVisibilityOff ();
1774  actor->GetProperty ()->ShadingOff ();
1775 
1776  // Save the pointer/ID pair to the global actor map
1777  (*cloud_actor_map_)[id].actor = actor;
1778 
1779  // Save the viewpoint transformation matrix to the global actor map
1780  vtkSmartPointer<vtkMatrix4x4> transformation = vtkSmartPointer<vtkMatrix4x4>::New();
1781  convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
1782  (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1783 
1784  return (true);
1785 }
1786 
1787 /////////////////////////////////////////////////////////////////////////////////////////////
1788 template <typename PointT> bool
1789 pcl::visualization::PCLVisualizer::updatePolygonMesh (
1790  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1791  const std::vector<pcl::Vertices> &verts,
1792  const std::string &id)
1793 {
1794  if (verts.empty ())
1795  {
1796  pcl::console::print_error ("[addPolygonMesh] No vertices given!\n");
1797  return (false);
1798  }
1799 
1800  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1801  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1802  if (am_it == cloud_actor_map_->end ())
1803  return (false);
1804 
1805  // Get the current poly data
1806  vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1807  if (!polydata)
1808  return (false);
1809  vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
1810  if (!cells)
1811  return (false);
1812  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
1813  // Copy the new point array in
1814  vtkIdType nr_points = cloud->size ();
1815  points->SetNumberOfPoints (nr_points);
1816 
1817  // Get a pointer to the beginning of the data array
1818  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
1819 
1820  int ptr = 0;
1821  std::vector<int> lookup;
1822  // If the dataset is dense (no NaNs)
1823  if (cloud->is_dense)
1824  {
1825  for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1826  std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1827  }
1828  else
1829  {
1830  lookup.resize (nr_points);
1831  vtkIdType j = 0; // true point index
1832  for (vtkIdType i = 0; i < nr_points; ++i)
1833  {
1834  // Check if the point is invalid
1835  if (!isFinite ((*cloud)[i]))
1836  continue;
1837 
1838  lookup [i] = static_cast<int> (j);
1839  std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1840  j++;
1841  ptr += 3;
1842  }
1843  nr_points = j;
1844  points->SetNumberOfPoints (nr_points);
1845  }
1846 
1847  // Update colors
1848  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1849  if (!colors)
1850  return (false);
1851  int rgb_idx = -1;
1852  std::vector<pcl::PCLPointField> fields;
1853  rgb_idx = pcl::getFieldIndex<PointT> ("rgb", fields);
1854  if (rgb_idx == -1)
1855  rgb_idx = pcl::getFieldIndex<PointT> ("rgba", fields);
1856  if (rgb_idx != -1 && colors)
1857  {
1858  int j = 0;
1859  std::uint32_t offset = fields[rgb_idx].offset;
1860  for (std::size_t i = 0; i < cloud->size (); ++i)
1861  {
1862  if (!isFinite ((*cloud)[i]))
1863  continue;
1864  const pcl::RGB* const rgb_data = reinterpret_cast<const pcl::RGB*>(reinterpret_cast<const char*> (&(*cloud)[i]) + offset);
1865  unsigned char color[3];
1866  color[0] = rgb_data->r;
1867  color[1] = rgb_data->g;
1868  color[2] = rgb_data->b;
1869  colors->SetTupleValue (j++, color);
1870  }
1871  }
1872 
1873  // Get the maximum size of a polygon
1874  int max_size_of_polygon = -1;
1875  for (const auto &vertex : verts)
1876  if (max_size_of_polygon < static_cast<int> (vertex.vertices.size ()))
1877  max_size_of_polygon = static_cast<int> (vertex.vertices.size ());
1878 
1879  // Update the cells
1880  cells = vtkSmartPointer<vtkCellArray>::New ();
1881  vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
1882  int idx = 0;
1883  if (!lookup.empty ())
1884  {
1885  for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
1886  {
1887  std::size_t n_points = verts[i].vertices.size ();
1888  *cell++ = n_points;
1889  for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
1890  *cell = lookup[verts[i].vertices[j]];
1891  }
1892  }
1893  else
1894  {
1895  for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
1896  {
1897  std::size_t n_points = verts[i].vertices.size ();
1898  *cell++ = n_points;
1899  for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
1900  *cell = verts[i].vertices[j];
1901  }
1902  }
1903  cells->GetData ()->SetNumberOfValues (idx);
1904  cells->Squeeze ();
1905  // Set the the vertices
1906  polydata->SetPolys (cells);
1907 
1908  return (true);
1909 }
1910 
1911 #ifdef vtkGenericDataArray_h
1912 #undef SetTupleValue
1913 #undef InsertNextTupleValue
1914 #undef GetTupleValue
1915 #endif
1916 
1917 #endif
pcl::visualization::PCLVisualizer::updateSphere
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.
Definition: pcl_visualizer.hpp:603
pcl::visualization::PCLVisualizer::addPointCloudNormals
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.
Definition: pcl_visualizer.hpp:799
pcl::uint32_t
std::uint32_t uint32_t
Definition: types.h:58
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
pcl::visualization::PointCloudGeometryHandler::getName
virtual std::string getName() const =0
Abstract getName method.
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::visualization::PCLVisualizer::updatePointCloud
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.
Definition: pcl_visualizer.hpp:1471
pcl::isFinite
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:55
pcl::visualization::PointCloudColorHandlerCustom
Handler for predefined user colors.
Definition: point_cloud_color_handlers.h:205
pcl::PointCloud::empty
bool empty() const
Definition: point_cloud.h:462
pcl::visualization::PointCloudColorHandler
Base Handler class for PointCloud colors.
Definition: point_cloud_color_handlers.h:65
pcl::visualization::PCLVisualizer::addPointCloud
bool addPointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a Point Cloud (templated) to screen.
Definition: pcl_visualizer.hpp:76
pcl::visualization::PointCloudColorHandler::getName
virtual std::string getName() const =0
Abstract getName method.
pcl::visualization::PCLVisualizer::GeometryHandlerConstPtr
GeometryHandler::ConstPtr GeometryHandlerConstPtr
Definition: pcl_visualizer.h:94
pcl::visualization::PCLVisualizer::addPolygon
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)
Definition: pcl_visualizer.hpp:315
pcl::visualization::PCLVisualizer::updateCorrespondences
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.
Definition: pcl_visualizer.hpp:1270
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:628
pcl::PointCloud::isOrganized
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:332
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
pcl::visualization::allocVtkUnstructuredGrid
PCL_EXPORTS void allocVtkUnstructuredGrid(vtkSmartPointer< vtkUnstructuredGrid > &polydata)
Allocate a new unstructured grid smartpointer.
pcl::console::print_error
PCL_EXPORTS void print_error(const char *format,...)
Print an error message on stream with colors.
pcl::visualization::PCLVisualizer::addCorrespondences
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.
Definition: pcl_visualizer.hpp:1115
pcl::RGB
A structure representing RGB color information.
Definition: point_types.hpp:345
pcl::PointCloud::sensor_origin_
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:422
pcl::PointCloud::sensor_orientation_
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:424
pcl::visualization::PointCloudGeometryHandler::isCapable
bool isCapable() const
Checl if this handler is capable of handling the input data or not.
Definition: point_cloud_geometry_handlers.h:93
pcl::PointCloud::is_dense
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:419
pcl::visualization::PCLVisualizer::updatePolygonMesh
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.
Definition: pcl_visualizer.hpp:1789
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:459
pcl::visualization::PointCloudColorHandler::getColor
virtual vtkSmartPointer< vtkDataArray > getColor() const
Obtain the actual color for the input dataset as a VTK data array.
Definition: point_cloud_color_handlers.h:107
shapes.h
pcl::visualization::CloudActor
Definition: actor_map.h:59
pcl::visualization::PointCloudColorHandlerRGBField
RGB handler class for colors.
Definition: point_cloud_color_handlers.h:268
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:430
pcl::visualization::PCLVisualizer::ColorHandlerConstPtr
ColorHandler::ConstPtr ColorHandlerConstPtr
Definition: pcl_visualizer.h:98
pcl::visualization::CloudActor::actor
vtkSmartPointer< vtkLODActor > actor
The actor holding the data to render.
Definition: actor_map.h:79
pcl::visualization::PCLVisualizer::addText3D
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.
Definition: pcl_visualizer.hpp:632
pcl::PlanarPolygon
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
Definition: planar_polygon.h:54
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition: correspondence.h:90
pcl::visualization::PCLVisualizer::addLine
bool addLine(const P1 &pt1, const P2 &pt2, const std::string &id="line", int viewport=0)
Add a line segment from two points.
Definition: pcl_visualizer.hpp:547
pcl::visualization::PointCloudGeometryHandler
Base handler class for PointCloud geometry.
Definition: point_cloud_geometry_handlers.h:61
pcl::visualization::createLine
PCL_EXPORTS vtkSmartPointer< vtkDataSet > createLine(const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
Create a line shape from two points.
pcl::visualization::PCLVisualizer::addPolygonMesh
bool addPolygonMesh(const pcl::PolygonMesh &polymesh, const std::string &id="polygon", int viewport=0)
Add a PolygonMesh object to screen.
pcl::visualization::PointCloudColorHandler::isCapable
bool isCapable() const
Check if this handler is capable of handling the input data or not.
Definition: point_cloud_color_handlers.h:90
pcl::visualization::PointCloudGeometryHandlerXYZ
XYZ handler class for PointCloud geometry.
Definition: point_cloud_geometry_handlers.h:139
pcl::visualization::PCLVisualizer::addArrow
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.
Definition: pcl_visualizer.hpp:458
pcl::visualization::PCLVisualizer::addPointCloudPrincipalCurvatures
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.
pcl::visualization::PCLVisualizer::addSphere
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.
Definition: pcl_visualizer.hpp:596
vtkSmartPointer< vtkPolyData >
pcl::visualization::PointCloudGeometryHandler::getGeometry
virtual void getGeometry(vtkSmartPointer< vtkPoints > &points) const =0
Obtain the actual point geometry for the input dataset in VTK format.
pcl::visualization::PCLVisualizer::addPointCloudIntensityGradients
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.
Definition: pcl_visualizer.hpp:1043