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