Point Cloud Library (PCL)  1.7.2
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 <vtkSphereSource.h>
49 #include <vtkProperty2D.h>
50 #include <vtkDataSetSurfaceFilter.h>
51 #include <vtkPointData.h>
52 #include <vtkPolyDataMapper.h>
53 #include <vtkProperty.h>
54 #include <vtkMapper.h>
55 #include <vtkCellData.h>
56 #include <vtkDataSetMapper.h>
57 #include <vtkRenderer.h>
58 #include <vtkRendererCollection.h>
59 #include <vtkAppendPolyData.h>
60 #include <vtkTextProperty.h>
61 #include <vtkLODActor.h>
62 
63 #include <pcl/visualization/common/shapes.h>
64 
65 // Support for VTK 7.1 upwards
66 #ifdef vtkGenericDataArray_h
67 #define SetTupleValue SetTypedTuple
68 #define InsertNextTupleValue InsertNextTypedTuple
69 #define GetTupleValue GetTypedTuple
70 #endif
71 
72 //////////////////////////////////////////////////////////////////////////////////////////////
73 template <typename PointT> bool
75  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
76  const std::string &id, int viewport)
77 {
78  // Convert the PointCloud to VTK PolyData
79  PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
80  return (addPointCloud<PointT> (cloud, geometry_handler, id, viewport));
81 }
82 
83 //////////////////////////////////////////////////////////////////////////////////////////////
84 template <typename PointT> bool
86  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
87  const PointCloudGeometryHandler<PointT> &geometry_handler,
88  const std::string &id, int viewport)
89 {
90  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
91  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
92 
93  if (am_it != cloud_actor_map_->end ())
94  {
95  PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
96  return (false);
97  }
98 
99  //PointCloudColorHandlerRandom<PointT> color_handler (cloud);
100  PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
101  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
102 }
103 
104 //////////////////////////////////////////////////////////////////////////////////////////////
105 template <typename PointT> bool
107  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
108  const GeometryHandlerConstPtr &geometry_handler,
109  const std::string &id, int viewport)
110 {
111  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
112  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
113 
114  if (am_it != cloud_actor_map_->end ())
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  am_it->second.geometry_handlers.push_back (geometry_handler);
119  return (true);
120  }
121 
122  //PointCloudColorHandlerRandom<PointT> color_handler (cloud);
123  PointCloudColorHandlerCustom<PointT> color_handler (cloud, 255, 255, 255);
124  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
125 }
126 
127 //////////////////////////////////////////////////////////////////////////////////////////////
128 template <typename PointT> bool
130  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
131  const PointCloudColorHandler<PointT> &color_handler,
132  const std::string &id, int viewport)
133 {
134  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
135  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
136 
137  if (am_it != cloud_actor_map_->end ())
138  {
139  PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
140 
141  // Here we're just pushing the handlers onto the queue. If needed, something fancier could
142  // be done such as checking if a specific handler already exists, etc.
143  //cloud_actor_map_[id].color_handlers.push_back (color_handler);
144  //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_));
145  return (false);
146  }
147  // Convert the PointCloud to VTK PolyData
148  PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
149  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
150 }
151 
152 //////////////////////////////////////////////////////////////////////////////////////////////
153 template <typename PointT> bool
155  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
156  const ColorHandlerConstPtr &color_handler,
157  const std::string &id, int viewport)
158 {
159  // Check to see if this entry already exists (has it been already added to the visualizer?)
160  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
161  if (am_it != cloud_actor_map_->end ())
162  {
163  // Here we're just pushing the handlers onto the queue. If needed, something fancier could
164  // be done such as checking if a specific handler already exists, etc.
165  am_it->second.color_handlers.push_back (color_handler);
166  return (true);
167  }
168 
169  PointCloudGeometryHandlerXYZ<PointT> geometry_handler (cloud);
170  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
171 }
172 
173 //////////////////////////////////////////////////////////////////////////////////////////////
174 template <typename PointT> bool
176  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
177  const GeometryHandlerConstPtr &geometry_handler,
178  const ColorHandlerConstPtr &color_handler,
179  const std::string &id, int viewport)
180 {
181  // Check to see if this entry already exists (has it been already added to the visualizer?)
182  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
183  if (am_it != cloud_actor_map_->end ())
184  {
185  // Here we're just pushing the handlers onto the queue. If needed, something fancier could
186  // be done such as checking if a specific handler already exists, etc.
187  am_it->second.geometry_handlers.push_back (geometry_handler);
188  am_it->second.color_handlers.push_back (color_handler);
189  return (true);
190  }
191  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
192 }
193 
194 //////////////////////////////////////////////////////////////////////////////////////////////
195 template <typename PointT> bool
197  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
198  const PointCloudColorHandler<PointT> &color_handler,
199  const PointCloudGeometryHandler<PointT> &geometry_handler,
200  const std::string &id, int viewport)
201 {
202  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
203  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
204 
205  if (am_it != cloud_actor_map_->end ())
206  {
207  PCL_WARN ("[addPointCloud] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
208  // Here we're just pushing the handlers onto the queue. If needed, something fancier could
209  // be done such as checking if a specific handler already exists, etc.
210  //cloud_actor_map_[id].geometry_handlers.push_back (geometry_handler);
211  //cloud_actor_map_[id].color_handlers.push_back (color_handler);
212  //style_->setCloudActorMap (boost::make_shared<CloudActorMap> (cloud_actor_map_));
213  return (false);
214  }
215  return (fromHandlersToScreen (geometry_handler, color_handler, id, viewport, cloud->sensor_origin_, cloud->sensor_orientation_));
216 }
217 
218 //////////////////////////////////////////////////////////////////////////////////////////////
219 template <typename PointT> void
220 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
221  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
224 {
226  if (!polydata)
227  {
228  allocVtkPolyData (polydata);
230  polydata->SetVerts (vertices);
231  }
232 
233  // Create the supporting structures
234  vertices = polydata->GetVerts ();
235  if (!vertices)
237 
238  vtkIdType nr_points = cloud->points.size ();
239  // Create the point set
240  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
241  if (!points)
242  {
244  points->SetDataTypeToFloat ();
245  polydata->SetPoints (points);
246  }
247  points->SetNumberOfPoints (nr_points);
248 
249  // Get a pointer to the beginning of the data array
250  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
251 
252  // Set the points
253  if (cloud->is_dense)
254  {
255  for (vtkIdType i = 0; i < nr_points; ++i)
256  memcpy (&data[i * 3], &cloud->points[i].x, 12); // sizeof (float) * 3
257  }
258  else
259  {
260  vtkIdType j = 0; // true point index
261  for (vtkIdType i = 0; i < nr_points; ++i)
262  {
263  // Check if the point is invalid
264  if (!pcl_isfinite (cloud->points[i].x) ||
265  !pcl_isfinite (cloud->points[i].y) ||
266  !pcl_isfinite (cloud->points[i].z))
267  continue;
268 
269  memcpy (&data[j * 3], &cloud->points[i].x, 12); // sizeof (float) * 3
270  j++;
271  }
272  nr_points = j;
273  points->SetNumberOfPoints (nr_points);
274  }
275 
276  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
277  updateCells (cells, initcells, nr_points);
278 
279  // Set the cells and the vertices
280  vertices->SetCells (nr_points, cells);
281 }
282 
283 //////////////////////////////////////////////////////////////////////////////////////////////
284 template <typename PointT> void
285 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
289 {
291  if (!polydata)
292  {
293  allocVtkPolyData (polydata);
295  polydata->SetVerts (vertices);
296  }
297 
298  // Use the handler to obtain the geometry
300  geometry_handler.getGeometry (points);
301  polydata->SetPoints (points);
302 
303  vtkIdType nr_points = points->GetNumberOfPoints ();
304 
305  // Create the supporting structures
306  vertices = polydata->GetVerts ();
307  if (!vertices)
309 
310  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
311  updateCells (cells, initcells, nr_points);
312  // Set the cells and the vertices
313  vertices->SetCells (nr_points, cells);
314 }
315 
316 ////////////////////////////////////////////////////////////////////////////////////////////
317 template <typename PointT> bool
319  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
320  double r, double g, double b, const std::string &id, int viewport)
321 {
322  vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (cloud);
323  if (!data)
324  return (false);
325 
326  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
327  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
328  if (am_it != shape_actor_map_->end ())
329  {
331 
332  // Add old data
333 #if VTK_MAJOR_VERSION < 6
334  all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
335 #else
336  all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
337 #endif
338 
339  // Add new data
341 #if VTK_MAJOR_VERSION < 6
342  surface_filter->AddInput (vtkUnstructuredGrid::SafeDownCast (data));
343 #else
344  surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
345 #endif
346  vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
347 #if VTK_MAJOR_VERSION < 6
348  all_data->AddInput (poly_data);
349 #else
350  all_data->AddInputData (poly_data);
351 #endif
352 
353  // Create an Actor
355  createActorFromVTKDataSet (all_data->GetOutput (), actor);
356  actor->GetProperty ()->SetRepresentationToWireframe ();
357  actor->GetProperty ()->SetColor (r, g, b);
358  actor->GetMapper ()->ScalarVisibilityOff ();
359  removeActorFromRenderer (am_it->second, viewport);
360  addActorToRenderer (actor, viewport);
361 
362  // Save the pointer/ID pair to the global actor map
363  (*shape_actor_map_)[id] = actor;
364  }
365  else
366  {
367  // Create an Actor
369  createActorFromVTKDataSet (data, actor);
370  actor->GetProperty ()->SetRepresentationToWireframe ();
371  actor->GetProperty ()->SetColor (r, g, b);
372  actor->GetMapper ()->ScalarVisibilityOff ();
373  addActorToRenderer (actor, viewport);
374 
375  // Save the pointer/ID pair to the global actor map
376  (*shape_actor_map_)[id] = actor;
377  }
378 
379  return (true);
380 }
381 
382 ////////////////////////////////////////////////////////////////////////////////////////////
383 template <typename PointT> bool
385  const pcl::PlanarPolygon<PointT> &polygon,
386  double r, double g, double b, const std::string &id, int viewport)
387 {
388  vtkSmartPointer<vtkDataSet> data = createPolygon<PointT> (polygon);
389  if (!data)
390  return (false);
391 
392  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
393  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
394  if (am_it != shape_actor_map_->end ())
395  {
397 
398  // Add old data
399 #if VTK_MAJOR_VERSION < 6
400  all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
401 #else
402  all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
403 #endif
404 
405  // Add new data
407 #if VTK_MAJOR_VERSION < 6
408  surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data));
409 #else
410  surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
411 #endif
412  vtkSmartPointer<vtkPolyData> poly_data = surface_filter->GetOutput ();
413 #if VTK_MAJOR_VERSION < 6
414  all_data->AddInput (poly_data);
415 #else
416  all_data->AddInputData (poly_data);
417 #endif
418 
419  // Create an Actor
421  createActorFromVTKDataSet (all_data->GetOutput (), actor);
422  actor->GetProperty ()->SetRepresentationToWireframe ();
423  actor->GetProperty ()->SetColor (r, g, b);
424  actor->GetMapper ()->ScalarVisibilityOn ();
425  actor->GetProperty ()->BackfaceCullingOff ();
426  removeActorFromRenderer (am_it->second, viewport);
427  addActorToRenderer (actor, viewport);
428 
429  // Save the pointer/ID pair to the global actor map
430  (*shape_actor_map_)[id] = actor;
431  }
432  else
433  {
434  // Create an Actor
436  createActorFromVTKDataSet (data, actor);
437  actor->GetProperty ()->SetRepresentationToWireframe ();
438  actor->GetProperty ()->SetColor (r, g, b);
439  actor->GetMapper ()->ScalarVisibilityOn ();
440  actor->GetProperty ()->BackfaceCullingOff ();
441  addActorToRenderer (actor, viewport);
442 
443  // Save the pointer/ID pair to the global actor map
444  (*shape_actor_map_)[id] = actor;
445  }
446  return (true);
447 }
448 
449 ////////////////////////////////////////////////////////////////////////////////////////////
450 template <typename PointT> bool
452  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
453  const std::string &id, int viewport)
454 {
455  return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5, id, viewport));
456 }
457 
458 ////////////////////////////////////////////////////////////////////////////////////////////
459 template <typename P1, typename P2> bool
460 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
461 {
462  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
463  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
464  if (am_it != shape_actor_map_->end ())
465  {
466  PCL_WARN ("[addLine] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
467  return (false);
468  }
469 
470  vtkSmartPointer<vtkDataSet> data = createLine (pt1.getVector4fMap (), pt2.getVector4fMap ());
471 
472  // Create an Actor
474  createActorFromVTKDataSet (data, actor);
475  actor->GetProperty ()->SetRepresentationToWireframe ();
476  actor->GetProperty ()->SetColor (r, g, b);
477  actor->GetMapper ()->ScalarVisibilityOff ();
478  addActorToRenderer (actor, viewport);
479 
480  // Save the pointer/ID pair to the global actor map
481  (*shape_actor_map_)[id] = actor;
482  return (true);
483 }
484 
485 ////////////////////////////////////////////////////////////////////////////////////////////
486 template <typename P1, typename P2> bool
487 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id, int viewport)
488 {
489  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
490  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
491  if (am_it != shape_actor_map_->end ())
492  {
493  PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
494  return (false);
495  }
496 
497  // Create an Actor
499  leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
500  leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
501  leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
502  leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
503  leader->SetArrowStyleToFilled ();
504  leader->AutoLabelOn ();
505 
506  leader->GetProperty ()->SetColor (r, g, b);
507  addActorToRenderer (leader, viewport);
508 
509  // Save the pointer/ID pair to the global actor map
510  (*shape_actor_map_)[id] = leader;
511  return (true);
512 }
513 
514 ////////////////////////////////////////////////////////////////////////////////////////////
515 template <typename P1, typename P2> bool
516 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)
517 {
518  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
519  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
520  if (am_it != shape_actor_map_->end ())
521  {
522  PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
523  return (false);
524  }
525 
526  // Create an Actor
528  leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
529  leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
530  leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
531  leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
532  leader->SetArrowStyleToFilled ();
533  leader->SetArrowPlacementToPoint1 ();
534  if (display_length)
535  leader->AutoLabelOn ();
536  else
537  leader->AutoLabelOff ();
538 
539  leader->GetProperty ()->SetColor (r, g, b);
540  addActorToRenderer (leader, viewport);
541 
542  // Save the pointer/ID pair to the global actor map
543  (*shape_actor_map_)[id] = leader;
544  return (true);
545 }
546 ////////////////////////////////////////////////////////////////////////////////////////////
547 template <typename P1, typename P2> bool
548 pcl::visualization::PCLVisualizer::addArrow (const P1 &pt1, const P2 &pt2,
549  double r_line, double g_line, double b_line,
550  double r_text, double g_text, double b_text,
551  const std::string &id, int viewport)
552 {
553  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
554  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
555  if (am_it != shape_actor_map_->end ())
556  {
557  PCL_WARN ("[addArrow] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
558  return (false);
559  }
560 
561  // Create an Actor
563  leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
564  leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
565  leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
566  leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
567  leader->SetArrowStyleToFilled ();
568  leader->AutoLabelOn ();
569 
570  leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
571 
572  leader->GetProperty ()->SetColor (r_line, g_line, b_line);
573  addActorToRenderer (leader, viewport);
574 
575  // Save the pointer/ID pair to the global actor map
576  (*shape_actor_map_)[id] = leader;
577  return (true);
578 }
579 
580 ////////////////////////////////////////////////////////////////////////////////////////////
581 template <typename P1, typename P2> bool
582 pcl::visualization::PCLVisualizer::addLine (const P1 &pt1, const P2 &pt2, const std::string &id, int viewport)
583 {
584  return (!addLine (pt1, pt2, 0.5, 0.5, 0.5, id, viewport));
585 }
586 
587 ////////////////////////////////////////////////////////////////////////////////////////////
588 template <typename PointT> bool
589 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id, int viewport)
590 {
591  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
592  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
593  if (am_it != shape_actor_map_->end ())
594  {
595  PCL_WARN ("[addSphere] A shape with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
596  return (false);
597  }
598 
600  data->SetRadius (radius);
601  data->SetCenter (double (center.x), double (center.y), double (center.z));
602  data->SetPhiResolution (10);
603  data->SetThetaResolution (10);
604  data->LatLongTessellationOff ();
605  data->Update ();
606 
607  // Setup actor and mapper
609  mapper->SetInputConnection (data->GetOutputPort ());
610 
611  // Create an Actor
613  actor->SetMapper (mapper);
614  //createActorFromVTKDataSet (data, actor);
615  actor->GetProperty ()->SetRepresentationToSurface ();
616  actor->GetProperty ()->SetInterpolationToFlat ();
617  actor->GetProperty ()->SetColor (r, g, b);
618  actor->GetMapper ()->ImmediateModeRenderingOn ();
619  actor->GetMapper ()->StaticOn ();
620  actor->GetMapper ()->ScalarVisibilityOff ();
621  actor->GetMapper ()->Update ();
622  addActorToRenderer (actor, viewport);
623 
624  // Save the pointer/ID pair to the global actor map
625  (*shape_actor_map_)[id] = actor;
626  return (true);
627 }
628 
629 ////////////////////////////////////////////////////////////////////////////////////////////
630 template <typename PointT> bool
631 pcl::visualization::PCLVisualizer::addSphere (const PointT &center, double radius, const std::string &id, int viewport)
632 {
633  return (addSphere (center, radius, 0.5, 0.5, 0.5, id, viewport));
634 }
635 
636 ////////////////////////////////////////////////////////////////////////////////////////////
637 template<typename PointT> bool
638 pcl::visualization::PCLVisualizer::updateSphere (const PointT &center, double radius, double r, double g, double b, const std::string &id)
639 {
640  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
641  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
642  if (am_it == shape_actor_map_->end ())
643  return (false);
644 
645  //////////////////////////////////////////////////////////////////////////
646  // Get the actor pointer
647  vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
648 #if VTK_MAJOR_VERSION < 6
649  vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer ();
650 #else
651  vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
652 #endif
653  vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
654 
655  src->SetCenter (double (center.x), double (center.y), double (center.z));
656  src->SetRadius (radius);
657  src->Update ();
658  actor->GetProperty ()->SetColor (r, g, b);
659  actor->Modified ();
660 
661  return (true);
662 }
663 
664 //////////////////////////////////////////////////
665 template <typename PointT> bool
667  const std::string &text,
668  const PointT& position,
669  double textScale,
670  double r,
671  double g,
672  double b,
673  const std::string &id,
674  int viewport)
675 {
676  std::string tid;
677  if (id.empty ())
678  tid = text;
679  else
680  tid = id;
681 
682  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
683  ShapeActorMap::iterator am_it = shape_actor_map_->find (tid);
684  if (am_it != shape_actor_map_->end ())
685  {
686  pcl::console::print_warn (stderr, "[addText3d] A text with id <%s> already exists! Please choose a different id and retry.\n", tid.c_str ());
687  return (false);
688  }
689 
691  textSource->SetText (text.c_str());
692  textSource->Update ();
693 
695  textMapper->SetInputConnection (textSource->GetOutputPort ());
696 
697  // Since each follower may follow a different camera, we need different followers
698  rens_->InitTraversal ();
699  vtkRenderer* renderer = NULL;
700  int i = 1;
701  while ((renderer = rens_->GetNextItem ()) != NULL)
702  {
703  // Should we add the actor to all renderers or just to i-nth renderer?
704  if (viewport == 0 || viewport == i)
705  {
707  textActor->SetMapper (textMapper);
708  textActor->SetPosition (position.x, position.y, position.z);
709  textActor->SetScale (textScale);
710  textActor->GetProperty ()->SetColor (r, g, b);
711  textActor->SetCamera (renderer->GetActiveCamera ());
712 
713  renderer->AddActor (textActor);
714  renderer->Render ();
715 
716  // Save the pointer/ID pair to the global actor map. If we are saving multiple vtkFollowers
717  // for multiple viewport
718  std::string alternate_tid = tid;
719  alternate_tid.append(i, '*');
720 
721  (*shape_actor_map_)[(viewport == 0) ? tid : alternate_tid] = textActor;
722  }
723 
724  ++i;
725  }
726 
727  return (true);
728 }
729 
730 //////////////////////////////////////////////////////////////////////////////////////////////
731 template <typename PointNT> bool
733  const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
734  int level, float scale, const std::string &id, int viewport)
735 {
736  return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale, id, viewport));
737 }
738 
739 //////////////////////////////////////////////////////////////////////////////////////////////
740 template <typename PointT, typename PointNT> bool
742  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
743  const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
744  int level, float scale,
745  const std::string &id, int viewport)
746 {
747  if (normals->points.size () != cloud->points.size ())
748  {
749  PCL_ERROR ("[addPointCloudNormals] The number of points differs from the number of normals!\n");
750  return (false);
751  }
752  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
753  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
754 
755  if (am_it != cloud_actor_map_->end ())
756  {
757  PCL_WARN ("[addPointCloudNormals] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
758  return (false);
759  }
760 
763 
764  points->SetDataTypeToFloat ();
766  data->SetNumberOfComponents (3);
767 
768 
769  vtkIdType nr_normals = 0;
770  float* pts = 0;
771 
772  // If the cloud is organized, then distribute the normal step in both directions
773  if (cloud->isOrganized () && normals->isOrganized ())
774  {
775  vtkIdType point_step = static_cast<vtkIdType> (sqrt (double (level)));
776  nr_normals = (static_cast<vtkIdType> ((cloud->width - 1)/ point_step) + 1) *
777  (static_cast<vtkIdType> ((cloud->height - 1) / point_step) + 1);
778  pts = new float[2 * nr_normals * 3];
779 
780  vtkIdType cell_count = 0;
781  for (vtkIdType y = 0; y < normals->height; y += point_step)
782  for (vtkIdType x = 0; x < normals->width; x += point_step)
783  {
784  PointT p = (*cloud)(x, y);
785  p.x += (*normals)(x, y).normal[0] * scale;
786  p.y += (*normals)(x, y).normal[1] * scale;
787  p.z += (*normals)(x, y).normal[2] * scale;
788 
789  pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
790  pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
791  pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
792  pts[2 * cell_count * 3 + 3] = p.x;
793  pts[2 * cell_count * 3 + 4] = p.y;
794  pts[2 * cell_count * 3 + 5] = p.z;
795 
796  lines->InsertNextCell (2);
797  lines->InsertCellPoint (2 * cell_count);
798  lines->InsertCellPoint (2 * cell_count + 1);
799  cell_count ++;
800  }
801  }
802  else
803  {
804  nr_normals = (cloud->points.size () - 1) / level + 1 ;
805  pts = new float[2 * nr_normals * 3];
806 
807  for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
808  {
809  PointT p = cloud->points[i];
810  p.x += normals->points[i].normal[0] * scale;
811  p.y += normals->points[i].normal[1] * scale;
812  p.z += normals->points[i].normal[2] * scale;
813 
814  pts[2 * j * 3 + 0] = cloud->points[i].x;
815  pts[2 * j * 3 + 1] = cloud->points[i].y;
816  pts[2 * j * 3 + 2] = cloud->points[i].z;
817  pts[2 * j * 3 + 3] = p.x;
818  pts[2 * j * 3 + 4] = p.y;
819  pts[2 * j * 3 + 5] = p.z;
820 
821  lines->InsertNextCell (2);
822  lines->InsertCellPoint (2 * j);
823  lines->InsertCellPoint (2 * j + 1);
824  }
825  }
826 
827  data->SetArray (&pts[0], 2 * nr_normals * 3, 0);
828  points->SetData (data);
829 
831  polyData->SetPoints (points);
832  polyData->SetLines (lines);
833 
835 #if VTK_MAJOR_VERSION < 6
836  mapper->SetInput (polyData);
837 #else
838  mapper->SetInputData (polyData);
839 #endif
840  mapper->SetColorModeToMapScalars();
841  mapper->SetScalarModeToUsePointData();
842 
843  // create actor
845  actor->SetMapper (mapper);
846 
847  // Add it to all renderers
848  addActorToRenderer (actor, viewport);
849 
850  // Save the pointer/ID pair to the global actor map
851  (*cloud_actor_map_)[id].actor = actor;
852  return (true);
853 }
854 
855 //////////////////////////////////////////////////////////////////////////////////////////////
856 template <typename PointT, typename GradientT> bool
858  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
859  const typename pcl::PointCloud<GradientT>::ConstPtr &gradients,
860  int level, double scale,
861  const std::string &id, int viewport)
862 {
863  if (gradients->points.size () != cloud->points.size ())
864  {
865  PCL_ERROR ("[addPointCloudGradients] The number of points differs from the number of gradients!\n");
866  return (false);
867  }
868  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
869  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
870 
871  if (am_it != cloud_actor_map_->end ())
872  {
873  PCL_WARN ("[addPointCloudGradients] A PointCloud with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
874  return (false);
875  }
876 
879 
880  points->SetDataTypeToFloat ();
882  data->SetNumberOfComponents (3);
883 
884  vtkIdType nr_gradients = (cloud->points.size () - 1) / level + 1 ;
885  float* pts = new float[2 * nr_gradients * 3];
886 
887  for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
888  {
889  PointT p = cloud->points[i];
890  p.x += gradients->points[i].gradient[0] * scale;
891  p.y += gradients->points[i].gradient[1] * scale;
892  p.z += gradients->points[i].gradient[2] * scale;
893 
894  pts[2 * j * 3 + 0] = cloud->points[i].x;
895  pts[2 * j * 3 + 1] = cloud->points[i].y;
896  pts[2 * j * 3 + 2] = cloud->points[i].z;
897  pts[2 * j * 3 + 3] = p.x;
898  pts[2 * j * 3 + 4] = p.y;
899  pts[2 * j * 3 + 5] = p.z;
900 
901  lines->InsertNextCell(2);
902  lines->InsertCellPoint(2*j);
903  lines->InsertCellPoint(2*j+1);
904  }
905 
906  data->SetArray (&pts[0], 2 * nr_gradients * 3, 0);
907  points->SetData (data);
908 
910  polyData->SetPoints(points);
911  polyData->SetLines(lines);
912 
914 #if VTK_MAJOR_VERSION < 6
915  mapper->SetInput (polyData);
916 #else
917  mapper->SetInputData (polyData);
918 #endif
919  mapper->SetColorModeToMapScalars();
920  mapper->SetScalarModeToUsePointData();
921 
922  // create actor
924  actor->SetMapper (mapper);
925 
926  // Add it to all renderers
927  addActorToRenderer (actor, viewport);
928 
929  // Save the pointer/ID pair to the global actor map
930  (*cloud_actor_map_)[id].actor = actor;
931  return (true);
932 }
933 
934 //////////////////////////////////////////////////////////////////////////////////////////////
935 template <typename PointT> bool
937  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
938  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
939  const std::vector<int> &correspondences,
940  const std::string &id,
941  int viewport)
942 {
943  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
944  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
945  if (am_it != shape_actor_map_->end ())
946  {
947  PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
948  return (false);
949  }
950 
951  int n_corr = int (correspondences.size ());
953 
954  // Prepare colors
956  line_colors->SetNumberOfComponents (3);
957  line_colors->SetName ("Colors");
958  line_colors->SetNumberOfTuples (n_corr);
959  unsigned char* colors = line_colors->GetPointer (0);
960  memset (colors, 0, line_colors->GetNumberOfTuples () * line_colors->GetNumberOfComponents ());
961  pcl::RGB rgb;
962  // Will use random colors or RED by default
963  rgb.r = 255; rgb.g = rgb.b = 0;
964 
965  // Prepare coordinates
967  line_points->SetNumberOfPoints (2 * n_corr);
968 
970  line_cells_id->SetNumberOfComponents (3);
971  line_cells_id->SetNumberOfTuples (n_corr);
972  vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
974 
976  line_tcoords->SetNumberOfComponents (1);
977  line_tcoords->SetNumberOfTuples (n_corr * 2);
978  line_tcoords->SetName ("Texture Coordinates");
979 
980  double tc[3] = {0.0, 0.0, 0.0};
981 
982  int j = 0, idx = 0;
983  // Draw lines between the best corresponding points
984  for (int i = 0; i < n_corr; ++i)
985  {
986  const PointT &p_src = source_points->points[i];
987  const PointT &p_tgt = target_points->points[correspondences[i]];
988 
989  int id1 = j * 2 + 0, id2 = j * 2 + 1;
990  // Set the points
991  line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
992  line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
993  // Set the cell ID
994  *line_cell_id++ = 2;
995  *line_cell_id++ = id1;
996  *line_cell_id++ = id2;
997  // Set the texture coords
998  tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
999  tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1000 
1001  getRandomColors (rgb);
1002  colors[idx+0] = rgb.r;
1003  colors[idx+1] = rgb.g;
1004  colors[idx+2] = rgb.b;
1005  }
1006  line_colors->SetNumberOfTuples (j);
1007  line_cells_id->SetNumberOfTuples (j);
1008  line_cells->SetCells (n_corr, line_cells_id);
1009  line_points->SetNumberOfPoints (j*2);
1010  line_tcoords->SetNumberOfTuples (j*2);
1011 
1012  // Fill in the lines
1013  line_data->SetPoints (line_points);
1014  line_data->SetLines (line_cells);
1015  line_data->GetPointData ()->SetTCoords (line_tcoords);
1016  line_data->GetCellData ()->SetScalars (line_colors);
1017 
1018  // Create an Actor
1020  createActorFromVTKDataSet (line_data, actor);
1021  actor->GetProperty ()->SetRepresentationToWireframe ();
1022  actor->GetProperty ()->SetOpacity (0.5);
1023  addActorToRenderer (actor, viewport);
1024 
1025  // Save the pointer/ID pair to the global actor map
1026  (*shape_actor_map_)[id] = actor;
1027 
1028  return (true);
1029 }
1030 
1031 //////////////////////////////////////////////////////////////////////////////////////////////
1032 template <typename PointT> bool
1034  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1035  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1036  const pcl::Correspondences &correspondences,
1037  int nth,
1038  const std::string &id,
1039  int viewport)
1040 {
1041  if (correspondences.empty ())
1042  {
1043  PCL_DEBUG ("[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1044  return (false);
1045  }
1046 
1047  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1048  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
1049  if (am_it != shape_actor_map_->end ())
1050  {
1051  PCL_WARN ("[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n", id.c_str ());
1052  return (false);
1053  }
1054 
1055  int n_corr = int (correspondences.size () / nth + 1);
1057 
1058  // Prepare colors
1060  line_colors->SetNumberOfComponents (3);
1061  line_colors->SetName ("Colors");
1062  line_colors->SetNumberOfTuples (n_corr);
1063  unsigned char* colors = line_colors->GetPointer (0);
1064  memset (colors, 0, line_colors->GetNumberOfTuples () * line_colors->GetNumberOfComponents ());
1065  pcl::RGB rgb;
1066  // Will use random colors or RED by default
1067  rgb.r = 255; rgb.g = rgb.b = 0;
1068 
1069  // Prepare coordinates
1071  line_points->SetNumberOfPoints (2 * n_corr);
1072 
1074  line_cells_id->SetNumberOfComponents (3);
1075  line_cells_id->SetNumberOfTuples (n_corr);
1076  vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1078 
1080  line_tcoords->SetNumberOfComponents (1);
1081  line_tcoords->SetNumberOfTuples (n_corr * 2);
1082  line_tcoords->SetName ("Texture Coordinates");
1083 
1084  double tc[3] = {0.0, 0.0, 0.0};
1085 
1086  int j = 0, idx = 0;
1087  // Draw lines between the best corresponding points
1088  for (size_t i = 0; i < correspondences.size (); i += nth, idx = j * 3, ++j)
1089  {
1090  const PointT &p_src = source_points->points[correspondences[i].index_query];
1091  const PointT &p_tgt = target_points->points[correspondences[i].index_match];
1092 
1093  int id1 = j * 2 + 0, id2 = j * 2 + 1;
1094  // Set the points
1095  line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1096  line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1097  // Set the cell ID
1098  *line_cell_id++ = 2;
1099  *line_cell_id++ = id1;
1100  *line_cell_id++ = id2;
1101  // Set the texture coords
1102  tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1103  tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1104 
1105  getRandomColors (rgb);
1106  colors[idx+0] = rgb.r;
1107  colors[idx+1] = rgb.g;
1108  colors[idx+2] = rgb.b;
1109  }
1110  line_colors->SetNumberOfTuples (j);
1111  line_cells_id->SetNumberOfTuples (j);
1112  line_cells->SetCells (n_corr, line_cells_id);
1113  line_points->SetNumberOfPoints (j*2);
1114  line_tcoords->SetNumberOfTuples (j*2);
1115 
1116  // Fill in the lines
1117  line_data->SetPoints (line_points);
1118  line_data->SetLines (line_cells);
1119  line_data->GetPointData ()->SetTCoords (line_tcoords);
1120  line_data->GetCellData ()->SetScalars (line_colors);
1121 
1122  // Create an Actor
1124  createActorFromVTKDataSet (line_data, actor);
1125  actor->GetProperty ()->SetRepresentationToWireframe ();
1126  actor->GetProperty ()->SetOpacity (0.5);
1127  addActorToRenderer (actor, viewport);
1128 
1129  // Save the pointer/ID pair to the global actor map
1130  (*shape_actor_map_)[id] = actor;
1131 
1132  return (true);
1133 }
1134 
1135 //////////////////////////////////////////////////////////////////////////////////////////////
1136 template <typename PointT> bool
1138  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1139  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1140  const pcl::Correspondences &correspondences,
1141  int nth,
1142  const std::string &id)
1143 {
1144  if (correspondences.empty ())
1145  {
1146  PCL_DEBUG ("[updateCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1147  return (false);
1148  }
1149 
1150  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1151  ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
1152  if (am_it == shape_actor_map_->end ())
1153  return (false);
1154 
1155  vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
1156  vtkSmartPointer<vtkPolyData> line_data = reinterpret_cast<vtkPolyDataMapper*>(actor->GetMapper ())->GetInput ();
1157 
1158  int n_corr = int (correspondences.size () / nth + 1);
1159 
1160  // Prepare colors
1162  line_colors->SetNumberOfComponents (3);
1163  line_colors->SetName ("Colors");
1164  line_colors->SetNumberOfTuples (n_corr);
1165  unsigned char* colors = line_colors->GetPointer (0);
1166  memset (colors, 0, line_colors->GetNumberOfTuples () * line_colors->GetNumberOfComponents ());
1167  pcl::RGB rgb;
1168  // Will use random colors or RED by default
1169  rgb.r = 255.0; rgb.g = rgb.b = 0.0;
1170 
1171  // Prepare coordinates
1173  line_points->SetNumberOfPoints (2 * n_corr);
1174 
1176  line_cells_id->SetNumberOfComponents (3);
1177  line_cells_id->SetNumberOfTuples (n_corr);
1178  vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1179  vtkSmartPointer<vtkCellArray> line_cells = line_data->GetLines ();
1180 
1182  line_tcoords->SetNumberOfComponents (1);
1183  line_tcoords->SetNumberOfTuples (n_corr * 2);
1184  line_tcoords->SetName ("Texture Coordinates");
1185 
1186  double tc[3] = {0.0, 0.0, 0.0};
1187 
1188  int j = 0, idx = 0;
1189  // Draw lines between the best corresponding points
1190  for (size_t i = 0; i < correspondences.size (); i += nth, idx = j * 3, ++j)
1191  {
1192  const PointT &p_src = source_points->points[correspondences[i].index_query];
1193  const PointT &p_tgt = target_points->points[correspondences[i].index_match];
1194 
1195  int id1 = j * 2 + 0, id2 = j * 2 + 1;
1196  // Set the points
1197  line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1198  line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1199  // Set the cell ID
1200  *line_cell_id++ = 2;
1201  *line_cell_id++ = id1;
1202  *line_cell_id++ = id2;
1203  // Set the texture coords
1204  tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1205  tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1206 
1207  getRandomColors (rgb);
1208  colors[idx+0] = rgb.r;
1209  colors[idx+1] = rgb.g;
1210  colors[idx+2] = rgb.b;
1211  }
1212  line_colors->SetNumberOfTuples (j);
1213  line_cells_id->SetNumberOfTuples (j);
1214  line_cells->SetCells (n_corr, line_cells_id);
1215  line_points->SetNumberOfPoints (j*2);
1216  line_tcoords->SetNumberOfTuples (j*2);
1217 
1218  // Fill in the lines
1219  line_data->SetPoints (line_points);
1220  line_data->SetLines (line_cells);
1221  line_data->GetPointData ()->SetTCoords (line_tcoords);
1222  line_data->GetCellData ()->SetScalars (line_colors);
1223 
1224  // Update the mapper
1225 #if VTK_MAJOR_VERSION < 6
1226  reinterpret_cast<vtkPolyDataMapper*>(actor->GetMapper ())->SetInput (line_data);
1227 #else
1228  reinterpret_cast<vtkPolyDataMapper*> (actor->GetMapper ())->SetInputData (line_data);
1229 #endif
1230 
1231  return (true);
1232 }
1233 
1234 //////////////////////////////////////////////////////////////////////////////////////////////
1235 template <typename PointT> bool
1236 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1237  const PointCloudGeometryHandler<PointT> &geometry_handler,
1238  const PointCloudColorHandler<PointT> &color_handler,
1239  const std::string &id,
1240  int viewport,
1241  const Eigen::Vector4f& sensor_origin,
1242  const Eigen::Quaternion<float>& sensor_orientation)
1243 {
1244  if (!geometry_handler.isCapable ())
1245  {
1246  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
1247  return (false);
1248  }
1249 
1250  if (!color_handler.isCapable ())
1251  {
1252  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
1253  return (false);
1254  }
1255 
1258  // Convert the PointCloud to VTK PolyData
1259  convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1260  // use the given geometry handler
1261 
1262  // Get the colors from the handler
1263  bool has_colors = false;
1264  double minmax[2];
1266  if (color_handler.getColor (scalars))
1267  {
1268  polydata->GetPointData ()->SetScalars (scalars);
1269  scalars->GetRange (minmax);
1270  has_colors = true;
1271  }
1272 
1273  // Create an Actor
1275  createActorFromVTKDataSet (polydata, actor);
1276  if (has_colors)
1277  actor->GetMapper ()->SetScalarRange (minmax);
1278 
1279  // Add it to all renderers
1280  addActorToRenderer (actor, viewport);
1281 
1282  // Save the pointer/ID pair to the global actor map
1283  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1284  cloud_actor.actor = actor;
1285  cloud_actor.cells = initcells;
1286 
1287  // Save the viewpoint transformation matrix to the global actor map
1289  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1290  cloud_actor.viewpoint_transformation_ = transformation;
1291  cloud_actor.actor->SetUserMatrix (transformation);
1292  cloud_actor.actor->Modified ();
1293 
1294  return (true);
1295 }
1296 
1297 //////////////////////////////////////////////////////////////////////////////////////////////
1298 template <typename PointT> bool
1299 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1300  const PointCloudGeometryHandler<PointT> &geometry_handler,
1301  const ColorHandlerConstPtr &color_handler,
1302  const std::string &id,
1303  int viewport,
1304  const Eigen::Vector4f& sensor_origin,
1305  const Eigen::Quaternion<float>& sensor_orientation)
1306 {
1307  if (!geometry_handler.isCapable ())
1308  {
1309  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler.getName ().c_str ());
1310  return (false);
1311  }
1312 
1313  if (!color_handler->isCapable ())
1314  {
1315  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler->getName ().c_str ());
1316  return (false);
1317  }
1318 
1321  // Convert the PointCloud to VTK PolyData
1322  convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1323  // use the given geometry handler
1324 
1325  // Get the colors from the handler
1326  bool has_colors = false;
1327  double minmax[2];
1329  if (color_handler->getColor (scalars))
1330  {
1331  polydata->GetPointData ()->SetScalars (scalars);
1332  scalars->GetRange (minmax);
1333  has_colors = true;
1334  }
1335 
1336  // Create an Actor
1338  createActorFromVTKDataSet (polydata, actor);
1339  if (has_colors)
1340  actor->GetMapper ()->SetScalarRange (minmax);
1341 
1342  // Add it to all renderers
1343  addActorToRenderer (actor, viewport);
1344 
1345  // Save the pointer/ID pair to the global actor map
1346  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1347  cloud_actor.actor = actor;
1348  cloud_actor.cells = initcells;
1349  cloud_actor.color_handlers.push_back (color_handler);
1350 
1351  // Save the viewpoint transformation matrix to the global actor map
1353  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1354  cloud_actor.viewpoint_transformation_ = transformation;
1355  cloud_actor.actor->SetUserMatrix (transformation);
1356  cloud_actor.actor->Modified ();
1357 
1358  return (true);
1359 }
1360 
1361 //////////////////////////////////////////////////////////////////////////////////////////////
1362 template <typename PointT> bool
1363 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1364  const GeometryHandlerConstPtr &geometry_handler,
1365  const PointCloudColorHandler<PointT> &color_handler,
1366  const std::string &id,
1367  int viewport,
1368  const Eigen::Vector4f& sensor_origin,
1369  const Eigen::Quaternion<float>& sensor_orientation)
1370 {
1371  if (!geometry_handler->isCapable ())
1372  {
1373  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n", id.c_str (), geometry_handler->getName ().c_str ());
1374  return (false);
1375  }
1376 
1377  if (!color_handler.isCapable ())
1378  {
1379  PCL_WARN ("[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n", id.c_str (), color_handler.getName ().c_str ());
1380  return (false);
1381  }
1382 
1385  // Convert the PointCloud to VTK PolyData
1386  convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1387  // use the given geometry handler
1388 
1389  // Get the colors from the handler
1390  bool has_colors = false;
1391  double minmax[2];
1393  if (color_handler.getColor (scalars))
1394  {
1395  polydata->GetPointData ()->SetScalars (scalars);
1396  scalars->GetRange (minmax);
1397  has_colors = true;
1398  }
1399 
1400  // Create an Actor
1402  createActorFromVTKDataSet (polydata, actor);
1403  if (has_colors)
1404  actor->GetMapper ()->SetScalarRange (minmax);
1405 
1406  // Add it to all renderers
1407  addActorToRenderer (actor, viewport);
1408 
1409  // Save the pointer/ID pair to the global actor map
1410  CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1411  cloud_actor.actor = actor;
1412  cloud_actor.cells = initcells;
1413  cloud_actor.geometry_handlers.push_back (geometry_handler);
1414 
1415  // Save the viewpoint transformation matrix to the global actor map
1417  convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1418  cloud_actor.viewpoint_transformation_ = transformation;
1419  cloud_actor.actor->SetUserMatrix (transformation);
1420  cloud_actor.actor->Modified ();
1421 
1422  return (true);
1423 }
1424 
1425 //////////////////////////////////////////////////////////////////////////////////////////////
1426 template <typename PointT> bool
1428  const std::string &id)
1429 {
1430  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1431  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1432 
1433  if (am_it == cloud_actor_map_->end ())
1434  return (false);
1435 
1436  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1437  // Convert the PointCloud to VTK PolyData
1438  convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1439 
1440  // Set scalars to blank, since there is no way we can update them here.
1442  polydata->GetPointData ()->SetScalars (scalars);
1443  double minmax[2];
1444  minmax[0] = std::numeric_limits<double>::min ();
1445  minmax[1] = std::numeric_limits<double>::max ();
1446  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1447  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1448 
1449  // Update the mapper
1450 #if VTK_MAJOR_VERSION < 6
1451  reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1452 #else
1453  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1454 #endif
1455  return (true);
1456 }
1457 
1458 /////////////////////////////////////////////////////////////////////////////////////////////
1459 template <typename PointT> bool
1461  const PointCloudGeometryHandler<PointT> &geometry_handler,
1462  const std::string &id)
1463 {
1464  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1465  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1466 
1467  if (am_it == cloud_actor_map_->end ())
1468  return (false);
1469 
1470  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1471  if (!polydata)
1472  return (false);
1473  // Convert the PointCloud to VTK PolyData
1474  convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1475 
1476  // Set scalars to blank, since there is no way we can update them here.
1478  polydata->GetPointData ()->SetScalars (scalars);
1479  double minmax[2];
1480  minmax[0] = std::numeric_limits<double>::min ();
1481  minmax[1] = std::numeric_limits<double>::max ();
1482  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1483  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1484 
1485  // Update the mapper
1486 #if VTK_MAJOR_VERSION < 6
1487  reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1488 #else
1489  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1490 #endif
1491  return (true);
1492 }
1493 
1494 
1495 /////////////////////////////////////////////////////////////////////////////////////////////
1496 template <typename PointT> bool
1498  const PointCloudColorHandler<PointT> &color_handler,
1499  const std::string &id)
1500 {
1501  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1502  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1503 
1504  if (am_it == cloud_actor_map_->end ())
1505  return (false);
1506 
1507  // Get the current poly data
1508  vtkSmartPointer<vtkPolyData> polydata = reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1509  if (!polydata)
1510  return (false);
1511  vtkSmartPointer<vtkCellArray> vertices = polydata->GetVerts ();
1512  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
1513  // Copy the new point array in
1514  vtkIdType nr_points = cloud->points.size ();
1515  points->SetNumberOfPoints (nr_points);
1516 
1517  // Get a pointer to the beginning of the data array
1518  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
1519 
1520  int pts = 0;
1521  // If the dataset is dense (no NaNs)
1522  if (cloud->is_dense)
1523  {
1524  for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
1525  memcpy (&data[pts], &cloud->points[i].x, 12); // sizeof (float) * 3
1526  }
1527  else
1528  {
1529  vtkIdType j = 0; // true point index
1530  for (vtkIdType i = 0; i < nr_points; ++i)
1531  {
1532  // Check if the point is invalid
1533  if (!isFinite (cloud->points[i]))
1534  continue;
1535 
1536  memcpy (&data[pts], &cloud->points[i].x, 12); // sizeof (float) * 3
1537  pts += 3;
1538  j++;
1539  }
1540  nr_points = j;
1541  points->SetNumberOfPoints (nr_points);
1542  }
1543 
1544  vtkSmartPointer<vtkIdTypeArray> cells = vertices->GetData ();
1545  updateCells (cells, am_it->second.cells, nr_points);
1546 
1547  // Set the cells and the vertices
1548  vertices->SetCells (nr_points, cells);
1549 
1550  // Get the colors from the handler
1552  color_handler.getColor (scalars);
1553  double minmax[2];
1554  scalars->GetRange (minmax);
1555  // Update the data
1556  polydata->GetPointData ()->SetScalars (scalars);
1557 
1558  am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1559  am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1560 
1561  // Update the mapper
1562 #if VTK_MAJOR_VERSION < 6
1563  reinterpret_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1564 #else
1565  reinterpret_cast<vtkPolyDataMapper*> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1566 #endif
1567  return (true);
1568 }
1569 
1570 /////////////////////////////////////////////////////////////////////////////////////////////
1571 template <typename PointT> bool
1573  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1574  const std::vector<pcl::Vertices> &vertices,
1575  const std::string &id,
1576  int viewport)
1577 {
1578  if (vertices.empty () || cloud->points.empty ())
1579  return (false);
1580 
1581  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1582  if (am_it != cloud_actor_map_->end ())
1583  {
1584  pcl::console::print_warn (stderr,
1585  "[addPolygonMesh] A shape with id <%s> already exists! Please choose a different id and retry.\n",
1586  id.c_str ());
1587  return (false);
1588  }
1589 
1590  int rgb_idx = -1;
1591  std::vector<pcl::PCLPointField> fields;
1593  rgb_idx = pcl::getFieldIndex (*cloud, "rgb", fields);
1594  if (rgb_idx == -1)
1595  rgb_idx = pcl::getFieldIndex (*cloud, "rgba", fields);
1596  if (rgb_idx != -1)
1597  {
1599  colors->SetNumberOfComponents (3);
1600  colors->SetName ("Colors");
1601 
1602  pcl::RGB rgb_data;
1603  for (size_t i = 0; i < cloud->size (); ++i)
1604  {
1605  if (!isFinite (cloud->points[i]))
1606  continue;
1607  memcpy (&rgb_data, reinterpret_cast<const char*> (&cloud->points[i]) + fields[rgb_idx].offset, sizeof (pcl::RGB));
1608  unsigned char color[3];
1609  color[0] = rgb_data.r;
1610  color[1] = rgb_data.g;
1611  color[2] = rgb_data.b;
1612  colors->InsertNextTupleValue (color);
1613  }
1614  }
1615 
1616  // Create points from polyMesh.cloud
1618  vtkIdType nr_points = cloud->points.size ();
1619  points->SetNumberOfPoints (nr_points);
1621 
1622  // Get a pointer to the beginning of the data array
1623  float *data = static_cast<vtkFloatArray*> (points->GetData ())->GetPointer (0);
1624 
1625  int ptr = 0;
1626  std::vector<int> lookup;
1627  // If the dataset is dense (no NaNs)
1628  if (cloud->is_dense)
1629  {
1630  for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1631  memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3);
1632  }
1633  else
1634  {
1635  lookup.resize (nr_points);
1636  vtkIdType j = 0; // true point index
1637  for (vtkIdType i = 0; i < nr_points; ++i)
1638  {
1639  // Check if the point is invalid
1640  if (!isFinite (cloud->points[i]))
1641  continue;
1642 
1643  lookup[i] = static_cast<int> (j);
1644  memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3);
1645  j++;
1646  ptr += 3;
1647  }
1648  nr_points = j;
1649  points->SetNumberOfPoints (nr_points);
1650  }
1651 
1652  // Get the maximum size of a polygon
1653  int max_size_of_polygon = -1;
1654  for (size_t i = 0; i < vertices.size (); ++i)
1655  if (max_size_of_polygon < static_cast<int> (vertices[i].vertices.size ()))
1656  max_size_of_polygon = static_cast<int> (vertices[i].vertices.size ());
1657 
1658  if (vertices.size () > 1)
1659  {
1660  // Create polys from polyMesh.polygons
1662  vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
1663  int idx = 0;
1664  if (lookup.size () > 0)
1665  {
1666  for (size_t i = 0; i < vertices.size (); ++i, ++idx)
1667  {
1668  size_t n_points = vertices[i].vertices.size ();
1669  *cell++ = n_points;
1670  //cell_array->InsertNextCell (n_points);
1671  for (size_t j = 0; j < n_points; j++, ++idx)
1672  *cell++ = lookup[vertices[i].vertices[j]];
1673  //cell_array->InsertCellPoint (lookup[vertices[i].vertices[j]]);
1674  }
1675  }
1676  else
1677  {
1678  for (size_t i = 0; i < vertices.size (); ++i, ++idx)
1679  {
1680  size_t n_points = vertices[i].vertices.size ();
1681  *cell++ = n_points;
1682  //cell_array->InsertNextCell (n_points);
1683  for (size_t j = 0; j < n_points; j++, ++idx)
1684  *cell++ = vertices[i].vertices[j];
1685  //cell_array->InsertCellPoint (vertices[i].vertices[j]);
1686  }
1687  }
1689  allocVtkPolyData (polydata);
1690  cell_array->GetData ()->SetNumberOfValues (idx);
1691  cell_array->Squeeze ();
1692  polydata->SetPolys (cell_array);
1693  polydata->SetPoints (points);
1694 
1695  if (colors)
1696  polydata->GetPointData ()->SetScalars (colors);
1697 
1698  createActorFromVTKDataSet (polydata, actor, false);
1699  }
1700  else
1701  {
1703  size_t n_points = vertices[0].vertices.size ();
1704  polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1705 
1706  if (lookup.size () > 0)
1707  {
1708  for (size_t j = 0; j < (n_points - 1); ++j)
1709  polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1710  }
1711  else
1712  {
1713  for (size_t j = 0; j < (n_points - 1); ++j)
1714  polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1715  }
1717  allocVtkUnstructuredGrid (poly_grid);
1718  poly_grid->Allocate (1, 1);
1719  poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1720  poly_grid->SetPoints (points);
1721  if (colors)
1722  poly_grid->GetPointData ()->SetScalars (colors);
1723 
1724  createActorFromVTKDataSet (poly_grid, actor, false);
1725  }
1726  addActorToRenderer (actor, viewport);
1727  actor->GetProperty ()->SetRepresentationToSurface ();
1728  // Backface culling renders the visualization slower, but guarantees that we see all triangles
1729  actor->GetProperty ()->BackfaceCullingOff ();
1730  actor->GetProperty ()->SetInterpolationToFlat ();
1731  actor->GetProperty ()->EdgeVisibilityOff ();
1732  actor->GetProperty ()->ShadingOff ();
1733 
1734  // Save the pointer/ID pair to the global actor map
1735  (*cloud_actor_map_)[id].actor = actor;
1736 
1737  // Save the viewpoint transformation matrix to the global actor map
1739  convertToVtkMatrix (cloud->sensor_origin_, cloud->sensor_orientation_, transformation);
1740  (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1741 
1742  return (true);
1743 }
1744 
1745 /////////////////////////////////////////////////////////////////////////////////////////////
1746 template <typename PointT> bool
1748  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1749  const std::vector<pcl::Vertices> &verts,
1750  const std::string &id)
1751 {
1752  if (verts.empty ())
1753  {
1754  pcl::console::print_error ("[addPolygonMesh] No vertices given!\n");
1755  return (false);
1756  }
1757 
1758  // Check to see if this ID entry already exists (has it been already added to the visualizer?)
1759  CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
1760  if (am_it == cloud_actor_map_->end ())
1761  return (false);
1762 
1763  // Get the current poly data
1764  vtkSmartPointer<vtkPolyData> polydata = static_cast<vtkPolyDataMapper*>(am_it->second.actor->GetMapper ())->GetInput ();
1765  if (!polydata)
1766  return (false);
1767  vtkSmartPointer<vtkCellArray> cells = polydata->GetStrips ();
1768  if (!cells)
1769  return (false);
1770  vtkSmartPointer<vtkPoints> points = polydata->GetPoints ();
1771  // Copy the new point array in
1772  vtkIdType nr_points = cloud->points.size ();
1773  points->SetNumberOfPoints (nr_points);
1774 
1775  // Get a pointer to the beginning of the data array
1776  float *data = (static_cast<vtkFloatArray*> (points->GetData ()))->GetPointer (0);
1777 
1778  int ptr = 0;
1779  std::vector<int> lookup;
1780  // If the dataset is dense (no NaNs)
1781  if (cloud->is_dense)
1782  {
1783  for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1784  memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3);
1785  }
1786  else
1787  {
1788  lookup.resize (nr_points);
1789  vtkIdType j = 0; // true point index
1790  for (vtkIdType i = 0; i < nr_points; ++i)
1791  {
1792  // Check if the point is invalid
1793  if (!isFinite (cloud->points[i]))
1794  continue;
1795 
1796  lookup [i] = static_cast<int> (j);
1797  memcpy (&data[ptr], &cloud->points[i].x, sizeof (float) * 3);
1798  j++;
1799  ptr += 3;
1800  }
1801  nr_points = j;
1802  points->SetNumberOfPoints (nr_points);
1803  }
1804 
1805  // Update colors
1806  vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1807  int rgb_idx = -1;
1808  std::vector<pcl::PCLPointField> fields;
1809  rgb_idx = pcl::getFieldIndex (*cloud, "rgb", fields);
1810  if (rgb_idx == -1)
1811  rgb_idx = pcl::getFieldIndex (*cloud, "rgba", fields);
1812  if (rgb_idx != -1 && colors)
1813  {
1814  pcl::RGB rgb_data;
1815  int j = 0;
1816  for (size_t i = 0; i < cloud->size (); ++i)
1817  {
1818  if (!isFinite (cloud->points[i]))
1819  continue;
1820  memcpy (&rgb_data,
1821  reinterpret_cast<const char*> (&cloud->points[i]) + fields[rgb_idx].offset,
1822  sizeof (pcl::RGB));
1823  unsigned char color[3];
1824  color[0] = rgb_data.r;
1825  color[1] = rgb_data.g;
1826  color[2] = rgb_data.b;
1827  colors->SetTupleValue (j++, color);
1828  }
1829  }
1830 
1831  // Get the maximum size of a polygon
1832  int max_size_of_polygon = -1;
1833  for (size_t i = 0; i < verts.size (); ++i)
1834  if (max_size_of_polygon < static_cast<int> (verts[i].vertices.size ()))
1835  max_size_of_polygon = static_cast<int> (verts[i].vertices.size ());
1836 
1837  // Update the cells
1839  vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
1840  int idx = 0;
1841  if (lookup.size () > 0)
1842  {
1843  for (size_t i = 0; i < verts.size (); ++i, ++idx)
1844  {
1845  size_t n_points = verts[i].vertices.size ();
1846  *cell++ = n_points;
1847  for (size_t j = 0; j < n_points; j++, cell++, ++idx)
1848  *cell = lookup[verts[i].vertices[j]];
1849  }
1850  }
1851  else
1852  {
1853  for (size_t i = 0; i < verts.size (); ++i, ++idx)
1854  {
1855  size_t n_points = verts[i].vertices.size ();
1856  *cell++ = n_points;
1857  for (size_t j = 0; j < n_points; j++, cell++, ++idx)
1858  *cell = verts[i].vertices[j];
1859  }
1860  }
1861  cells->GetData ()->SetNumberOfValues (idx);
1862  cells->Squeeze ();
1863  // Set the the vertices
1864  polydata->SetPolys (cells);
1865 
1866  return (true);
1867 }
1868 
1869 #ifdef vtkGenericDataArray_h
1870 #undef SetTupleValue
1871 #undef InsertNextTupleValue
1872 #undef GetTupleValue
1873 #endif
1874 
1875 #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.
Definition: point_tests.h:53
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
Definition: io.h:59
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")
Update the specified correspondences to the display.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
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.
size_t size() const
Definition: point_cloud.h:440
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 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 colors.
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.
PCL_EXPORTS void getRandomColors(double &r, double &g, double &b, double min=0.2, double max=2.8)
Get (good) random values for R/G/B.
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.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
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.
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.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
PCL_EXPORTS void allocVtkUnstructuredGrid(vtkSmartPointer< vtkUnstructuredGrid > &polydata)
Allocate a new unstructured grid smartpointer.
PCL_EXPORTS void print_warn(const char *format,...)
Print a warning message on stream with colors.
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.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
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 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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
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.