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