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
size_t size() const
Definition: point_cloud.h:440
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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
ColorHandler::ConstPtr ColorHandlerConstPtr
bool addPointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a Point Cloud (templated) to screen.
virtual void getGeometry(vtkSmartPointer< vtkPoints > &points) const =0
Obtain the actual point geometry for the input dataset in VTK format.
static void convertToVtkMatrix(const Eigen::Matrix4f &m, vtkSmartPointer< vtkMatrix4x4 > &vtk_matrix)
Convert Eigen::Matrix4f to vtkMatrix4x4.
std::vector< ColorHandlerConstPtr > color_handlers
A vector of color handlers that can be used for rendering the data.
Definition: actor_map.h:84
bool addPointCloudIntensityGradients(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const typename pcl::PointCloud< GradientT >::ConstPtr &gradients, int level=100, double scale=1e-6, const std::string &id="cloud", int viewport=0)
Add the estimated surface intensity gradients of a Point Cloud to screen.
bool 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.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
bool addSphere(const PointT &center, double radius, const std::string &id="sphere", int viewport=0)
Add a sphere shape from a point and a radius.
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.
bool addCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const std::vector< int > &correspondences, const std::string &id="correspondences", int viewport=0)
Add the specified correspondences to the display.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
Definition: point_cloud.h:421
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
vtkSmartPointer< vtkIdTypeArray > cells
Internal cell array.
Definition: actor_map.h:96
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
bool addPolygonMesh(const pcl::PolygonMesh &polymesh, const std::string &id="polygon", int viewport=0)
Add a PolygonMesh object to screen.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
Definition: point_cloud.h:423
vtkSmartPointer< vtkLODActor > actor
The actor holding the data to render.
Definition: actor_map.h:78
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
bool addArrow(const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id="arrow", int viewport=0)
Add a line arrow segment between two points, and display the distance between them.
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.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Definition: point_cloud.h:331
Base handler class for PointCloud geometry.
std::vector< GeometryHandlerConstPtr > geometry_handlers
A vector of geometry handlers that can be used for rendering the data.
Definition: actor_map.h:81
virtual std::string getName() const =0
Abstract getName method.
GeometryHandler::ConstPtr GeometryHandlerConstPtr
bool addLine(const P1 &pt1, const P2 &pt2, const std::string &id="line", int viewport=0)
Add a line segment from two points.
bool addText3D(const std::string &text, const PointT &position, double textScale=1.0, double r=1.0, double g=1.0, double b=1.0, const std::string &id="", int viewport=0)
Add a 3d text to the scene.
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual std::string getName() const =0
Abstract getName method.
bool updateSphere(const PointT &center, double radius, double r, double g, double b, const std::string &id="sphere")
Update an existing sphere shape from a point and a radius.
vtkSmartPointer< vtkMatrix4x4 > viewpoint_transformation_
The viewpoint transformation matrix.
Definition: actor_map.h:93
bool addPointCloudNormals(const typename pcl::PointCloud< PointNT >::ConstPtr &cloud, int level=100, float scale=0.02f, const std::string &id="cloud", int viewport=0)
Add the estimated surface normals of a Point Cloud to screen.
virtual bool getColor(vtkSmartPointer< vtkDataArray > &scalars) const =0
Obtain the actual color for the input dataset as vtk scalars.
PCL_EXPORTS vtkSmartPointer< vtkDataSet > createLine(const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
Create a line shape from two points.
bool updatePolygonMesh(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::vector< pcl::Vertices > &vertices, const std::string &id="polygon")
Update a PolygonMesh object on screen.
PCL_EXPORTS void print_error(const char *format,...)
Print an error message on stream with colors.