Point Cloud Library (PCL)  1.9.0-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 
847  if (normals->empty ())
848  {
849  PCL_WARN ("[addPointCloudNormals] An empty normal cloud is given! Nothing to display.\n");
850  return (false);
851  }
852 
853  if (contains (id))
854  {
855  PCL_WARN ("[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
856  return (false);
857  }
858 
861 
862  points->SetDataTypeToFloat ();
864  data->SetNumberOfComponents (3);
865 
866 
867  vtkIdType nr_normals = 0;
868  float* pts = 0;
869 
870  // If the cloud is organized, then distribute the normal step in both directions
871  if (cloud->isOrganized () && normals->isOrganized ())
872  {
873  vtkIdType point_step = static_cast<vtkIdType> (sqrt (double (level)));
874  nr_normals = (static_cast<vtkIdType> ((cloud->width - 1)/ point_step) + 1) *
875  (static_cast<vtkIdType> ((cloud->height - 1) / point_step) + 1);
876  pts = new float[2 * nr_normals * 3];
877 
878  vtkIdType cell_count = 0;
879  for (vtkIdType y = 0; y < normals->height; y += point_step)
880  for (vtkIdType x = 0; x < normals->width; x += point_step)
881  {
882  PointT p = (*cloud)(x, y);
883  p.x += (*normals)(x, y).normal[0] * scale;
884  p.y += (*normals)(x, y).normal[1] * scale;
885  p.z += (*normals)(x, y).normal[2] * scale;
886 
887  pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
888  pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
889  pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
890  pts[2 * cell_count * 3 + 3] = p.x;
891  pts[2 * cell_count * 3 + 4] = p.y;
892  pts[2 * cell_count * 3 + 5] = p.z;
893 
894  lines->InsertNextCell (2);
895  lines->InsertCellPoint (2 * cell_count);
896  lines->InsertCellPoint (2 * cell_count + 1);
897  cell_count ++;
898  }
899  }
900  else
901  {
902  nr_normals = (cloud->points.size () - 1) / level + 1 ;
903  pts = new float[2 * nr_normals * 3];
904 
905  for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
906  {
907  PointT p = cloud->points[i];
908  p.x += normals->points[i].normal[0] * scale;
909  p.y += normals->points[i].normal[1] * scale;
910  p.z += normals->points[i].normal[2] * scale;
911 
912  pts[2 * j * 3 + 0] = cloud->points[i].x;
913  pts[2 * j * 3 + 1] = cloud->points[i].y;
914  pts[2 * j * 3 + 2] = cloud->points[i].z;
915  pts[2 * j * 3 + 3] = p.x;
916  pts[2 * j * 3 + 4] = p.y;
917  pts[2 * j * 3 + 5] = p.z;
918 
919  lines->InsertNextCell (2);
920  lines->InsertCellPoint (2 * j);
921  lines->InsertCellPoint (2 * j + 1);
922  }
923  }
924 
925  data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
926  points->SetData (data);
927 
929  polyData->SetPoints (points);
930  polyData->SetLines (lines);
931 
933 #if VTK_MAJOR_VERSION < 6
934  mapper->SetInput (polyData);
935 #else
936  mapper->SetInputData (polyData);
937 #endif
938  mapper->SetColorModeToMapScalars();
939  mapper->SetScalarModeToUsePointData();
940 
941  // create actor
943  actor->SetMapper (mapper);
944 
945  // Use cloud view point info
947  convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
948  actor->SetUserMatrix (transformation);
949 
950  // Add it to all renderers
951  addActorToRenderer (actor, viewport);
952 
953  // Save the pointer/ID pair to the global actor map
954  (*cloud_actor_map_)[id].actor = actor;
955  return (true);
956 }
957 
958 //////////////////////////////////////////////////////////////////////////////////////////////
959 template <typename PointNT> bool
961  const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
963  int level, float scale,
964  const std::string &id, int viewport)
965 {
966  return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale, id, viewport));
967 }
968 
969 //////////////////////////////////////////////////////////////////////////////////////////////
970 template <typename PointT, typename PointNT> bool
972  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
973  const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
975  int level, float scale,
976  const std::string &id, int viewport)
977 {
978  if (pcs->points.size () != cloud->points.size () || normals->points.size () != cloud->points.size ())
979  {
980  pcl::console::print_error ("[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
981  return (false);
982  }
983 
984  if (contains (id))
985  {
986  PCL_WARN ("[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
987  return (false);
988  }
989 
992 
993  // Setup two colors - one for each line
994  unsigned char green[3] = {0, 255, 0};
995  unsigned char blue[3] = {0, 0, 255};
996 
997  // Setup the colors array
999  line_1_colors->SetNumberOfComponents (3);
1000  line_1_colors->SetName ("Colors");
1002  line_2_colors->SetNumberOfComponents (3);
1003  line_2_colors->SetName ("Colors");
1004 
1005  // Create the first sets of lines
1006  for (size_t i = 0; i < cloud->points.size (); i+=level)
1007  {
1008  PointT p = cloud->points[i];
1009  p.x += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[0]) * scale;
1010  p.y += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[1]) * scale;
1011  p.z += (pcs->points[i].pc1 * pcs->points[i].principal_curvature[2]) * scale;
1012 
1014  line_1->SetPoint1 (cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
1015  line_1->SetPoint2 (p.x, p.y, p.z);
1016  line_1->Update ();
1017 #if VTK_MAJOR_VERSION < 6
1018  polydata_1->AddInput (line_1->GetOutput ());
1019 #else
1020  polydata_1->AddInputData (line_1->GetOutput ());
1021 #endif
1022  line_1_colors->InsertNextTupleValue (green);
1023  }
1024  polydata_1->Update ();
1025  vtkSmartPointer<vtkPolyData> line_1_data = polydata_1->GetOutput ();
1026  line_1_data->GetCellData ()->SetScalars (line_1_colors);
1027 
1028  // Create the second sets of lines
1029  for (size_t i = 0; i < cloud->points.size (); i += level)
1030  {
1031  Eigen::Vector3f pc (pcs->points[i].principal_curvature[0],
1032  pcs->points[i].principal_curvature[1],
1033  pcs->points[i].principal_curvature[2]);
1034  Eigen::Vector3f normal (normals->points[i].normal[0],
1035  normals->points[i].normal[1],
1036  normals->points[i].normal[2]);
1037  Eigen::Vector3f pc_c = pc.cross (normal);
1038 
1039  PointT p = cloud->points[i];
1040  p.x += (pcs->points[i].pc2 * pc_c[0]) * scale;
1041  p.y += (pcs->points[i].pc2 * pc_c[1]) * scale;
1042  p.z += (pcs->points[i].pc2 * pc_c[2]) * scale;
1043 
1045  line_2->SetPoint1 (cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
1046  line_2->SetPoint2 (p.x, p.y, p.z);
1047  line_2->Update ();
1048 #if VTK_MAJOR_VERSION < 6
1049  polydata_2->AddInput (line_2->GetOutput ());
1050 #else
1051  polydata_2->AddInputData (line_2->GetOutput ());
1052 #endif
1053 
1054  line_2_colors->InsertNextTupleValue (blue);
1055  }
1056  polydata_2->Update ();
1057  vtkSmartPointer<vtkPolyData> line_2_data = polydata_2->GetOutput ();
1058  line_2_data->GetCellData ()->SetScalars (line_2_colors);
1059 
1060  // Assemble the two sets of lines
1062 #if VTK_MAJOR_VERSION < 6
1063  alldata->AddInput (line_1_data);
1064  alldata->AddInput (line_2_data);
1065 #else
1066  alldata->AddInputData (line_1_data);
1067  alldata->AddInputData (line_2_data);
1068 #endif
1069 
1070  // Create an Actor
1072  createActorFromVTKDataSet (alldata->GetOutput (), actor);
1073  actor->GetMapper ()->SetScalarModeToUseCellData ();
1074 
1075  // Add it to all renderers
1076  addActorToRenderer (actor, viewport);
1077 
1078  // Save the pointer/ID pair to the global actor map
1079  CloudActor act;
1080  act.actor = actor;
1081  (*cloud_actor_map_)[id] = act;
1082  return (true);
1083 }
1084 
1085 //////////////////////////////////////////////////////////////////////////////////////////////
1086 template <typename PointT, typename GradientT> bool
1088  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1089  const typename pcl::PointCloud<GradientT>::ConstPtr &gradients,
1090  int level, double scale,
1091  const std::string &id, int viewport)
1092 {
1093  if (gradients->points.size () != cloud->points.size ())
1094  {
1095  PCL_ERROR ("[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1096  return (false);
1097  }
1098  if (contains (id))
1099  {
1100  PCL_WARN ("[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1101  return (false);
1102  }
1103 
1106 
1107  points->SetDataTypeToFloat ();
1109  data->SetNumberOfComponents (3);
1110 
1111  vtkIdType nr_gradients = (cloud->points.size () - 1) / level + 1 ;
1112  float* pts = new float[2 * nr_gradients * 3];
1113 
1114  for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1115  {
1116  PointT p = cloud->points[i];
1117  p.x += gradients->points[i].gradient[0] * scale;
1118  p.y += gradients->points[i].gradient[1] * scale;
1119  p.z += gradients->points[i].gradient[2] * scale;
1120 
1121  pts[2 * j * 3 + 0] = cloud->points[i].x;
1122  pts[2 * j * 3 + 1] = cloud->points[i].y;
1123  pts[2 * j * 3 + 2] = cloud->points[i].z;
1124  pts[2 * j * 3 + 3] = p.x;
1125  pts[2 * j * 3 + 4] = p.y;
1126  pts[2 * j * 3 + 5] = p.z;
1127 
1128  lines->InsertNextCell(2);
1129  lines->InsertCellPoint(2*j);
1130  lines->InsertCellPoint(2*j+1);
1131  }
1132 
1133  data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1134  points->SetData (data);
1135 
1137  polyData->SetPoints(points);
1138  polyData->SetLines(lines);
1139 
1141 #if VTK_MAJOR_VERSION < 6
1142  mapper->SetInput (polyData);
1143 #else
1144  mapper->SetInputData (polyData);
1145 #endif
1146  mapper->SetColorModeToMapScalars();
1147  mapper->SetScalarModeToUsePointData();
1148 
1149  // create actor
1151  actor->SetMapper (mapper);
1152 
1153  // Add it to all renderers
1154  addActorToRenderer (actor, viewport);
1155 
1156  // Save the pointer/ID pair to the global actor map
1157  (*cloud_actor_map_)[id].actor = actor;
1158  return (true);
1159 }
1160 
1161 //////////////////////////////////////////////////////////////////////////////////////////////
1162 template <typename PointT> bool
1164  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1165  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1166  const std::vector<int> &correspondences,
1167  const std::string &id,
1168  int viewport)
1169 {
1170  pcl::Correspondences corrs;
1171  corrs.resize (correspondences.size ());
1172 
1173  size_t index = 0;
1174  for (pcl::Correspondences::iterator corrs_it (corrs.begin ()); corrs_it != corrs.end (); ++corrs_it)
1175  {
1176  corrs_it->index_query = index;
1177  corrs_it->index_match = correspondences[index];
1178  index++;
1179  }
1180 
1181  return (addCorrespondences<PointT> (source_points, target_points, corrs, id, viewport));
1182 }
1183 
1184 //////////////////////////////////////////////////////////////////////////////////////////////
1185 template <typename PointT> bool
1187  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1188  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1189  const pcl::Correspondences &correspondences,
1190  int nth,
1191  const std::string &id,
1192  int viewport,
1193  bool overwrite)
1194 {
1195  if (correspondences.empty ())
1196  {
1197  PCL_DEBUG ("[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1198  return (false);
1199  }
1200 
1201  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1202  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
1203  if (am_it != shape_actor_map_->end () && overwrite == false)
1204  {
1205  PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1206  return (false);
1207  } else if (am_it == shape_actor_map_->end () && overwrite == true)
1208  {
1209  overwrite = false; // Correspondences doesn't exist, add them instead of updating them
1210  }
1211 
1212  int n_corr = int (correspondences.size () / nth);
1214 
1215  // Prepare colors
1217  line_colors->SetNumberOfComponents (3);
1218  line_colors->SetName ("Colors");
1219  line_colors->SetNumberOfTuples (n_corr);
1220 
1221  // Prepare coordinates
1223  line_points->SetNumberOfPoints (2 * n_corr);
1224 
1226  line_cells_id->SetNumberOfComponents (3);
1227  line_cells_id->SetNumberOfTuples (n_corr);
1228  vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1230 
1232  line_tcoords->SetNumberOfComponents (1);
1233  line_tcoords->SetNumberOfTuples (n_corr * 2);
1234  line_tcoords->SetName ("Texture Coordinates");
1235 
1236  double tc[3] = {0.0, 0.0, 0.0};
1237 
1238  Eigen::Affine3f source_transformation;
1239  source_transformation.linear () = source_points->sensor_orientation_.matrix ();
1240  source_transformation.translation () = source_points->sensor_origin_.head (3);
1241  Eigen::Affine3f target_transformation;
1242  target_transformation.linear () = target_points->sensor_orientation_.matrix ();
1243  target_transformation.translation () = target_points->sensor_origin_.head (3);
1244 
1245  int j = 0;
1246  // Draw lines between the best corresponding points
1247  for (size_t i = 0; i < correspondences.size (); i += nth, ++j)
1248  {
1249  if (correspondences[i].index_match == -1)
1250  {
1251  PCL_WARN ("[addCorrespondences] No valid index_match for correspondence %d\n", i);
1252  continue;
1253  }
1254 
1255  PointT p_src (source_points->points[correspondences[i].index_query]);
1256  PointT p_tgt (target_points->points[correspondences[i].index_match]);
1257 
1258  p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1259  p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1260 
1261  int id1 = j * 2 + 0, id2 = j * 2 + 1;
1262  // Set the points
1263  line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1264  line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1265  // Set the cell ID
1266  *line_cell_id++ = 2;
1267  *line_cell_id++ = id1;
1268  *line_cell_id++ = id2;
1269  // Set the texture coords
1270  tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1271  tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1272 
1273  float rgb[3];
1274  rgb[0] = vtkMath::Random (32, 255); // min / max
1275  rgb[1] = vtkMath::Random (32, 255);
1276  rgb[2] = vtkMath::Random (32, 255);
1277  line_colors->InsertTuple (i, rgb);
1278  }
1279  line_colors->SetNumberOfTuples (j);
1280  line_cells_id->SetNumberOfTuples (j);
1281  line_cells->SetCells (n_corr, line_cells_id);
1282  line_points->SetNumberOfPoints (j*2);
1283  line_tcoords->SetNumberOfTuples (j*2);
1284 
1285  // Fill in the lines
1286  line_data->SetPoints (line_points);
1287  line_data->SetLines (line_cells);
1288  line_data->GetPointData ()->SetTCoords (line_tcoords);
1289  line_data->GetCellData ()->SetScalars (line_colors);
1290 
1291  // Create an Actor
1292  if (!overwrite)
1293  {
1295  createActorFromVTKDataSet (line_data, actor);
1296  actor->GetProperty ()->SetRepresentationToWireframe ();
1297  actor->GetProperty ()->SetOpacity (0.5);
1298  addActorToRenderer (actor, viewport);
1299 
1300  // Save the pointer/ID pair to the global actor map
1301  (*shape_actor_map_)[id] = actor;
1302  }
1303  else
1304  {
1305  vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
1306  if (!actor)
1307  return (false);
1308  // Update the mapper
1309 #if VTK_MAJOR_VERSION < 6
1310  reinterpret_cast<vtkPolyDataMapper*>(actor->GetMapper ())->SetInput (line_data);
1311 #else
1312  reinterpret_cast<vtkPolyDataMapper*> (actor->GetMapper ())->SetInputData (line_data);
1313 #endif
1314  }
1315 
1316  return (true);
1317 }
1318 
1319 //////////////////////////////////////////////////////////////////////////////////////////////
1320 template <typename PointT> bool
1322  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1323  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1324  const pcl::Correspondences &correspondences,
1325  int nth,
1326  const std::string &id,
1327  int viewport)
1328 {
1329  return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth, id, viewport, true));
1330 }
1331 
1332 //////////////////////////////////////////////////////////////////////////////////////////////
1333 template <typename PointT> bool
1334 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1335  const PointCloudGeometryHandler<PointT> &geometry_handler,
1336  const PointCloudColorHandler<PointT> &color_handler,
1337  const std::string &id,
1338  int viewport,
1339  const Eigen::Vector4f& sensor_origin,
1340  const Eigen::Quaternion<float>& sensor_orientation)
1341 {
1342  if (!geometry_handler.isCapable ())
1343  {
1344  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
1345  return (false);
1346  }
1347 
1348  if (!color_handler.isCapable ())
1349  {
1350  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
1351  return (false);
1352  }
1353 
1356  // Convert the PointCloud to VTK PolyData
1357  convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1358  // use the given geometry handler
1359 
1360  // Get the colors from the handler
1361  bool has_colors = false;
1362  double minmax[2];
1364  if (color_handler.getColor (scalars))
1365  {
1366  polydata->GetPointData ()->SetScalars (scalars);
1367  scalars->GetRange (minmax);
1368  has_colors = true;
1369  }
1370 
1371  // Create an Actor
1373  createActorFromVTKDataSet (polydata, actor);
1374  if (has_colors)
1375  actor->GetMapper ()->SetScalarRange (minmax);
1376 
1377  // Add it to all renderers
1378  addActorToRenderer (actor, viewport);
1379 
1380  // Save the pointer/ID pair to the global actor map
1381  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1382  cloud_actor.actor = actor;
1383  cloud_actor.cells = initcells;
1384 
1385  // Save the viewpoint transformation matrix to the global actor map
1387  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1388  cloud_actor.viewpoint_transformation_ = transformation;
1389  cloud_actor.actor->SetUserMatrix (transformation);
1390  cloud_actor.actor->Modified ();
1391 
1392  return (true);
1393 }
1394 
1395 //////////////////////////////////////////////////////////////////////////////////////////////
1396 template <typename PointT> bool
1397 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1398  const PointCloudGeometryHandler<PointT> &geometry_handler,
1399  const ColorHandlerConstPtr &color_handler,
1400  const std::string &id,
1401  int viewport,
1402  const Eigen::Vector4f& sensor_origin,
1403  const Eigen::Quaternion<float>& sensor_orientation)
1404 {
1405  if (!geometry_handler.isCapable ())
1406  {
1407  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
1408  return (false);
1409  }
1410 
1411  if (!color_handler->isCapable ())
1412  {
1413  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler->getName ().c_str ());
1414  return (false);
1415  }
1416 
1419  // Convert the PointCloud to VTK PolyData
1420  convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1421  // use the given geometry handler
1422 
1423  // Get the colors from the handler
1424  bool has_colors = false;
1425  double minmax[2];
1427  if (color_handler->getColor (scalars))
1428  {
1429  polydata->GetPointData ()->SetScalars (scalars);
1430  scalars->GetRange (minmax);
1431  has_colors = true;
1432  }
1433 
1434  // Create an Actor
1436  createActorFromVTKDataSet (polydata, actor);
1437  if (has_colors)
1438  actor->GetMapper ()->SetScalarRange (minmax);
1439 
1440  // Add it to all renderers
1441  addActorToRenderer (actor, viewport);
1442 
1443  // Save the pointer/ID pair to the global actor map
1444  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1445  cloud_actor.actor = actor;
1446  cloud_actor.cells = initcells;
1447  cloud_actor.color_handlers.push_back (color_handler);
1448 
1449  // Save the viewpoint transformation matrix to the global actor map
1451  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1452  cloud_actor.viewpoint_transformation_ = transformation;
1453  cloud_actor.actor->SetUserMatrix (transformation);
1454  cloud_actor.actor->Modified ();
1455 
1456  return (true);
1457 }
1458 
1459 //////////////////////////////////////////////////////////////////////////////////////////////
1460 template <typename PointT> bool
1461 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1462  const GeometryHandlerConstPtr &geometry_handler,
1463  const PointCloudColorHandler<PointT> &color_handler,
1464  const std::string &id,
1465  int viewport,
1466  const Eigen::Vector4f& sensor_origin,
1467  const Eigen::Quaternion<float>& sensor_orientation)
1468 {
1469  if (!geometry_handler->isCapable ())
1470  {
1471  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler->getName ().c_str ());
1472  return (false);
1473  }
1474 
1475  if (!color_handler.isCapable ())
1476  {
1477  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
1478  return (false);
1479  }
1480 
1483  // Convert the PointCloud to VTK PolyData
1484  convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1485  // use the given geometry handler
1486 
1487  // Get the colors from the handler
1488  bool has_colors = false;
1489  double minmax[2];
1491  if (color_handler.getColor (scalars))
1492  {
1493  polydata->GetPointData ()->SetScalars (scalars);
1494  scalars->GetRange (minmax);
1495  has_colors = true;
1496  }
1497 
1498  // Create an Actor
1500  createActorFromVTKDataSet (polydata, actor);
1501  if (has_colors)
1502  actor->GetMapper ()->SetScalarRange (minmax);
1503 
1504  // Add it to all renderers
1505  addActorToRenderer (actor, viewport);
1506 
1507  // Save the pointer/ID pair to the global actor map
1508  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1509  cloud_actor.actor = actor;
1510  cloud_actor.cells = initcells;
1511  cloud_actor.geometry_handlers.push_back (geometry_handler);
1512 
1513  // Save the viewpoint transformation matrix to the global actor map
1515  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1516  cloud_actor.viewpoint_transformation_ = transformation;
1517  cloud_actor.actor->SetUserMatrix (transformation);
1518  cloud_actor.actor->Modified ();
1519 
1520  return (true);
1521 }
1522 
1523 //////////////////////////////////////////////////////////////////////////////////////////////
1524 template <typename PointT> bool
1526  const std::string &id)
1527 {
1528  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1529  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1530 
1531  if (am_it == cloud_actor_map_->end ())
1532  return (false);
1533 
1534  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1535  // Convert the PointCloud to VTK PolyData
1536  convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1537 
1538  // Set scalars to blank, since there is no way we can update them here.
1540  polydata->GetPointData ()->SetScalars (scalars);
1541  double minmax[2];
1542  minmax[0] = std::numeric_limits<double>::min ();
1543  minmax[1] = std::numeric_limits<double>::max ();
1544 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1545  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1546 #endif
1547  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1548 
1549  // Update the mapper
1550 #if VTK_MAJOR_VERSION < 6
1551  reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1552 #else
1553  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1554 #endif
1555  return (true);
1556 }
1557 
1558 /////////////////////////////////////////////////////////////////////////////////////////////
1559 template <typename PointT> bool
1561  const PointCloudGeometryHandler<PointT> &geometry_handler,
1562  const std::string &id)
1563 {
1564  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1565  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1566 
1567  if (am_it == cloud_actor_map_->end ())
1568  return (false);
1569 
1570  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1571  if (!polydata)
1572  return (false);
1573  // Convert the PointCloud to VTK PolyData
1574  convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1575 
1576  // Set scalars to blank, since there is no way we can update them here.
1578  polydata->GetPointData ()->SetScalars (scalars);
1579  double minmax[2];
1580  minmax[0] = std::numeric_limits<double>::min ();
1581  minmax[1] = std::numeric_limits<double>::max ();
1582 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1583  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1584 #endif
1585  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1586 
1587  // Update the mapper
1588 #if VTK_MAJOR_VERSION < 6
1589  reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1590 #else
1591  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1592 #endif
1593  return (true);
1594 }
1595 
1596 
1597 /////////////////////////////////////////////////////////////////////////////////////////////
1598 template <typename PointT> bool
1600  const PointCloudColorHandler<PointT> &color_handler,
1601  const std::string &id)
1602 {
1603  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1604  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1605 
1606  if (am_it == cloud_actor_map_->end ())
1607  return (false);
1608 
1609  // Get the current poly data
1610  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1611  if (!polydata)
1612  return (false);
1613  vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
1614  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
1615  // Copy the new point array in
1616  vtkIdType nr_points = cloud->points.size ();
1617  points->SetNumberOfPoints (nr_points);
1618 
1619  // Get a pointer to the beginning of the data array
1620  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
1621 
1622  vtkIdType pts = 0;
1623  // If the dataset is dense (no NaNs)
1624  if (cloud->is_dense)
1625  {
1626  for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
1627  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[pts]);
1628  }
1629  else
1630  {
1631  vtkIdType j = 0; // true point index
1632  for (vtkIdType i = 0; i < nr_points; ++i)
1633  {
1634  // Check if the point is invalid
1635  if (!isFinite (cloud->points[i]))
1636  continue;
1637  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[pts]);
1638  pts += 3;
1639  j++;
1640  }
1641  nr_points = j;
1642  points->SetNumberOfPoints (nr_points);
1643  }
1644 
1645  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
1646  updateCells (cells, am_it->second.cells, nr_points);
1647 
1648  // Set the cells and the vertices
1649  vertices->SetCells (nr_points, cells);
1650 
1651  // Get the colors from the handler
1652  bool has_colors = false;
1653  double minmax[2];
1655  if (color_handler.getColor (scalars))
1656  {
1657  // Update the data
1658  polydata->GetPointData ()->SetScalars (scalars);
1659  scalars->GetRange (minmax);
1660  has_colors = true;
1661  }
1662 
1663 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1664  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1665 #endif
1666 
1667  if (has_colors)
1668  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1669 
1670  // Update the mapper
1671 #if VTK_MAJOR_VERSION < 6
1672  reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1673 #else
1674  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1675 #endif
1676  return (true);
1677 }
1678 
1679 /////////////////////////////////////////////////////////////////////////////////////////////
1680 template <typename PointT> bool
1682  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1683  const std::vector<pcl::Vertices> &vertices,
1684  const std::string &id,
1685  int viewport)
1686 {
1687  if (vertices.empty () || cloud->points.empty ())
1688  return (false);
1689 
1690  if (contains (id))
1691  {
1692  PCL_WARN ("[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1693  return (false);
1694  }
1695 
1696  int rgb_idx = -1;
1697  std::vector<pcl::PCLPointField> fields;
1699  rgb_idx = pcl::getFieldIndex (*cloud, "rgb", fields);
1700  if (rgb_idx == -1)
1701  rgb_idx = pcl::getFieldIndex (*cloud, "rgba", fields);
1702  if (rgb_idx != -1)
1703  {
1705  colors->SetNumberOfComponents (3);
1706  colors->SetName ("Colors");
1707  uint32_t offset = fields[rgb_idx].offset;
1708  for (size_t i = 0; i < cloud->size (); ++i)
1709  {
1710  if (!isFinite (cloud->points[i]))
1711  continue;
1712  const pcl::RGB* const rgb_data = reinterpret_cast<const pcl::RGB*>(reinterpret_cast<const char*> (&cloud->points[i]) + offset);
1713  unsigned char color[3];
1714  color[0] = rgb_data->r;
1715  color[1] = rgb_data->g;
1716  color[2] = rgb_data->b;
1717  colors->InsertNextTupleValue (color);
1718  }
1719  }
1720 
1721  // Create points from polyMesh.cloud
1723  vtkIdType nr_points = cloud->points.size ();
1724  points->SetNumberOfPoints (nr_points);
1726 
1727  // Get a pointer to the beginning of the data array
1728  float *data = static_cast<vtkFloatArray*> (points->GetData ())->GetPointer (0);
1729 
1730  vtkIdType ptr = 0;
1731  std::vector<int> lookup;
1732  // If the dataset is dense (no NaNs)
1733  if (cloud->is_dense)
1734  {
1735  for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1736  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
1737  }
1738  else
1739  {
1740  lookup.resize (nr_points);
1741  vtkIdType j = 0; // true point index
1742  for (vtkIdType i = 0; i < nr_points; ++i)
1743  {
1744  // Check if the point is invalid
1745  if (!isFinite (cloud->points[i]))
1746  continue;
1747 
1748  lookup[i] = static_cast<int> (j);
1749  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
1750  j++;
1751  ptr += 3;
1752  }
1753  nr_points = j;
1754  points->SetNumberOfPoints (nr_points);
1755  }
1756 
1757  // Get the maximum size of a polygon
1758  int max_size_of_polygon = -1;
1759  for (size_t i = 0; i < vertices.size (); ++i)
1760  if (max_size_of_polygon < static_cast<int> (vertices[i].vertices.size ()))
1761  max_size_of_polygon = static_cast<int> (vertices[i].vertices.size ());
1762 
1763  if (vertices.size () > 1)
1764  {
1765  // Create polys from polyMesh.polygons
1767  vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
1768  int idx = 0;
1769  if (lookup.size () > 0)
1770  {
1771  for (size_t i = 0; i < vertices.size (); ++i, ++idx)
1772  {
1773  size_t n_points = vertices[i].vertices.size ();
1774  *cell++ = n_points;
1775  //cell_array->InsertNextCell (n_points);
1776  for (size_t j = 0; j < n_points; j++, ++idx)
1777  *cell++ = lookup[vertices[i].vertices[j]];
1778  //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
1779  }
1780  }
1781  else
1782  {
1783  for (size_t i = 0; i < vertices.size (); ++i, ++idx)
1784  {
1785  size_t n_points = vertices[i].vertices.size ();
1786  *cell++ = n_points;
1787  //cell_array->InsertNextCell (n_points);
1788  for (size_t j = 0; j < n_points; j++, ++idx)
1789  *cell++ = vertices[i].vertices[j];
1790  //cell_array->InsertCellPoint (vertices[i].vertices[j]);
1791  }
1792  }
1794  allocVtkPolyData (polydata);
1795  cell_array->GetData ()->SetNumberOfValues (idx);
1796  cell_array->Squeeze ();
1797  polydata->SetPolys (cell_array);
1798  polydata->SetPoints (points);
1799 
1800  if (colors)
1801  polydata->GetPointData ()->SetScalars (colors);
1802 
1803  createActorFromVTKDataSet (polydata, actor, false);
1804  }
1805  else
1806  {
1808  size_t n_points = vertices[0].vertices.size ();
1809  polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1810 
1811  if (lookup.size () > 0)
1812  {
1813  for (size_t j = 0; j < (n_points - 1); ++j)
1814  polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1815  }
1816  else
1817  {
1818  for (size_t j = 0; j < (n_points - 1); ++j)
1819  polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1820  }
1822  allocVtkUnstructuredGrid (poly_grid);
1823  poly_grid->Allocate (1, 1);
1824  poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1825  poly_grid->SetPoints (points);
1826  if (colors)
1827  poly_grid->GetPointData ()->SetScalars (colors);
1828 
1829  createActorFromVTKDataSet (poly_grid, actor, false);
1830  }
1831  addActorToRenderer (actor, viewport);
1832  actor->GetProperty ()->SetRepresentationToSurface ();
1833  // Backface culling renders the visualization slower, but guarantees that we see all triangles
1834  actor->GetProperty ()->BackfaceCullingOff ();
1835  actor->GetProperty ()->SetInterpolationToFlat ();
1836  actor->GetProperty ()->EdgeVisibilityOff ();
1837  actor->GetProperty ()->ShadingOff ();
1838 
1839  // Save the pointer/ID pair to the global actor map
1840  (*cloud_actor_map_)[id].actor = actor;
1841 
1842  // Save the viewpoint transformation matrix to the global actor map
1844  convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
1845  (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1846 
1847  return (true);
1848 }
1849 
1850 /////////////////////////////////////////////////////////////////////////////////////////////
1851 template <typename PointT> bool
1853  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1854  const std::vector<pcl::Vertices> &verts,
1855  const std::string &id)
1856 {
1857  if (verts.empty ())
1858  {
1859  pcl::console::print_error ("[addPolygonMesh] No vertices given!\n");
1860  return (false);
1861  }
1862 
1863  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1864  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1865  if (am_it == cloud_actor_map_->end ())
1866  return (false);
1867 
1868  // Get the current poly data
1869  vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1870  if (!polydata)
1871  return (false);
1872  vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
1873  if (!cells)
1874  return (false);
1875  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
1876  // Copy the new point array in
1877  vtkIdType nr_points = cloud->points.size ();
1878  points->SetNumberOfPoints (nr_points);
1879 
1880  // Get a pointer to the beginning of the data array
1881  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
1882 
1883  int ptr = 0;
1884  std::vector<int> lookup;
1885  // If the dataset is dense (no NaNs)
1886  if (cloud->is_dense)
1887  {
1888  for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1889  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
1890  }
1891  else
1892  {
1893  lookup.resize (nr_points);
1894  vtkIdType j = 0; // true point index
1895  for (vtkIdType i = 0; i < nr_points; ++i)
1896  {
1897  // Check if the point is invalid
1898  if (!isFinite (cloud->points[i]))
1899  continue;
1900 
1901  lookup [i] = static_cast<int> (j);
1902  std::copy (&cloud->points[i].x, &cloud->points[i].x + 3, &data[ptr]);
1903  j++;
1904  ptr += 3;
1905  }
1906  nr_points = j;
1907  points->SetNumberOfPoints (nr_points);
1908  }
1909 
1910  // Update colors
1911  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1912  if (!colors)
1913  return (false);
1914  int rgb_idx = -1;
1915  std::vector<pcl::PCLPointField> fields;
1916  rgb_idx = pcl::getFieldIndex (*cloud, "rgb", fields);
1917  if (rgb_idx == -1)
1918  rgb_idx = pcl::getFieldIndex (*cloud, "rgba", fields);
1919  if (rgb_idx != -1 && colors)
1920  {
1921  int j = 0;
1922  uint32_t offset = fields[rgb_idx].offset;
1923  for (size_t i = 0; i < cloud->size (); ++i)
1924  {
1925  if (!isFinite (cloud->points[i]))
1926  continue;
1927  const pcl::RGB* const rgb_data = reinterpret_cast<const pcl::RGB*>(reinterpret_cast<const char*> (&cloud->points[i]) + offset);
1928  unsigned char color[3];
1929  color[0] = rgb_data->r;
1930  color[1] = rgb_data->g;
1931  color[2] = rgb_data->b;
1932  colors->SetTupleValue (j++, color);
1933  }
1934  }
1935 
1936  // Get the maximum size of a polygon
1937  int max_size_of_polygon = -1;
1938  for (size_t i = 0; i < verts.size (); ++i)
1939  if (max_size_of_polygon < static_cast<int> (verts[i].vertices.size ()))
1940  max_size_of_polygon = static_cast<int> (verts[i].vertices.size ());
1941 
1942  // Update the cells
1944  vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
1945  int idx = 0;
1946  if (lookup.size () > 0)
1947  {
1948  for (size_t i = 0; i < verts.size (); ++i, ++idx)
1949  {
1950  size_t n_points = verts[i].vertices.size ();
1951  *cell++ = n_points;
1952  for (size_t j = 0; j < n_points; j++, cell++, ++idx)
1953  *cell = lookup[verts[i].vertices[j]];
1954  }
1955  }
1956  else
1957  {
1958  for (size_t i = 0; i < verts.size (); ++i, ++idx)
1959  {
1960  size_t n_points = verts[i].vertices.size ();
1961  *cell++ = n_points;
1962  for (size_t j = 0; j < n_points; j++, cell++, ++idx)
1963  *cell = verts[i].vertices[j];
1964  }
1965  }
1966  cells->GetData ()->SetNumberOfValues (idx);
1967  cells->Squeeze ();
1968  // Set the the vertices
1969  polydata->SetPolys (cells);
1970 
1971  return (true);
1972 }
1973 
1974 #ifdef vtkGenericDataArray_h
1975 #undef SetTupleValue
1976 #undef InsertNextTupleValue
1977 #undef GetTupleValue
1978 #endif
1979 
1980 #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
size_t size() const
Definition: point_cloud.h:448
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
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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.
static void convertToVtkMatrix(const Eigen::Matrix4f &m, vtkSmartPointer< vtkMatrix4x4 > &vtk_matrix)
Convert Eigen::Matrix4f to vtkMatrix4x4.
std::vector< ColorHandlerConstPtr > color_handlers
A vector of color handlers that can be used for rendering the data.
Definition: actor_map.h:84
bool addPointCloudIntensityGradients(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const typename pcl::PointCloud< GradientT >::ConstPtr &gradients, int level=100, double scale=1e-6, const std::string &id="cloud", int viewport=0)
Add the estimated surface intensity gradients of a Point Cloud to screen.
bool contains(const std::string &id) const
Check if the cloud, shape, or coordinate with the given id was already added to this visualizer...
bool addPolygon(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, double r, double g, double b, const std::string &id="polygon", int viewport=0)
Add a polygon (polyline) that represents the input point cloud (connects all points in order) ...
bool addPointCloudPrincipalCurvatures(const typename pcl::PointCloud< PointNT >::ConstPtr &cloud, const typename pcl::PointCloud< pcl::PrincipalCurvatures >::ConstPtr &pcs, int level=100, float scale=1.0f, const std::string &id="cloud", int viewport=0)
Add the estimated principal curvatures of a Point Cloud to screen.
Base Handler class for PointCloud colors.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
bool addSphere(const PointT &center, double radius, const std::string &id="sphere", int viewport=0)
Add a sphere shape from a point and a radius.
bool addCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const std::vector< int > &correspondences, const std::string &id="correspondences", int viewport=0)
Add the specified correspondences to the display.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
bool updateCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const pcl::Correspondences &correspondences, int nth, const std::string &id="correspondences", int viewport=0)
Update the specified correspondences to the display.
vtkSmartPointer< vtkIdTypeArray > cells
Internal cell array.
Definition: actor_map.h:96
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.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
vtkSmartPointer< vtkLODActor > actor
The actor holding the data to render.
Definition: actor_map.h:78
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
bool addArrow(const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id="arrow", int viewport=0)
Add a line arrow segment between two points, and display the distance between them.
bool updatePointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud")
Updates the XYZ data for an existing cloud object id on screen.
bool isCapable() const
Checl if this handler is capable of handling the input data or not.
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:418
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:331
Base handler class for PointCloud geometry.
std::vector< GeometryHandlerConstPtr > geometry_handlers
A vector of geometry handlers that can be used for rendering the data.
Definition: actor_map.h:81
virtual std::string getName() const =0
Abstract getName method.
GeometryHandler::ConstPtr GeometryHandlerConstPtr
bool addLine(const P1 &pt1, const P2 &pt2, const std::string &id="line", int viewport=0)
Add a line segment from two points.
bool addText3D(const std::string &text, const PointT &position, double textScale=1.0, double r=1.0, double g=1.0, double b=1.0, const std::string &id="", int viewport=0)
Add a 3d text to the scene.
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual std::string getName() const =0
Abstract getName method.
bool updateSphere(const PointT &center, double radius, double r, double g, double b, const std::string &id="sphere")
Update an existing sphere shape from a point and a radius.
vtkSmartPointer< vtkMatrix4x4 > viewpoint_transformation_
The viewpoint transformation matrix.
Definition: actor_map.h:93
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 empty() const
Definition: point_cloud.h:450