Point Cloud Library (PCL)  1.8.0
pcl_visualizer.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  */
38 #ifndef PCL_PCL_VISUALIZER_H_
39 #define PCL_PCL_VISUALIZER_H_
40 
41 // PCL includes
42 #include <pcl/correspondence.h>
43 #include <pcl/ModelCoefficients.h>
44 #include <pcl/PolygonMesh.h>
45 #include <pcl/TextureMesh.h>
46 //
47 #include <pcl/console/print.h>
48 #include <pcl/visualization/common/actor_map.h>
49 #include <pcl/visualization/common/common.h>
50 #include <pcl/visualization/point_cloud_geometry_handlers.h>
51 #include <pcl/visualization/point_cloud_color_handlers.h>
52 #include <pcl/visualization/point_picking_event.h>
53 #include <pcl/visualization/area_picking_event.h>
54 #include <pcl/visualization/interactor_style.h>
55 
56 // VTK includes
57 class vtkPolyData;
58 class vtkTextActor;
59 class vtkRenderWindow;
60 class vtkOrientationMarkerWidget;
61 class vtkAppendPolyData;
62 class vtkRenderWindow;
63 class vtkRenderWindowInteractor;
64 class vtkTransform;
65 class vtkInteractorStyle;
66 class vtkLODActor;
67 class vtkProp;
68 class vtkActor;
69 class vtkDataSet;
70 class vtkUnstructuredGrid;
71 
72 namespace pcl
73 {
74  template <typename T> class PointCloud;
75  template <typename T> class PlanarPolygon;
76 
77  namespace visualization
78  {
79  /** \brief PCL Visualizer main class.
80  * \author Radu B. Rusu
81  * \ingroup visualization
82  * \note This class can NOT be used across multiple threads. Only call functions of objects of this class
83  * from the same thread that they were created in! Some methods, e.g. addPointCloud, will crash if called
84  * from other threads.
85  */
86  class PCL_EXPORTS PCLVisualizer
87  {
88  public:
89  typedef boost::shared_ptr<PCLVisualizer> Ptr;
90  typedef boost::shared_ptr<const PCLVisualizer> ConstPtr;
91 
95 
99 
100  /** \brief PCL Visualizer constructor.
101  * \param[in] name the window name (empty by default)
102  * \param[in] create_interactor if true (default), create an interactor, false otherwise
103  */
104  PCLVisualizer (const std::string &name = "", const bool create_interactor = true);
105 
106  /** \brief PCL Visualizer constructor.
107  * \param[in] argc
108  * \param[in] argv
109  * \param[in] name the window name (empty by default)
110  * \param[in] style interactor style (defaults to PCLVisualizerInteractorStyle)
111  * \param[in] create_interactor if true (default), create an interactor, false otherwise
112  */
113  PCLVisualizer (int &argc, char **argv, const std::string &name = "",
114  PCLVisualizerInteractorStyle* style = PCLVisualizerInteractorStyle::New (), const bool create_interactor = true);
115 
116  /** \brief PCL Visualizer destructor. */
117  virtual ~PCLVisualizer ();
118 
119  /** \brief Enables/Disabled the underlying window mode to full screen.
120  * \note This might or might not work, depending on your window manager.
121  * See the VTK documentation for additional details.
122  * \param[in] mode true for full screen, false otherwise
123  */
124  void
125  setFullScreen (bool mode);
126 
127  /** \brief Set the visualizer window name.
128  * \param[in] name the name of the window
129  */
130  void
131  setWindowName (const std::string &name);
132 
133  /** \brief Enables or disable the underlying window borders.
134  * \note This might or might not work, depending on your window manager.
135  * See the VTK documentation for additional details.
136  * \param[in] mode true for borders, false otherwise
137  */
138  void
139  setWindowBorders (bool mode);
140 
141  /** \brief Register a callback boost::function for keyboard events
142  * \param[in] cb a boost function that will be registered as a callback for a keyboard event
143  * \return a connection object that allows to disconnect the callback function.
144  */
145  boost::signals2::connection
146  registerKeyboardCallback (boost::function<void (const pcl::visualization::KeyboardEvent&)> cb);
147 
148  /** \brief Register a callback function for keyboard events
149  * \param[in] callback the function that will be registered as a callback for a keyboard event
150  * \param[in] cookie user data that is passed to the callback
151  * \return a connection object that allows to disconnect the callback function.
152  */
153  inline boost::signals2::connection
154  registerKeyboardCallback (void (*callback) (const pcl::visualization::KeyboardEvent&, void*), void* cookie = NULL)
155  {
156  return (registerKeyboardCallback (boost::bind (callback, _1, cookie)));
157  }
158 
159  /** \brief Register a callback function for keyboard events
160  * \param[in] callback the member function that will be registered as a callback for a keyboard event
161  * \param[in] instance instance to the class that implements the callback function
162  * \param[in] cookie user data that is passed to the callback
163  * \return a connection object that allows to disconnect the callback function.
164  */
165  template<typename T> inline boost::signals2::connection
166  registerKeyboardCallback (void (T::*callback) (const pcl::visualization::KeyboardEvent&, void*), T& instance, void* cookie = NULL)
167  {
168  return (registerKeyboardCallback (boost::bind (callback, boost::ref (instance), _1, cookie)));
169  }
170 
171  /** \brief Register a callback function for mouse events
172  * \param[in] cb a boost function that will be registered as a callback for a mouse event
173  * \return a connection object that allows to disconnect the callback function.
174  */
175  boost::signals2::connection
176  registerMouseCallback (boost::function<void (const pcl::visualization::MouseEvent&)> cb);
177 
178  /** \brief Register a callback function for mouse events
179  * \param[in] callback the function that will be registered as a callback for a mouse event
180  * \param[in] cookie user data that is passed to the callback
181  * \return a connection object that allows to disconnect the callback function.
182  */
183  inline boost::signals2::connection
184  registerMouseCallback (void (*callback) (const pcl::visualization::MouseEvent&, void*), void* cookie = NULL)
185  {
186  return (registerMouseCallback (boost::bind (callback, _1, cookie)));
187  }
188 
189  /** \brief Register a callback function for mouse events
190  * \param[in] callback the member function that will be registered as a callback for a mouse event
191  * \param[in] instance instance to the class that implements the callback function
192  * \param[in] cookie user data that is passed to the callback
193  * \return a connection object that allows to disconnect the callback function.
194  */
195  template<typename T> inline boost::signals2::connection
196  registerMouseCallback (void (T::*callback) (const pcl::visualization::MouseEvent&, void*), T& instance, void* cookie = NULL)
197  {
198  return (registerMouseCallback (boost::bind (callback, boost::ref (instance), _1, cookie)));
199  }
200 
201  /** \brief Register a callback function for point picking events
202  * \param[in] cb a boost function that will be registered as a callback for a point picking event
203  * \return a connection object that allows to disconnect the callback function.
204  */
205  boost::signals2::connection
206  registerPointPickingCallback (boost::function<void (const pcl::visualization::PointPickingEvent&)> cb);
207 
208  /** \brief Register a callback function for point picking events
209  * \param[in] callback the function that will be registered as a callback for a point picking event
210  * \param[in] cookie user data that is passed to the callback
211  * \return a connection object that allows to disconnect the callback function.
212  */
213  boost::signals2::connection
214  registerPointPickingCallback (void (*callback) (const pcl::visualization::PointPickingEvent&, void*), void* cookie = NULL);
215 
216  /** \brief Register a callback function for point picking events
217  * \param[in] callback the member function that will be registered as a callback for a point picking event
218  * \param[in] instance instance to the class that implements the callback function
219  * \param[in] cookie user data that is passed to the callback
220  * \return a connection object that allows to disconnect the callback function.
221  */
222  template<typename T> inline boost::signals2::connection
223  registerPointPickingCallback (void (T::*callback) (const pcl::visualization::PointPickingEvent&, void*), T& instance, void* cookie = NULL)
224  {
225  return (registerPointPickingCallback (boost::bind (callback, boost::ref (instance), _1, cookie)));
226  }
227 
228  /** \brief Register a callback function for area picking events
229  * \param[in] cb a boost function that will be registered as a callback for an area picking event
230  * \return a connection object that allows to disconnect the callback function.
231  */
232  boost::signals2::connection
233  registerAreaPickingCallback (boost::function<void (const pcl::visualization::AreaPickingEvent&)> cb);
234 
235  /** \brief Register a callback function for area picking events
236  * \param[in] callback the function that will be registered as a callback for an area picking event
237  * \param[in] cookie user data that is passed to the callback
238  * \return a connection object that allows to disconnect the callback function.
239  */
240  boost::signals2::connection
241  registerAreaPickingCallback (void (*callback) (const pcl::visualization::AreaPickingEvent&, void*), void* cookie = NULL);
242 
243  /** \brief Register a callback function for area picking events
244  * \param[in] callback the member function that will be registered as a callback for an area picking event
245  * \param[in] instance instance to the class that implements the callback function
246  * \param[in] cookie user data that is passed to the callback
247  * \return a connection object that allows to disconnect the callback function.
248  */
249  template<typename T> inline boost::signals2::connection
250  registerAreaPickingCallback (void (T::*callback) (const pcl::visualization::AreaPickingEvent&, void*), T& instance, void* cookie = NULL)
251  {
252  return (registerAreaPickingCallback (boost::bind (callback, boost::ref (instance), _1, cookie)));
253  }
254 
255  /** \brief Spin method. Calls the interactor and runs an internal loop. */
256  void
257  spin ();
258 
259  /** \brief Spin once method. Calls the interactor and updates the screen once.
260  * \param[in] time - How long (in ms) should the visualization loop be allowed to run.
261  * \param[in] force_redraw - if false it might return without doing anything if the
262  * interactor's framerate does not require a redraw yet.
263  */
264  void
265  spinOnce (int time = 1, bool force_redraw = false);
266 
267  /** \brief Adds a widget which shows an interactive axes display for orientation
268  * \param[in] interactor - Pointer to the vtk interactor object used by the PCLVisualizer window
269  */
270  void
271  addOrientationMarkerWidgetAxes (vtkRenderWindowInteractor* interactor);
272 
273  /** \brief Disables the Orientatation Marker Widget so it is removed from the renderer */
274  void
275  removeOrientationMarkerWidgetAxes ();
276 
277  /** \brief Adds 3D axes describing a coordinate system to screen at 0,0,0.
278  * \param[in] scale the scale of the axes (default: 1)
279  * \param[in] viewport the view port where the 3D axes should be added (default: all)
280  */
281  PCL_DEPRECATED (
282  "addCoordinateSystem (scale, viewport) is deprecated, please use function "
283  "addCoordinateSystem (scale, id, viewport) with id a unique string identifier.")
284  void
285  addCoordinateSystem (double scale, int viewport);
286 
287  /** \brief Adds 3D axes describing a coordinate system to screen at 0,0,0.
288  * \param[in] scale the scale of the axes (default: 1)
289  * \param[in] id the coordinate system object id (default: reference)
290  * \param[in] viewport the view port where the 3D axes should be added (default: all)
291  */
292  void
293  addCoordinateSystem (double scale = 1.0, const std::string& id = "reference", int viewport = 0);
294 
295  /** \brief Adds 3D axes describing a coordinate system to screen at x, y, z
296  * \param[in] scale the scale of the axes (default: 1)
297  * \param[in] x the X position of the axes
298  * \param[in] y the Y position of the axes
299  * \param[in] z the Z position of the axes
300  * \param[in] viewport the view port where the 3D axes should be added (default: all)
301  */
302  PCL_DEPRECATED (
303  "addCoordinateSystem (scale, x, y, z, viewport) is deprecated, please use function "
304  "addCoordinateSystem (scale, x, y, z, id, viewport) with id a unique string identifier.")
305  void
306  addCoordinateSystem (double scale, float x, float y, float z, int viewport);
307 
308  /** \brief Adds 3D axes describing a coordinate system to screen at x, y, z
309  * \param[in] scale the scale of the axes (default: 1)
310  * \param[in] x the X position of the axes
311  * \param[in] y the Y position of the axes
312  * \param[in] z the Z position of the axes
313  * \param[in] id the coordinate system object id (default: reference)
314  * \param[in] viewport the view port where the 3D axes should be added (default: all)
315  */
316  void
317  addCoordinateSystem (double scale, float x, float y, float z, const std::string &id = "reference", int viewport = 0);
318 
319  /** \brief Adds 3D axes describing a coordinate system to screen at x, y, z, Roll,Pitch,Yaw
320  *
321  * \param[in] scale the scale of the axes (default: 1)
322  * \param[in] t transformation matrix
323  * \param[in] viewport the view port where the 3D axes should be added (default: all)
324  */
325  PCL_DEPRECATED (
326  "addCoordinateSystem (scale, t, viewport) is deprecated, please use function "
327  "addCoordinateSystem (scale, t, id, viewport) with id a unique string identifier.")
328  void
329  addCoordinateSystem (double scale, const Eigen::Affine3f& t, int viewport);
330 
331  /** \brief Adds 3D axes describing a coordinate system to screen at x, y, z, Roll,Pitch,Yaw
332  *
333  * \param[in] scale the scale of the axes (default: 1)
334  * \param[in] t transformation matrix
335  * \param[in] id the coordinate system object id (default: reference)
336  * \param[in] viewport the view port where the 3D axes should be added (default: all)
337  *
338  * RPY Angles
339  * Rotate the reference frame by the angle roll about axis x
340  * Rotate the reference frame by the angle pitch about axis y
341  * Rotate the reference frame by the angle yaw about axis z
342  *
343  * Description:
344  * Sets the orientation of the Prop3D. Orientation is specified as
345  * X,Y and Z rotations in that order, but they are performed as
346  * RotateZ, RotateX, and finally RotateY.
347  *
348  * All axies use right hand rule. x=red axis, y=green axis, z=blue axis
349  * z direction is point into the screen.
350  * \code
351  * z
352  * \
353  * \
354  * \
355  * -----------> x
356  * |
357  * |
358  * |
359  * |
360  * |
361  * |
362  * y
363  * \endcode
364  */
365 
366  void
367  addCoordinateSystem (double scale, const Eigen::Affine3f& t, const std::string &id = "reference", int viewport = 0);
368 
369  /** \brief Removes a previously added 3D axes (coordinate system)
370  * \param[in] viewport view port where the 3D axes should be removed from (default: all)
371  */
372  PCL_DEPRECATED (
373  "removeCoordinateSystem (viewport) is deprecated, please use function "
374  "addCoordinateSystem (id, viewport) with id a unique string identifier.")
375  bool
376  removeCoordinateSystem (int viewport);
377 
378  /** \brief Removes a previously added 3D axes (coordinate system)
379  * \param[in] id the coordinate system object id (default: reference)
380  * \param[in] viewport view port where the 3D axes should be removed from (default: all)
381  */
382  bool
383  removeCoordinateSystem (const std::string &id = "reference", int viewport = 0);
384 
385  /** \brief Removes a Point Cloud from screen, based on a given ID.
386  * \param[in] id the point cloud object id (i.e., given on \a addPointCloud)
387  * \param[in] viewport view port from where the Point Cloud should be removed (default: all)
388  * \return true if the point cloud is successfully removed and false if the point cloud is
389  * not actually displayed
390  */
391  bool
392  removePointCloud (const std::string &id = "cloud", int viewport = 0);
393 
394  /** \brief Removes a PolygonMesh from screen, based on a given ID.
395  * \param[in] id the polygon object id (i.e., given on \a addPolygonMesh)
396  * \param[in] viewport view port from where the PolygonMesh should be removed (default: all)
397  */
398  inline bool
399  removePolygonMesh (const std::string &id = "polygon", int viewport = 0)
400  {
401  // Polygon Meshes are represented internally as point clouds with special cell array structures since 1.4
402  return (removePointCloud (id, viewport));
403  }
404 
405  /** \brief Removes an added shape from screen (line, polygon, etc.), based on a given ID
406  * \note This methods also removes PolygonMesh objects and PointClouds, if they match the ID
407  * \param[in] id the shape object id (i.e., given on \a addLine etc.)
408  * \param[in] viewport view port from where the Point Cloud should be removed (default: all)
409  */
410  bool
411  removeShape (const std::string &id = "cloud", int viewport = 0);
412 
413  /** \brief Removes an added 3D text from the scene, based on a given ID
414  * \param[in] id the 3D text id (i.e., given on \a addText3D etc.)
415  * \param[in] viewport view port from where the 3D text should be removed (default: all)
416  */
417  bool
418  removeText3D (const std::string &id = "cloud", int viewport = 0);
419 
420  /** \brief Remove all point cloud data on screen from the given viewport.
421  * \param[in] viewport view port from where the clouds should be removed (default: all)
422  */
423  bool
424  removeAllPointClouds (int viewport = 0);
425 
426  /** \brief Remove all 3D shape data on screen from the given viewport.
427  * \param[in] viewport view port from where the shapes should be removed (default: all)
428  */
429  bool
430  removeAllShapes (int viewport = 0);
431 
432  /** \brief Removes all existing 3D axes (coordinate systems)
433  * \param[in] viewport view port where the 3D axes should be removed from (default: all)
434  */
435  bool
436  removeAllCoordinateSystems (int viewport = 0);
437 
438  /** \brief Set the viewport's background color.
439  * \param[in] r the red component of the RGB color
440  * \param[in] g the green component of the RGB color
441  * \param[in] b the blue component of the RGB color
442  * \param[in] viewport the view port (default: all)
443  */
444  void
445  setBackgroundColor (const double &r, const double &g, const double &b, int viewport = 0);
446 
447  /** \brief Add a text to screen
448  * \param[in] text the text to add
449  * \param[in] xpos the X position on screen where the text should be added
450  * \param[in] ypos the Y position on screen where the text should be added
451  * \param[in] id the text object id (default: equal to the "text" parameter)
452  * \param[in] viewport the view port (default: all)
453  */
454  bool
455  addText (const std::string &text,
456  int xpos, int ypos,
457  const std::string &id = "", int viewport = 0);
458 
459  /** \brief Add a text to screen
460  * \param[in] text the text to add
461  * \param[in] xpos the X position on screen where the text should be added
462  * \param[in] ypos the Y position on screen where the text should be added
463  * \param[in] r the red color value
464  * \param[in] g the green color value
465  * \param[in] b the blue color vlaue
466  * \param[in] id the text object id (default: equal to the "text" parameter)
467  * \param[in] viewport the view port (default: all)
468  */
469  bool
470  addText (const std::string &text, int xpos, int ypos, double r, double g, double b,
471  const std::string &id = "", int viewport = 0);
472 
473  /** \brief Add a text to screen
474  * \param[in] text the text to add
475  * \param[in] xpos the X position on screen where the text should be added
476  * \param[in] ypos the Y position on screen where the text should be added
477  * \param[in] fontsize the fontsize of the text
478  * \param[in] r the red color value
479  * \param[in] g the green color value
480  * \param[in] b the blue color vlaue
481  * \param[in] id the text object id (default: equal to the "text" parameter)
482  * \param[in] viewport the view port (default: all)
483  */
484  bool
485  addText (const std::string &text, int xpos, int ypos, int fontsize, double r, double g, double b,
486  const std::string &id = "", int viewport = 0);
487 
488 
489  /** \brief Update a text to screen
490  * \param[in] text the text to update
491  * \param[in] xpos the new X position on screen
492  * \param[in] ypos the new Y position on screen
493  * \param[in] id the text object id (default: equal to the "text" parameter)
494  */
495  bool
496  updateText (const std::string &text,
497  int xpos, int ypos,
498  const std::string &id = "");
499 
500  /** \brief Update a text to screen
501  * \param[in] text the text to update
502  * \param[in] xpos the new X position on screen
503  * \param[in] ypos the new Y position on screen
504  * \param[in] r the red color value
505  * \param[in] g the green color value
506  * \param[in] b the blue color vlaue
507  * \param[in] id the text object id (default: equal to the "text" parameter)
508  */
509  bool
510  updateText (const std::string &text,
511  int xpos, int ypos, double r, double g, double b,
512  const std::string &id = "");
513 
514  /** \brief Update a text to screen
515  * \param[in] text the text to update
516  * \param[in] xpos the new X position on screen
517  * \param[in] ypos the new Y position on screen
518  * \param[in] fontsize the fontsize of the text
519  * \param[in] r the red color value
520  * \param[in] g the green color value
521  * \param[in] b the blue color vlaue
522  * \param[in] id the text object id (default: equal to the "text" parameter)
523  */
524  bool
525  updateText (const std::string &text,
526  int xpos, int ypos, int fontsize, double r, double g, double b,
527  const std::string &id = "");
528 
529  /** \brief Set the pose of an existing shape.
530  *
531  * Returns false if the shape doesn't exist, true if the pose was successfully
532  * updated.
533  *
534  * \param[in] id the shape or cloud object id (i.e., given on \a addLine etc.)
535  * \param[in] pose the new pose
536  * \return false if no shape or cloud with the specified ID was found
537  */
538  bool
539  updateShapePose (const std::string &id, const Eigen::Affine3f& pose);
540 
541  /** \brief Set the pose of an existing coordinate system.
542  *
543  * Returns false if the coordinate system doesn't exist, true if the pose was successfully
544  * updated.
545  *
546  * \param[in] id the point cloud object id (i.e., given on \a addCoordinateSystem etc.)
547  * \param[in] pose the new pose
548  * \return false if no coordinate system with the specified ID was found
549  */
550  bool
551  updateCoordinateSystemPose (const std::string &id, const Eigen::Affine3f& pose);
552 
553  /** \brief Set the pose of an existing point cloud.
554  *
555  * Returns false if the point cloud doesn't exist, true if the pose was successfully
556  * updated.
557  *
558  * \param[in] id the point cloud object id (i.e., given on \a addPointCloud etc.)
559  * \param[in] pose the new pose
560  * \return false if no point cloud with the specified ID was found
561  */
562  bool
563  updatePointCloudPose (const std::string &id, const Eigen::Affine3f& pose);
564 
565  /** \brief Add a 3d text to the scene
566  * \param[in] text the text to add
567  * \param[in] position the world position where the text should be added
568  * \param[in] textScale the scale of the text to render
569  * \param[in] r the red color value
570  * \param[in] g the green color value
571  * \param[in] b the blue color value
572  * \param[in] id the text object id (default: equal to the "text" parameter)
573  * \param[in] viewport the view port (default: all)
574  */
575  template <typename PointT> bool
576  addText3D (const std::string &text,
577  const PointT &position,
578  double textScale = 1.0,
579  double r = 1.0, double g = 1.0, double b = 1.0,
580  const std::string &id = "", int viewport = 0);
581 
582  /** \brief Check if the cloud, shape, or coordinate with the given id was already added to this vizualizer.
583  * \param[in] id the id of the cloud, shape, or coordinate to check
584  * \return true if a cloud, shape, or coordinate with the specified id was found
585  */
586  inline bool
587  contains(const std::string &id) const
588  {
589  return (cloud_actor_map_->find (id) != cloud_actor_map_->end () ||
590  shape_actor_map_->find (id) != shape_actor_map_->end () ||
591  coordinate_actor_map_->find (id) != coordinate_actor_map_-> end());
592  }
593 
594  /** \brief Add the estimated surface normals of a Point Cloud to screen.
595  * \param[in] cloud the input point cloud dataset containing XYZ data and normals
596  * \param[in] level display only every level'th point (default: 100)
597  * \param[in] scale the normal arrow scale (default: 0.02m)
598  * \param[in] id the point cloud object id (default: cloud)
599  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
600  */
601  template <typename PointNT> bool
602  addPointCloudNormals (const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
603  int level = 100, float scale = 0.02f,
604  const std::string &id = "cloud", int viewport = 0);
605 
606  /** \brief Add the estimated surface normals of a Point Cloud to screen.
607  * \param[in] cloud the input point cloud dataset containing the XYZ data
608  * \param[in] normals the input point cloud dataset containing the normal data
609  * \param[in] level display only every level'th point (default: 100)
610  * \param[in] scale the normal arrow scale (default: 0.02m)
611  * \param[in] id the point cloud object id (default: cloud)
612  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
613  */
614  template <typename PointT, typename PointNT> bool
615  addPointCloudNormals (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
616  const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
617  int level = 100, float scale = 0.02f,
618  const std::string &id = "cloud", int viewport = 0);
619 
620  /** \brief Add the estimated principal curvatures of a Point Cloud to screen.
621  * \param[in] cloud the input point cloud dataset containing the XYZ data and normals
622  * \param[in] pcs the input point cloud dataset containing the principal curvatures data
623  * \param[in] level display only every level'th point. Default: 100
624  * \param[in] scale the normal arrow scale. Default: 1.0
625  * \param[in] id the point cloud object id. Default: "cloud"
626  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
627  */
628  template <typename PointNT> bool
629  addPointCloudPrincipalCurvatures (
630  const typename pcl::PointCloud<PointNT>::ConstPtr &cloud,
632  int level = 100, float scale = 1.0f,
633  const std::string &id = "cloud", int viewport = 0);
634 
635  /** \brief Add the estimated principal curvatures of a Point Cloud to screen.
636  * \param[in] cloud the input point cloud dataset containing the XYZ data
637  * \param[in] normals the input point cloud dataset containing the normal data
638  * \param[in] pcs the input point cloud dataset containing the principal curvatures data
639  * \param[in] level display only every level'th point. Default: 100
640  * \param[in] scale the normal arrow scale. Default: 1.0
641  * \param[in] id the point cloud object id. Default: "cloud"
642  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
643  */
644  template <typename PointT, typename PointNT> bool
645  addPointCloudPrincipalCurvatures (
646  const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
647  const typename pcl::PointCloud<PointNT>::ConstPtr &normals,
649  int level = 100, float scale = 1.0f,
650  const std::string &id = "cloud", int viewport = 0);
651 
652  /** \brief Add the estimated surface intensity gradients of a Point Cloud to screen.
653  * \param[in] cloud the input point cloud dataset containing the XYZ data
654  * \param[in] gradients the input point cloud dataset containing the intensity gradient data
655  * \param[in] level display only every level'th point (default: 100)
656  * \param[in] scale the intensity gradient arrow scale (default: 1e-6m)
657  * \param[in] id the point cloud object id (default: cloud)
658  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
659  */
660  template <typename PointT, typename GradientT> bool
661  addPointCloudIntensityGradients (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
662  const typename pcl::PointCloud<GradientT>::ConstPtr &gradients,
663  int level = 100, double scale = 1e-6,
664  const std::string &id = "cloud", int viewport = 0);
665 
666  /** \brief Add a Point Cloud (templated) to screen.
667  * \param[in] cloud the input point cloud dataset
668  * \param[in] id the point cloud object id (default: cloud)
669  * \param viewport the view port where the Point Cloud should be added (default: all)
670  */
671  template <typename PointT> bool
672  addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
673  const std::string &id = "cloud", int viewport = 0);
674 
675  /** \brief Updates the XYZ data for an existing cloud object id on screen.
676  * \param[in] cloud the input point cloud dataset
677  * \param[in] id the point cloud object id to update (default: cloud)
678  * \return false if no cloud with the specified ID was found
679  */
680  template <typename PointT> bool
681  updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
682  const std::string &id = "cloud");
683 
684  /** \brief Updates the XYZ data for an existing cloud object id on screen.
685  * \param[in] cloud the input point cloud dataset
686  * \param[in] geometry_handler the geometry handler to use
687  * \param[in] id the point cloud object id to update (default: cloud)
688  * \return false if no cloud with the specified ID was found
689  */
690  template <typename PointT> bool
691  updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
692  const PointCloudGeometryHandler<PointT> &geometry_handler,
693  const std::string &id = "cloud");
694 
695  /** \brief Updates the XYZ data for an existing cloud object id on screen.
696  * \param[in] cloud the input point cloud dataset
697  * \param[in] color_handler the color handler to use
698  * \param[in] id the point cloud object id to update (default: cloud)
699  * \return false if no cloud with the specified ID was found
700  */
701  template <typename PointT> bool
702  updatePointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
703  const PointCloudColorHandler<PointT> &color_handler,
704  const std::string &id = "cloud");
705 
706  /** \brief Add a Point Cloud (templated) to screen.
707  * \param[in] cloud the input point cloud dataset
708  * \param[in] geometry_handler use a geometry handler object to extract the XYZ data
709  * \param[in] id the point cloud object id (default: cloud)
710  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
711  */
712  template <typename PointT> bool
713  addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
714  const PointCloudGeometryHandler<PointT> &geometry_handler,
715  const std::string &id = "cloud", int viewport = 0);
716 
717  /** \brief Add a Point Cloud (templated) to screen.
718  *
719  * Because the geometry handler is given as a pointer, it will be pushed back to the list of available
720  * handlers, rather than replacing the current active geometric handler. This makes it possible to
721  * switch between different geometric handlers 'on-the-fly' at runtime, from the PCLVisualizer
722  * interactor interface (using Alt+0..9).
723  *
724  * \param[in] cloud the input point cloud dataset
725  * \param[in] geometry_handler use a geometry handler object to extract the XYZ data
726  * \param[in] id the point cloud object id (default: cloud)
727  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
728  */
729  template <typename PointT> bool
730  addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
731  const GeometryHandlerConstPtr &geometry_handler,
732  const std::string &id = "cloud", int viewport = 0);
733 
734  /** \brief Add a Point Cloud (templated) to screen.
735  * \param[in] cloud the input point cloud dataset
736  * \param[in] color_handler a specific PointCloud visualizer handler for colors
737  * \param[in] id the point cloud object id (default: cloud)
738  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
739  */
740  template <typename PointT> bool
741  addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
742  const PointCloudColorHandler<PointT> &color_handler,
743  const std::string &id = "cloud", int viewport = 0);
744 
745  /** \brief Add a Point Cloud (templated) to screen.
746  *
747  * Because the color handler is given as a pointer, it will be pushed back to the list of available
748  * handlers, rather than replacing the current active color handler. This makes it possible to
749  * switch between different color handlers 'on-the-fly' at runtime, from the PCLVisualizer
750  * interactor interface (using 0..9).
751  *
752  * \param[in] cloud the input point cloud dataset
753  * \param[in] color_handler a specific PointCloud visualizer handler for colors
754  * \param[in] id the point cloud object id (default: cloud)
755  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
756  */
757  template <typename PointT> bool
758  addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
759  const ColorHandlerConstPtr &color_handler,
760  const std::string &id = "cloud", int viewport = 0);
761 
762  /** \brief Add a Point Cloud (templated) to screen.
763  *
764  * Because the geometry/color handler is given as a pointer, it will be pushed back to the list of
765  * available handlers, rather than replacing the current active handler. This makes it possible to
766  * switch between different handlers 'on-the-fly' at runtime, from the PCLVisualizer interactor
767  * interface (using [Alt+]0..9).
768  *
769  * \param[in] cloud the input point cloud dataset
770  * \param[in] geometry_handler a specific PointCloud visualizer handler for geometry
771  * \param[in] color_handler a specific PointCloud visualizer handler for colors
772  * \param[in] id the point cloud object id (default: cloud)
773  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
774  */
775  template <typename PointT> bool
776  addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
777  const GeometryHandlerConstPtr &geometry_handler,
778  const ColorHandlerConstPtr &color_handler,
779  const std::string &id = "cloud", int viewport = 0);
780 
781  /** \brief Add a binary blob Point Cloud to screen.
782  *
783  * Because the geometry/color handler is given as a pointer, it will be pushed back to the list of
784  * available handlers, rather than replacing the current active handler. This makes it possible to
785  * switch between different handlers 'on-the-fly' at runtime, from the PCLVisualizer interactor
786  * interface (using [Alt+]0..9).
787  *
788  * \param[in] cloud the input point cloud dataset
789  * \param[in] geometry_handler a specific PointCloud visualizer handler for geometry
790  * \param[in] color_handler a specific PointCloud visualizer handler for colors
791  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
792  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
793  * \param[in] id the point cloud object id (default: cloud)
794  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
795  */
796  bool
797  addPointCloud (const pcl::PCLPointCloud2::ConstPtr &cloud,
798  const GeometryHandlerConstPtr &geometry_handler,
799  const ColorHandlerConstPtr &color_handler,
800  const Eigen::Vector4f& sensor_origin,
801  const Eigen::Quaternion<float>& sensor_orientation,
802  const std::string &id = "cloud", int viewport = 0);
803 
804  /** \brief Add a binary blob Point Cloud to screen.
805  *
806  * Because the geometry/color handler is given as a pointer, it will be pushed back to the list of
807  * available handlers, rather than replacing the current active handler. This makes it possible to
808  * switch between different handlers 'on-the-fly' at runtime, from the PCLVisualizer interactor
809  * interface (using [Alt+]0..9).
810  *
811  * \param[in] cloud the input point cloud dataset
812  * \param[in] geometry_handler a specific PointCloud visualizer handler for geometry
813  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
814  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
815  * \param[in] id the point cloud object id (default: cloud)
816  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
817  */
818  bool
819  addPointCloud (const pcl::PCLPointCloud2::ConstPtr &cloud,
820  const GeometryHandlerConstPtr &geometry_handler,
821  const Eigen::Vector4f& sensor_origin,
822  const Eigen::Quaternion<float>& sensor_orientation,
823  const std::string &id = "cloud", int viewport = 0);
824 
825  /** \brief Add a binary blob Point Cloud to screen.
826  *
827  * Because the geometry/color handler is given as a pointer, it will be pushed back to the list of
828  * available handlers, rather than replacing the current active handler. This makes it possible to
829  * switch between different handlers 'on-the-fly' at runtime, from the PCLVisualizer interactor
830  * interface (using [Alt+]0..9).
831  *
832  * \param[in] cloud the input point cloud dataset
833  * \param[in] color_handler a specific PointCloud visualizer handler for colors
834  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
835  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
836  * \param[in] id the point cloud object id (default: cloud)
837  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
838  */
839  bool
840  addPointCloud (const pcl::PCLPointCloud2::ConstPtr &cloud,
841  const ColorHandlerConstPtr &color_handler,
842  const Eigen::Vector4f& sensor_origin,
843  const Eigen::Quaternion<float>& sensor_orientation,
844  const std::string &id = "cloud", int viewport = 0);
845 
846  /** \brief Add a Point Cloud (templated) to screen.
847  * \param[in] cloud the input point cloud dataset
848  * \param[in] color_handler a specific PointCloud visualizer handler for colors
849  * \param[in] geometry_handler use a geometry handler object to extract the XYZ data
850  * \param[in] id the point cloud object id (default: cloud)
851  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
852  */
853  template <typename PointT> bool
854  addPointCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
855  const PointCloudColorHandler<PointT> &color_handler,
856  const PointCloudGeometryHandler<PointT> &geometry_handler,
857  const std::string &id = "cloud", int viewport = 0);
858 
859  /** \brief Add a PointXYZ Point Cloud to screen.
860  * \param[in] cloud the input point cloud dataset
861  * \param[in] id the point cloud object id (default: cloud)
862  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
863  */
864  inline bool
866  const std::string &id = "cloud", int viewport = 0)
867  {
868  return (addPointCloud<pcl::PointXYZ> (cloud, id, viewport));
869  }
870 
871 
872  /** \brief Add a PointXYZRGB Point Cloud to screen.
873  * \param[in] cloud the input point cloud dataset
874  * \param[in] id the point cloud object id (default: cloud)
875  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
876  */
877  inline bool
879  const std::string &id = "cloud", int viewport = 0)
880  {
882  return (addPointCloud<pcl::PointXYZRGB> (cloud, color_handler, id, viewport));
883  }
884 
885  /** \brief Add a PointXYZRGBA Point Cloud to screen.
886  * \param[in] cloud the input point cloud dataset
887  * \param[in] id the point cloud object id (default: cloud)
888  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
889  */
890  inline bool
892  const std::string &id = "cloud", int viewport = 0)
893  {
895  return (addPointCloud<pcl::PointXYZRGBA> (cloud, color_handler, id, viewport));
896  }
897 
898  /** \brief Add a PointXYZL Point Cloud to screen.
899  * \param[in] cloud the input point cloud dataset
900  * \param[in] id the point cloud object id (default: cloud)
901  * \param[in] viewport the view port where the Point Cloud should be added (default: all)
902  */
903  inline bool
905  const std::string &id = "cloud", int viewport = 0)
906  {
908  return (addPointCloud<pcl::PointXYZL> (cloud, color_handler, id, viewport));
909  }
910 
911  /** \brief Updates the XYZ data for an existing cloud object id on screen.
912  * \param[in] cloud the input point cloud dataset
913  * \param[in] id the point cloud object id to update (default: cloud)
914  * \return false if no cloud with the specified ID was found
915  */
916  inline bool
918  const std::string &id = "cloud")
919  {
920  return (updatePointCloud<pcl::PointXYZ> (cloud, id));
921  }
922 
923  /** \brief Updates the XYZRGB data for an existing cloud object id on screen.
924  * \param[in] cloud the input point cloud dataset
925  * \param[in] id the point cloud object id to update (default: cloud)
926  * \return false if no cloud with the specified ID was found
927  */
928  inline bool
930  const std::string &id = "cloud")
931  {
933  return (updatePointCloud<pcl::PointXYZRGB> (cloud, color_handler, id));
934  }
935 
936  /** \brief Updates the XYZRGBA data for an existing cloud object id on screen.
937  * \param[in] cloud the input point cloud dataset
938  * \param[in] id the point cloud object id to update (default: cloud)
939  * \return false if no cloud with the specified ID was found
940  */
941  inline bool
943  const std::string &id = "cloud")
944  {
946  return (updatePointCloud<pcl::PointXYZRGBA> (cloud, color_handler, id));
947  }
948 
949  /** \brief Updates the XYZL data for an existing cloud object id on screen.
950  * \param[in] cloud the input point cloud dataset
951  * \param[in] id the point cloud object id to update (default: cloud)
952  * \return false if no cloud with the specified ID was found
953  */
954  inline bool
956  const std::string &id = "cloud")
957  {
959  return (updatePointCloud<pcl::PointXYZL> (cloud, color_handler, id));
960  }
961 
962  /** \brief Add a PolygonMesh object to screen
963  * \param[in] polymesh the polygonal mesh
964  * \param[in] id the polygon object id (default: "polygon")
965  * \param[in] viewport the view port where the PolygonMesh should be added (default: all)
966  */
967  bool
968  addPolygonMesh (const pcl::PolygonMesh &polymesh,
969  const std::string &id = "polygon",
970  int viewport = 0);
971 
972  /** \brief Add a PolygonMesh object to screen
973  * \param[in] cloud the polygonal mesh point cloud
974  * \param[in] vertices the polygonal mesh vertices
975  * \param[in] id the polygon object id (default: "polygon")
976  * \param[in] viewport the view port where the PolygonMesh should be added (default: all)
977  */
978  template <typename PointT> bool
979  addPolygonMesh (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
980  const std::vector<pcl::Vertices> &vertices,
981  const std::string &id = "polygon",
982  int viewport = 0);
983 
984  /** \brief Update a PolygonMesh object on screen
985  * \param[in] cloud the polygonal mesh point cloud
986  * \param[in] vertices the polygonal mesh vertices
987  * \param[in] id the polygon object id (default: "polygon")
988  * \return false if no polygonmesh with the specified ID was found
989  */
990  template <typename PointT> bool
991  updatePolygonMesh (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
992  const std::vector<pcl::Vertices> &vertices,
993  const std::string &id = "polygon");
994 
995  /** \brief Update a PolygonMesh object on screen
996  * \param[in] polymesh the polygonal mesh
997  * \param[in] id the polygon object id (default: "polygon")
998  * \return false if no polygonmesh with the specified ID was found
999  */
1000  bool
1001  updatePolygonMesh (const pcl::PolygonMesh &polymesh,
1002  const std::string &id = "polygon");
1003 
1004  /** \brief Add a Polygonline from a polygonMesh object to screen
1005  * \param[in] polymesh the polygonal mesh from where the polylines will be extracted
1006  * \param[in] id the polygon object id (default: "polygon")
1007  * \param[in] viewport the view port where the PolygonMesh should be added (default: all)
1008  */
1009  bool
1010  addPolylineFromPolygonMesh (const pcl::PolygonMesh &polymesh,
1011  const std::string &id = "polyline",
1012  int viewport = 0);
1013 
1014  /** \brief Add the specified correspondences to the display.
1015  * \param[in] source_points The source points
1016  * \param[in] target_points The target points
1017  * \param[in] correspondences The mapping from source points to target points. Each element must be an index into target_points
1018  * \param[in] id the polygon object id (default: "correspondences")
1019  * \param[in] viewport the view port where the correspondences should be added (default: all)
1020  */
1021  template <typename PointT> bool
1022  addCorrespondences (const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1023  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1024  const std::vector<int> & correspondences,
1025  const std::string &id = "correspondences",
1026  int viewport = 0);
1027 
1028  /** \brief Add a TextureMesh object to screen
1029  * \param[in] polymesh the textured polygonal mesh
1030  * \param[in] id the texture mesh object id (default: "texture")
1031  * \param[in] viewport the view port where the TextureMesh should be added (default: all)
1032  */
1033  bool
1034  addTextureMesh (const pcl::TextureMesh &polymesh,
1035  const std::string &id = "texture",
1036  int viewport = 0);
1037 
1038  /** \brief Add the specified correspondences to the display.
1039  * \param[in] source_points The source points
1040  * \param[in] target_points The target points
1041  * \param[in] correspondences The mapping from source points to target points. Each element must be an index into target_points
1042  * \param[in] nth display only the Nth correspondence (e.g., skip the rest)
1043  * \param[in] id the polygon object id (default: "correspondences")
1044  * \param[in] viewport the view port where the correspondences should be added (default: all)
1045  * \param[in] overwrite allow to overwrite already existing correspondences
1046  */
1047  template <typename PointT> bool
1048  addCorrespondences (const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1049  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1050  const pcl::Correspondences &correspondences,
1051  int nth,
1052  const std::string &id = "correspondences",
1053  int viewport = 0,
1054  bool overwrite = false);
1055 
1056  /** \brief Add the specified correspondences to the display.
1057  * \param[in] source_points The source points
1058  * \param[in] target_points The target points
1059  * \param[in] correspondences The mapping from source points to target points. Each element must be an index into target_points
1060  * \param[in] id the polygon object id (default: "correspondences")
1061  * \param[in] viewport the view port where the correspondences should be added (default: all)
1062  */
1063  template <typename PointT> bool
1065  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1066  const pcl::Correspondences &correspondences,
1067  const std::string &id = "correspondences",
1068  int viewport = 0)
1069  {
1070  // If Nth not given, display all correspondences
1071  return (addCorrespondences<PointT> (source_points, target_points,
1072  correspondences, 1, id, viewport));
1073  }
1074 
1075  /** \brief Update the specified correspondences to the display.
1076  * \param[in] source_points The source points
1077  * \param[in] target_points The target points
1078  * \param[in] correspondences The mapping from source points to target points. Each element must be an index into target_points
1079  * \param[in] nth display only the Nth correspondence (e.g., skip the rest)
1080  * \param[in] id the polygon object id (default: "correspondences")
1081  * \param[in] viewport the view port where the correspondences should be updated (default: all)
1082  */
1083  template <typename PointT> bool
1084  updateCorrespondences (
1085  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1086  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1087  const pcl::Correspondences &correspondences,
1088  int nth,
1089  const std::string &id = "correspondences",
1090  int viewport = 0);
1091 
1092  /** \brief Update the specified correspondences to the display.
1093  * \param[in] source_points The source points
1094  * \param[in] target_points The target points
1095  * \param[in] correspondences The mapping from source points to target points. Each element must be an index into target_points
1096  * \param[in] id the polygon object id (default: "correspondences")
1097  * \param[in] viewport the view port where the correspondences should be updated (default: all)
1098  */
1099  template <typename PointT> bool
1101  const typename pcl::PointCloud<PointT>::ConstPtr &source_points,
1102  const typename pcl::PointCloud<PointT>::ConstPtr &target_points,
1103  const pcl::Correspondences &correspondences,
1104  const std::string &id = "correspondences",
1105  int viewport = 0)
1106  {
1107  // If Nth not given, display all correspondences
1108  return (updateCorrespondences<PointT> (source_points, target_points,
1109  correspondences, 1, id, viewport));
1110  }
1111 
1112  /** \brief Remove the specified correspondences from the display.
1113  * \param[in] id the polygon correspondences object id (i.e., given on \ref addCorrespondences)
1114  * \param[in] viewport view port from where the correspondences should be removed (default: all)
1115  */
1116  void
1117  removeCorrespondences (const std::string &id = "correspondences", int viewport = 0);
1118 
1119  /** \brief Get the color handler index of a rendered PointCloud based on its ID
1120  * \param[in] id the point cloud object id
1121  */
1122  int
1123  getColorHandlerIndex (const std::string &id);
1124 
1125  /** \brief Get the geometry handler index of a rendered PointCloud based on its ID
1126  * \param[in] id the point cloud object id
1127  */
1128  int
1129  getGeometryHandlerIndex (const std::string &id);
1130 
1131  /** \brief Update/set the color index of a renderered PointCloud based on its ID
1132  * \param[in] id the point cloud object id
1133  * \param[in] index the color handler index to use
1134  */
1135  bool
1136  updateColorHandlerIndex (const std::string &id, int index);
1137 
1138  /** \brief Set the rendering properties of a PointCloud (3x values - e.g., RGB)
1139  * \param[in] property the property type
1140  * \param[in] val1 the first value to be set
1141  * \param[in] val2 the second value to be set
1142  * \param[in] val3 the third value to be set
1143  * \param[in] id the point cloud object id (default: cloud)
1144  * \param[in] viewport the view port where the Point Cloud's rendering properties should be modified (default: all)
1145  * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
1146  */
1147  bool
1148  setPointCloudRenderingProperties (int property, double val1, double val2, double val3,
1149  const std::string &id = "cloud", int viewport = 0);
1150 
1151  /** \brief Set the rendering properties of a PointCloud (2x values - e.g., LUT minmax values)
1152  * \param[in] property the property type
1153  * \param[in] val1 the first value to be set
1154  * \param[in] val2 the second value to be set
1155  * \param[in] id the point cloud object id (default: cloud)
1156  * \param[in] viewport the view port where the Point Cloud's rendering properties should be modified (default: all)
1157  * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
1158  */
1159  bool
1160  setPointCloudRenderingProperties (int property, double val1, double val2,
1161  const std::string &id = "cloud", int viewport = 0);
1162 
1163  /** \brief Set the rendering properties of a PointCloud
1164  * \param[in] property the property type
1165  * \param[in] value the value to be set
1166  * \param[in] id the point cloud object id (default: cloud)
1167  * \param[in] viewport the view port where the Point Cloud's rendering properties should be modified (default: all)
1168  * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
1169  */
1170  bool
1171  setPointCloudRenderingProperties (int property, double value,
1172  const std::string &id = "cloud", int viewport = 0);
1173 
1174  /** \brief Get the rendering properties of a PointCloud
1175  * \param[in] property the property type
1176  * \param[in] value the resultant property value
1177  * \param[in] id the point cloud object id (default: cloud)
1178  * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
1179  */
1180  bool
1181  getPointCloudRenderingProperties (int property, double &value,
1182  const std::string &id = "cloud");
1183 
1184  /** \brief Set whether the point cloud is selected or not
1185  * \param[in] selected whether the cloud is selected or not (true = selected)
1186  * \param[in] id the point cloud object id (default: cloud)
1187  */
1188  bool
1189  setPointCloudSelected (const bool selected, const std::string &id = "cloud" );
1190 
1191  /** \brief Set the rendering properties of a shape
1192  * \param[in] property the property type
1193  * \param[in] value the value to be set
1194  * \param[in] id the shape object id
1195  * \param[in] viewport the view port where the shape's properties should be modified (default: all)
1196  * \note When using \ref addPolygonMesh you you should use \ref setPointCloudRenderingProperties
1197  * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
1198  */
1199  bool
1200  setShapeRenderingProperties (int property, double value,
1201  const std::string &id, int viewport = 0);
1202 
1203  /** \brief Set the rendering properties of a shape (3x values - e.g., RGB)
1204  * \param[in] property the property type
1205  * \param[in] val1 the first value to be set
1206  * \param[in] val2 the second value to be set
1207  * \param[in] val3 the third value to be set
1208  * \param[in] id the shape object id
1209  * \param[in] viewport the view port where the shape's properties should be modified (default: all)
1210  * \note When using \ref addPolygonMesh you you should use \ref setPointCloudRenderingProperties
1211  */
1212  bool
1213  setShapeRenderingProperties (int property, double val1, double val2, double val3,
1214  const std::string &id, int viewport = 0);
1215 
1216  /** \brief Returns true when the user tried to close the window */
1217  bool
1218  wasStopped () const;
1219 
1220  /** \brief Set the stopped flag back to false */
1221  void
1222  resetStoppedFlag ();
1223 
1224  /** \brief Stop the interaction and close the visualizaton window. */
1225  void
1226  close ();
1227 
1228  /** \brief Create a new viewport from [xmin,ymin] -> [xmax,ymax].
1229  * \param[in] xmin the minimum X coordinate for the viewport (0.0 <= 1.0)
1230  * \param[in] ymin the minimum Y coordinate for the viewport (0.0 <= 1.0)
1231  * \param[in] xmax the maximum X coordinate for the viewport (0.0 <= 1.0)
1232  * \param[in] ymax the maximum Y coordinate for the viewport (0.0 <= 1.0)
1233  * \param[in] viewport the id of the new viewport
1234  *
1235  * \note If no renderer for the current window exists, one will be created, and
1236  * the viewport will be set to 0 ('all'). In case one or multiple renderers do
1237  * exist, the viewport ID will be set to the total number of renderers - 1.
1238  */
1239  void
1240  createViewPort (double xmin, double ymin, double xmax, double ymax, int &viewport);
1241 
1242  /** \brief Create a new separate camera for the given viewport.
1243  * \param[in] viewport the viewport to create a new camera for.
1244  */
1245  void
1246  createViewPortCamera (const int viewport);
1247 
1248  /** \brief Add a polygon (polyline) that represents the input point cloud (connects all
1249  * points in order)
1250  * \param[in] cloud the point cloud dataset representing the polygon
1251  * \param[in] r the red channel of the color that the polygon should be rendered with
1252  * \param[in] g the green channel of the color that the polygon should be rendered with
1253  * \param[in] b the blue channel of the color that the polygon should be rendered with
1254  * \param[in] id (optional) the polygon id/name (default: "polygon")
1255  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1256  */
1257  template <typename PointT> bool
1258  addPolygon (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1259  double r, double g, double b,
1260  const std::string &id = "polygon", int viewport = 0);
1261 
1262  /** \brief Add a polygon (polyline) that represents the input point cloud (connects all
1263  * points in order)
1264  * \param[in] cloud the point cloud dataset representing the polygon
1265  * \param[in] id the polygon id/name (default: "polygon")
1266  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1267  */
1268  template <typename PointT> bool
1269  addPolygon (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1270  const std::string &id = "polygon",
1271  int viewport = 0);
1272 
1273  /** \brief Add a planar polygon that represents the input point cloud (connects all points in order)
1274  * \param[in] polygon the polygon to draw
1275  * \param[in] r the red channel of the color that the polygon should be rendered with
1276  * \param[in] g the green channel of the color that the polygon should be rendered with
1277  * \param[in] b the blue channel of the color that the polygon should be rendered with
1278  * \param[in] id the polygon id/name (default: "polygon")
1279  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1280  */
1281  template <typename PointT> bool
1282  addPolygon (const pcl::PlanarPolygon<PointT> &polygon,
1283  double r, double g, double b,
1284  const std::string &id = "polygon",
1285  int viewport = 0);
1286 
1287  /** \brief Add a line segment from two points
1288  * \param[in] pt1 the first (start) point on the line
1289  * \param[in] pt2 the second (end) point on the line
1290  * \param[in] id the line id/name (default: "line")
1291  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1292  */
1293  template <typename P1, typename P2> bool
1294  addLine (const P1 &pt1, const P2 &pt2, const std::string &id = "line",
1295  int viewport = 0);
1296 
1297  /** \brief Add a line segment from two points
1298  * \param[in] pt1 the first (start) point on the line
1299  * \param[in] pt2 the second (end) point on the line
1300  * \param[in] r the red channel of the color that the line should be rendered with
1301  * \param[in] g the green channel of the color that the line should be rendered with
1302  * \param[in] b the blue channel of the color that the line should be rendered with
1303  * \param[in] id the line id/name (default: "line")
1304  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1305  */
1306  template <typename P1, typename P2> bool
1307  addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b,
1308  const std::string &id = "line", int viewport = 0);
1309 
1310  /** \brief Add a line arrow segment between two points, and display the distance between them
1311  *
1312  * Arrow heads are attached to both end points of the arrow.
1313  *
1314  * \param[in] pt1 the first (start) point on the line
1315  * \param[in] pt2 the second (end) point on the line
1316  * \param[in] r the red channel of the color that the line should be rendered with
1317  * \param[in] g the green channel of the color that the line should be rendered with
1318  * \param[in] b the blue channel of the color that the line should be rendered with
1319  * \param[in] id the arrow id/name (default: "arrow")
1320  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1321  */
1322  template <typename P1, typename P2> bool
1323  addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b,
1324  const std::string &id = "arrow", int viewport = 0);
1325 
1326  /** \brief Add a line arrow segment between two points, and (optianally) display the distance between them
1327  *
1328  * Arrow head is attached on the **start** point (\c pt1) of the arrow.
1329  *
1330  * \param[in] pt1 the first (start) point on the line
1331  * \param[in] pt2 the second (end) point on the line
1332  * \param[in] r the red channel of the color that the line should be rendered with
1333  * \param[in] g the green channel of the color that the line should be rendered with
1334  * \param[in] b the blue channel of the color that the line should be rendered with
1335  * \param[in] display_length true if the length should be displayed on the arrow as text
1336  * \param[in] id the line id/name (default: "arrow")
1337  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1338  */
1339  template <typename P1, typename P2> bool
1340  addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, bool display_length,
1341  const std::string &id = "arrow", int viewport = 0);
1342 
1343  /** \brief Add a line arrow segment between two points, and display the distance between them in a given color
1344  *
1345  * Arrow heads are attached to both end points of the arrow.
1346  *
1347  * \param[in] pt1 the first (start) point on the line
1348  * \param[in] pt2 the second (end) point on the line
1349  * \param[in] r_line the red channel of the color that the line should be rendered with
1350  * \param[in] g_line the green channel of the color that the line should be rendered with
1351  * \param[in] b_line the blue channel of the color that the line should be rendered with
1352  * \param[in] r_text the red channel of the color that the text should be rendered with
1353  * \param[in] g_text the green channel of the color that the text should be rendered with
1354  * \param[in] b_text the blue channel of the color that the text should be rendered with
1355  * \param[in] id the line id/name (default: "arrow")
1356  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1357  */
1358  template <typename P1, typename P2> bool
1359  addArrow (const P1 &pt1, const P2 &pt2,
1360  double r_line, double g_line, double b_line,
1361  double r_text, double g_text, double b_text,
1362  const std::string &id = "arrow", int viewport = 0);
1363 
1364 
1365  /** \brief Add a sphere shape from a point and a radius
1366  * \param[in] center the center of the sphere
1367  * \param[in] radius the radius of the sphere
1368  * \param[in] id the sphere id/name (default: "sphere")
1369  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1370  */
1371  template <typename PointT> bool
1372  addSphere (const PointT &center, double radius, const std::string &id = "sphere",
1373  int viewport = 0);
1374 
1375  /** \brief Add a sphere shape from a point and a radius
1376  * \param[in] center the center of the sphere
1377  * \param[in] radius the radius of the sphere
1378  * \param[in] r the red channel of the color that the sphere should be rendered with
1379  * \param[in] g the green channel of the color that the sphere should be rendered with
1380  * \param[in] b the blue channel of the color that the sphere should be rendered with
1381  * \param[in] id the sphere id/name (default: "sphere")
1382  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1383  */
1384  template <typename PointT> bool
1385  addSphere (const PointT &center, double radius, double r, double g, double b,
1386  const std::string &id = "sphere", int viewport = 0);
1387 
1388  /** \brief Update an existing sphere shape from a point and a radius
1389  * \param[in] center the center of the sphere
1390  * \param[in] radius the radius of the sphere
1391  * \param[in] r the red channel of the color that the sphere should be rendered with
1392  * \param[in] g the green channel of the color that the sphere should be rendered with
1393  * \param[in] b the blue channel of the color that the sphere should be rendered with
1394  * \param[in] id the sphere id/name (default: "sphere")
1395  */
1396  template <typename PointT> bool
1397  updateSphere (const PointT &center, double radius, double r, double g, double b,
1398  const std::string &id = "sphere");
1399 
1400  /** \brief Add a vtkPolydata as a mesh
1401  * \param[in] polydata vtkPolyData
1402  * \param[in] id the model id/name (default: "PolyData")
1403  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1404  */
1405  bool
1406  addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata,
1407  const std::string & id = "PolyData",
1408  int viewport = 0);
1409 
1410  /** \brief Add a vtkPolydata as a mesh
1411  * \param[in] polydata vtkPolyData
1412  * \param[in] transform transformation to apply
1413  * \param[in] id the model id/name (default: "PolyData")
1414  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1415  */
1416  bool
1417  addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata,
1419  const std::string &id = "PolyData",
1420  int viewport = 0);
1421 
1422  /** \brief Add a PLYmodel as a mesh
1423  * \param[in] filename of the ply file
1424  * \param[in] id the model id/name (default: "PLYModel")
1425  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1426  */
1427  bool
1428  addModelFromPLYFile (const std::string &filename,
1429  const std::string &id = "PLYModel",
1430  int viewport = 0);
1431 
1432  /** \brief Add a PLYmodel as a mesh and applies given transformation
1433  * \param[in] filename of the ply file
1434  * \param[in] transform transformation to apply
1435  * \param[in] id the model id/name (default: "PLYModel")
1436  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1437  */
1438  bool
1439  addModelFromPLYFile (const std::string &filename,
1441  const std::string &id = "PLYModel",
1442  int viewport = 0);
1443 
1444  /** \brief Add a cylinder from a set of given model coefficients
1445  * \param[in] coefficients the model coefficients (point_on_axis, axis_direction, radius)
1446  * \param[in] id the cylinder id/name (default: "cylinder")
1447  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1448  *
1449  * \code
1450  * // The following are given (or computed using sample consensus techniques)
1451  * // See SampleConsensusModelCylinder for more information.
1452  * // Eigen::Vector3f pt_on_axis, axis_direction;
1453  * // float radius;
1454  *
1455  * pcl::ModelCoefficients cylinder_coeff;
1456  * cylinder_coeff.values.resize (7); // We need 7 values
1457  * cylinder_coeff.values[0] = pt_on_axis.x ();
1458  * cylinder_coeff.values[1] = pt_on_axis.y ();
1459  * cylinder_coeff.values[2] = pt_on_axis.z ();
1460  *
1461  * cylinder_coeff.values[3] = axis_direction.x ();
1462  * cylinder_coeff.values[4] = axis_direction.y ();
1463  * cylinder_coeff.values[5] = axis_direction.z ();
1464  *
1465  * cylinder_coeff.values[6] = radius;
1466  *
1467  * addCylinder (cylinder_coeff);
1468  * \endcode
1469  */
1470  bool
1471  addCylinder (const pcl::ModelCoefficients &coefficients,
1472  const std::string &id = "cylinder",
1473  int viewport = 0);
1474 
1475  /** \brief Add a sphere from a set of given model coefficients
1476  * \param[in] coefficients the model coefficients (sphere center, radius)
1477  * \param[in] id the sphere id/name (default: "sphere")
1478  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1479  *
1480  * \code
1481  * // The following are given (or computed using sample consensus techniques)
1482  * // See SampleConsensusModelSphere for more information
1483  * // Eigen::Vector3f sphere_center;
1484  * // float radius;
1485  *
1486  * pcl::ModelCoefficients sphere_coeff;
1487  * sphere_coeff.values.resize (4); // We need 4 values
1488  * sphere_coeff.values[0] = sphere_center.x ();
1489  * sphere_coeff.values[1] = sphere_center.y ();
1490  * sphere_coeff.values[2] = sphere_center.z ();
1491  *
1492  * sphere_coeff.values[3] = radius;
1493  *
1494  * addSphere (sphere_coeff);
1495  * \endcode
1496  */
1497  bool
1498  addSphere (const pcl::ModelCoefficients &coefficients,
1499  const std::string &id = "sphere",
1500  int viewport = 0);
1501 
1502  /** \brief Add a line from a set of given model coefficients
1503  * \param[in] coefficients the model coefficients (point_on_line, direction)
1504  * \param[in] id the line id/name (default: "line")
1505  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1506  *
1507  * \code
1508  * // The following are given (or computed using sample consensus techniques)
1509  * // See SampleConsensusModelLine for more information
1510  * // Eigen::Vector3f point_on_line, line_direction;
1511  *
1512  * pcl::ModelCoefficients line_coeff;
1513  * line_coeff.values.resize (6); // We need 6 values
1514  * line_coeff.values[0] = point_on_line.x ();
1515  * line_coeff.values[1] = point_on_line.y ();
1516  * line_coeff.values[2] = point_on_line.z ();
1517  *
1518  * line_coeff.values[3] = line_direction.x ();
1519  * line_coeff.values[4] = line_direction.y ();
1520  * line_coeff.values[5] = line_direction.z ();
1521  *
1522  * addLine (line_coeff);
1523  * \endcode
1524  */
1525  bool
1526  addLine (const pcl::ModelCoefficients &coefficients,
1527  const std::string &id = "line",
1528  int viewport = 0);
1529 
1530  /** \brief Add a plane from a set of given model coefficients
1531  * \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
1532  * \param[in] id the plane id/name (default: "plane")
1533  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1534  *
1535  * \code
1536  * // The following are given (or computed using sample consensus techniques)
1537  * // See SampleConsensusModelPlane for more information
1538  * // Eigen::Vector4f plane_parameters;
1539  *
1540  * pcl::ModelCoefficients plane_coeff;
1541  * plane_coeff.values.resize (4); // We need 4 values
1542  * plane_coeff.values[0] = plane_parameters.x ();
1543  * plane_coeff.values[1] = plane_parameters.y ();
1544  * plane_coeff.values[2] = plane_parameters.z ();
1545  * plane_coeff.values[3] = plane_parameters.w ();
1546  *
1547  * addPlane (plane_coeff);
1548  * \endcode
1549  */
1550  bool
1551  addPlane (const pcl::ModelCoefficients &coefficients,
1552  const std::string &id = "plane",
1553  int viewport = 0);
1554 
1555  bool
1556  addPlane (const pcl::ModelCoefficients &coefficients, double x, double y, double z,
1557  const std::string &id = "plane",
1558  int viewport = 0);
1559  /** \brief Add a circle from a set of given model coefficients
1560  * \param[in] coefficients the model coefficients (x, y, radius)
1561  * \param[in] id the circle id/name (default: "circle")
1562  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1563  *
1564  * \code
1565  * // The following are given (or computed using sample consensus techniques)
1566  * // See SampleConsensusModelCircle2D for more information
1567  * // float x, y, radius;
1568  *
1569  * pcl::ModelCoefficients circle_coeff;
1570  * circle_coeff.values.resize (3); // We need 3 values
1571  * circle_coeff.values[0] = x;
1572  * circle_coeff.values[1] = y;
1573  * circle_coeff.values[2] = radius;
1574  *
1575  * vtkSmartPointer<vtkDataSet> data = pcl::visualization::create2DCircle (circle_coeff, z);
1576  * \endcode
1577  */
1578  bool
1579  addCircle (const pcl::ModelCoefficients &coefficients,
1580  const std::string &id = "circle",
1581  int viewport = 0);
1582 
1583  /** \brief Add a cone from a set of given model coefficients
1584  * \param[in] coefficients the model coefficients (see \ref pcl::visualization::createCone)
1585  * \param[in] id the cone id/name (default: "cone")
1586  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1587  */
1588  bool
1589  addCone (const pcl::ModelCoefficients &coefficients,
1590  const std::string &id = "cone",
1591  int viewport = 0);
1592 
1593  /** \brief Add a cube from a set of given model coefficients
1594  * \param[in] coefficients the model coefficients (see \ref pcl::visualization::createCube)
1595  * \param[in] id the cube id/name (default: "cube")
1596  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1597  */
1598  bool
1599  addCube (const pcl::ModelCoefficients &coefficients,
1600  const std::string &id = "cube",
1601  int viewport = 0);
1602 
1603  /** \brief Add a cube from a set of given model coefficients
1604  * \param[in] translation a translation to apply to the cube from 0,0,0
1605  * \param[in] rotation a quaternion-based rotation to apply to the cube
1606  * \param[in] width the cube's width
1607  * \param[in] height the cube's height
1608  * \param[in] depth the cube's depth
1609  * \param[in] id the cube id/name (default: "cube")
1610  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1611  */
1612  bool
1613  addCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation,
1614  double width, double height, double depth,
1615  const std::string &id = "cube",
1616  int viewport = 0);
1617 
1618  /** \brief Add a cube
1619  * \param[in] x_min the min X coordinate
1620  * \param[in] x_max the max X coordinate
1621  * \param[in] y_min the min Y coordinate
1622  * \param[in] y_max the max Y coordinate
1623  * \param[in] z_min the min Z coordinate
1624  * \param[in] z_max the max Z coordinate
1625  * \param[in] r how much red (0.0 -> 1.0)
1626  * \param[in] g how much green (0.0 -> 1.0)
1627  * \param[in] b how much blue (0.0 -> 1.0)
1628  * \param[in] id the cube id/name (default: "cube")
1629  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1630  */
1631  bool
1632  addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max,
1633  double r = 1.0, double g = 1.0, double b = 1.0, const std::string &id = "cube", int viewport = 0);
1634 
1635  /** \brief Changes the visual representation for all actors to surface representation. */
1636  void
1637  setRepresentationToSurfaceForAllActors ();
1638 
1639  /** \brief Changes the visual representation for all actors to points representation. */
1640  void
1641  setRepresentationToPointsForAllActors ();
1642 
1643  /** \brief Changes the visual representation for all actors to wireframe representation. */
1644  void
1645  setRepresentationToWireframeForAllActors ();
1646 
1647  /** \brief Sets whether the 2D overlay text showing the framerate of the window is displayed or not.
1648  * \param[in] show_fps determines whether the fps text will be shown or not.
1649  */
1650  void
1651  setShowFPS (bool show_fps);
1652 
1653  /** \brief Renders a virtual scene as seen from the camera viewpoint and returns the rendered point cloud.
1654  * ATT: This method will only render the scene if only on viewport exists. Otherwise, returns an empty
1655  * point cloud and exits immediately.
1656  * \param[in] xres is the size of the window (X) used to render the scene
1657  * \param[in] yres is the size of the window (Y) used to render the scene
1658  * \param[in] cloud is the rendered point cloud
1659  */
1660  void
1661  renderView (int xres, int yres, pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
1662 
1663  /** \brief The purpose of this method is to render a CAD model added to the visualizer from different viewpoints
1664  * in order to simulate partial views of model. The viewpoint locations are the vertices of a tesselated sphere
1665  * build from an icosaheadron. The tesselation paremeter controls how many times the triangles of the original
1666  * icosahedron are divided to approximate the sphere and thus the number of partial view generated for a model,
1667  * with a tesselation_level of 0, 12 views are generated if use_vertices=true and 20 views if use_vertices=false
1668  *
1669  * \param[in] xres the size of the window (X) used to render the partial view of the object
1670  * \param[in] yres the size of the window (Y) used to render the partial view of the object
1671  * \param[in] cloud is a vector of pointcloud with XYZ information that represent the model as seen from the respective viewpoints.
1672  * \param[out] poses represent the transformation from object coordinates to camera coordinates for the respective viewpoint.
1673  * \param[out] enthropies are values between 0 and 1 representing which percentage of the model is seen from the respective viewpoint.
1674  * \param[in] tesselation_level represents the number of subdivisions applied to the triangles of original icosahedron.
1675  * \param[in] view_angle field of view of the virtual camera. Default: 45
1676  * \param[in] radius_sphere the tesselated sphere radius. Default: 1
1677  * \param[in] use_vertices if true, use the vertices of tesselated icosahedron (12,42,...) or if false, use the faces of tesselated
1678  * icosahedron (20,80,...). Default: true
1679  */
1680  void
1681  renderViewTesselatedSphere (
1682  int xres, int yres,
1684  std::vector<Eigen::Matrix4f,Eigen::aligned_allocator< Eigen::Matrix4f > > & poses, std::vector<float> & enthropies, int tesselation_level,
1685  float view_angle = 45, float radius_sphere = 1, bool use_vertices = true);
1686 
1687 
1688  /** \brief Initialize camera parameters with some default values. */
1689  void
1690  initCameraParameters ();
1691 
1692  /** \brief Search for camera parameters at the command line and set them internally.
1693  * \param[in] argc
1694  * \param[in] argv
1695  */
1696  bool
1697  getCameraParameters (int argc, char **argv);
1698 
1699  /** \brief Load camera parameters from a camera parameters file.
1700  * \param[in] file the name of the camera parameters file
1701  */
1702  bool
1703  loadCameraParameters (const std::string &file);
1704 
1705  /** \brief Checks whether the camera parameters were manually loaded.
1706  * \return True if valid "-cam" option is available in command line.
1707  * \sa cameraFileLoaded ()
1708  */
1709  bool
1710  cameraParamsSet () const;
1711 
1712  /** \brief Checks whether a camera file were automatically loaded.
1713  * \return True if a valid camera file is automatically loaded.
1714  * \note The camera file is saved by pressing "ctrl + s" during last run of the program
1715  * and restored automatically when the program runs this time.
1716  * \sa cameraParamsSet ()
1717  */
1718  bool
1719  cameraFileLoaded () const;
1720 
1721  /** \brief Get camera file for camera parameter saving/restoring.
1722  * \note This will be valid only when valid "-cam" option were available in command line
1723  * or a saved camera file were automatically loaded.
1724  * \sa cameraParamsSet (), cameraFileLoaded ()
1725  */
1726  std::string
1727  getCameraFile () const;
1728 
1729  /** \brief Update camera parameters and render. */
1730  void
1731  updateCamera ();
1732 
1733  /** \brief Reset camera parameters and render. */
1734  void
1735  resetCamera ();
1736 
1737  /** \brief Reset the camera direction from {0, 0, 0} to the center_{x, y, z} of a given dataset.
1738  * \param[in] id the point cloud object id (default: cloud)
1739  */
1740  void
1741  resetCameraViewpoint (const std::string &id = "cloud");
1742 
1743  /** \brief Set the camera pose given by position, viewpoint and up vector
1744  * \param[in] pos_x the x coordinate of the camera location
1745  * \param[in] pos_y the y coordinate of the camera location
1746  * \param[in] pos_z the z coordinate of the camera location
1747  * \param[in] view_x the x component of the view point of the camera
1748  * \param[in] view_y the y component of the view point of the camera
1749  * \param[in] view_z the z component of the view point of the camera
1750  * \param[in] up_x the x component of the view up direction of the camera
1751  * \param[in] up_y the y component of the view up direction of the camera
1752  * \param[in] up_z the y component of the view up direction of the camera
1753  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1754  */
1755  void
1756  setCameraPosition (double pos_x, double pos_y, double pos_z,
1757  double view_x, double view_y, double view_z,
1758  double up_x, double up_y, double up_z, int viewport = 0);
1759 
1760  /** \brief Set the camera location and viewup according to the given arguments
1761  * \param[in] pos_x the x coordinate of the camera location
1762  * \param[in] pos_y the y coordinate of the camera location
1763  * \param[in] pos_z the z coordinate of the camera location
1764  * \param[in] up_x the x component of the view up direction of the camera
1765  * \param[in] up_y the y component of the view up direction of the camera
1766  * \param[in] up_z the z component of the view up direction of the camera
1767  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1768  */
1769  void
1770  setCameraPosition (double pos_x, double pos_y, double pos_z,
1771  double up_x, double up_y, double up_z, int viewport = 0);
1772 
1773  /** \brief Set the camera parameters via an intrinsics and and extrinsics matrix
1774  * \note This assumes that the pixels are square and that the center of the image is at the center of the sensor.
1775  * \param[in] intrinsics the intrinsics that will be used to compute the VTK camera parameters
1776  * \param[in] extrinsics the extrinsics that will be used to compute the VTK camera parameters
1777  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1778  */
1779  void
1780  setCameraParameters (const Eigen::Matrix3f &intrinsics, const Eigen::Matrix4f &extrinsics, int viewport = 0);
1781 
1782  /** \brief Set the camera parameters by given a full camera data structure.
1783  * \param[in] camera camera structure containing all the camera parameters.
1784  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1785  */
1786  void
1787  setCameraParameters (const Camera &camera, int viewport = 0);
1788 
1789  /** \brief Set the camera clipping distances.
1790  * \param[in] near the near clipping distance (no objects closer than this to the camera will be drawn)
1791  * \param[in] far the far clipping distance (no objects further away than this to the camera will be drawn)
1792  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1793  */
1794  void
1795  setCameraClipDistances (double near, double far, int viewport = 0);
1796 
1797  /** \brief Set the camera vertical field of view.
1798  * \param[in] fovy vertical field of view in radians
1799  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1800  */
1801  void
1802  setCameraFieldOfView (double fovy, int viewport = 0);
1803 
1804  /** \brief Get the current camera parameters. */
1805  void
1806  getCameras (std::vector<Camera>& cameras);
1807 
1808 
1809  /** \brief Get the current viewing pose. */
1810  Eigen::Affine3f
1811  getViewerPose (int viewport = 0);
1812 
1813  /** \brief Save the current rendered image to disk, as a PNG screenshot.
1814  * \param[in] file the name of the PNG file
1815  */
1816  void
1817  saveScreenshot (const std::string &file);
1818 
1819  /** \brief Save the camera parameters to disk, as a .cam file.
1820  * \param[in] file the name of the .cam file
1821  */
1822  void
1823  saveCameraParameters (const std::string &file);
1824 
1825  /** \brief Get camera parameters and save them to a pcl::visualization::Camera.
1826  * \param[out] camera the name of the pcl::visualization::Camera
1827  */
1828  void
1829  getCameraParameters (Camera &camera);
1830 
1831  /** \brief Return a pointer to the underlying VTK Render Window used. */
1834  {
1835  return (win_);
1836  }
1837 
1838  /** \brief Return a pointer to the underlying VTK Renderer Collection. */
1841  {
1842  return (rens_);
1843  }
1844 
1845  /** \brief Return a pointer to the CloudActorMap this visualizer uses. */
1848  {
1849  return (cloud_actor_map_);
1850  }
1851 
1852  /** \brief Return a pointer to the ShapeActorMap this visualizer uses. */
1855  {
1856  return (shape_actor_map_);
1857  }
1858 
1859  /** \brief Set the position in screen coordinates.
1860  * \param[in] x where to move the window to (X)
1861  * \param[in] y where to move the window to (Y)
1862  */
1863  void
1864  setPosition (int x, int y);
1865 
1866  /** \brief Set the window size in screen coordinates.
1867  * \param[in] xw window size in horizontal (pixels)
1868  * \param[in] yw window size in vertical (pixels)
1869  */
1870  void
1871  setSize (int xw, int yw);
1872 
1873  /** \brief Use Vertex Buffer Objects renderers.
1874  * This is an optimization for the obsolete OpenGL backend. Modern OpenGL2 backend (VTK ≥ 6.3) uses vertex
1875  * buffer objects by default, transparently for the user.
1876  * \param[in] use_vbos set to true to use VBOs
1877  */
1878  void
1879  setUseVbos (bool use_vbos);
1880 
1881  /** \brief Set the ID of a cloud or shape to be used for LUT display
1882  * \param[in] id The id of the cloud/shape look up table to be displayed
1883  * The look up table is displayed by pressing 'u' in the PCLVisualizer */
1884  void
1885  setLookUpTableID (const std::string id);
1886 
1887  /** \brief Create the internal Interactor object. */
1888  void
1889  createInteractor ();
1890 
1891  /** \brief Set up our unique PCL interactor style for a given vtkRenderWindowInteractor object
1892  * attached to a given vtkRenderWindow
1893  * \param[in,out] iren the vtkRenderWindowInteractor object to set up
1894  * \param[in,out] win a vtkRenderWindow object that the interactor is attached to
1895  */
1896  void
1897  setupInteractor (vtkRenderWindowInteractor *iren,
1898  vtkRenderWindow *win);
1899 
1900  /** \brief Set up PCLVisualizer with custom interactor style for a given vtkRenderWindowInteractor object
1901  * attached to a given vtkRenderWindow
1902  * \param[in,out] iren the vtkRenderWindowInteractor object to set up
1903  * \param[in,out] win a vtkRenderWindow object that the interactor is attached to
1904  * \param[in,out] style a vtkInteractorStyle object
1905  */
1906  void
1907  setupInteractor (vtkRenderWindowInteractor *iren,
1908  vtkRenderWindow *win,
1909  vtkInteractorStyle *style);
1910 
1911  /** \brief Get a pointer to the current interactor style used. */
1914  {
1915  return (style_);
1916  }
1917  protected:
1918  /** \brief The render window interactor. */
1919 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4))
1921 #else
1923 #endif
1924  private:
1925  struct ExitMainLoopTimerCallback : public vtkCommand
1926  {
1927  static ExitMainLoopTimerCallback* New ()
1928  {
1929  return (new ExitMainLoopTimerCallback);
1930  }
1931  virtual void
1932  Execute (vtkObject*, unsigned long event_id, void*);
1933 
1934  int right_timer_id;
1935  PCLVisualizer* pcl_visualizer;
1936  };
1937 
1938  struct ExitCallback : public vtkCommand
1939  {
1940  static ExitCallback* New ()
1941  {
1942  return (new ExitCallback);
1943  }
1944  virtual void
1945  Execute (vtkObject*, unsigned long event_id, void*);
1946 
1947  PCLVisualizer* pcl_visualizer;
1948  };
1949 
1950  //////////////////////////////////////////////////////////////////////////////////////////////
1951  struct FPSCallback : public vtkCommand
1952  {
1953  static FPSCallback *New () { return (new FPSCallback); }
1954 
1955  FPSCallback () : actor (), pcl_visualizer (), decimated () {}
1956  FPSCallback (const FPSCallback& src) : vtkCommand (), actor (src.actor), pcl_visualizer (src.pcl_visualizer), decimated (src.decimated) {}
1957  FPSCallback& operator = (const FPSCallback& src) { actor = src.actor; pcl_visualizer = src.pcl_visualizer; decimated = src.decimated; return (*this); }
1958 
1959  virtual void
1960  Execute (vtkObject*, unsigned long event_id, void*);
1961 
1962  vtkTextActor *actor;
1963  PCLVisualizer* pcl_visualizer;
1964  bool decimated;
1965  };
1966 
1967  /** \brief The FPSCallback object for the current visualizer. */
1968  vtkSmartPointer<FPSCallback> update_fps_;
1969 
1970 #if !((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4))
1971  /** \brief Set to false if the interaction loop is running. */
1972  bool stopped_;
1973 
1974  /** \brief Global timer ID. Used in destructor only. */
1975  int timer_id_;
1976 #endif
1977  /** \brief Callback object enabling us to leave the main loop, when a timer fires. */
1978  vtkSmartPointer<ExitMainLoopTimerCallback> exit_main_loop_timer_callback_;
1979  vtkSmartPointer<ExitCallback> exit_callback_;
1980 
1981  /** \brief The collection of renderers used. */
1983 
1984  /** \brief The render window. */
1986 
1987  /** \brief The render window interactor style. */
1989 
1990  /** \brief Internal list with actor pointers and name IDs for point clouds. */
1991  CloudActorMapPtr cloud_actor_map_;
1992 
1993  /** \brief Internal list with actor pointers and name IDs for shapes. */
1994  ShapeActorMapPtr shape_actor_map_;
1995 
1996  /** \brief Internal list with actor pointers and viewpoint for coordinates. */
1997  CoordinateActorMapPtr coordinate_actor_map_;
1998 
1999  /** \brief Internal pointer to widget which contains a set of axes */
2001 
2002  /** \brief Boolean that holds whether or not the camera parameters were manually initialized */
2003  bool camera_set_;
2004 
2005  /** \brief Boolean that holds whether or not a camera file were automatically loaded */
2006  bool camera_file_loaded_;
2007 
2008  /** \brief Boolean that holds whether or not to use the vtkVertexBufferObjectMapper*/
2009  bool use_vbos_;
2010 
2011  /** \brief Internal method. Removes a vtk actor from the screen.
2012  * \param[in] actor a pointer to the vtk actor object
2013  * \param[in] viewport the view port where the actor should be removed from (default: all)
2014  */
2015  bool
2016  removeActorFromRenderer (const vtkSmartPointer<vtkLODActor> &actor,
2017  int viewport = 0);
2018 
2019  /** \brief Internal method. Removes a vtk actor from the screen.
2020  * \param[in] actor a pointer to the vtk actor object
2021  * \param[in] viewport the view port where the actor should be removed from (default: all)
2022  */
2023  bool
2024  removeActorFromRenderer (const vtkSmartPointer<vtkActor> &actor,
2025  int viewport = 0);
2026 
2027  /** \brief Internal method. Adds a vtk actor to screen.
2028  * \param[in] actor a pointer to the vtk actor object
2029  * \param[in] viewport port where the actor should be added to (default: 0/all)
2030  *
2031  * \note If viewport is set to 0, the actor will be added to all existing
2032  * renders. To select a specific viewport use an integer between 1 and N.
2033  */
2034  void
2035  addActorToRenderer (const vtkSmartPointer<vtkProp> &actor,
2036  int viewport = 0);
2037 
2038  /** \brief Internal method. Adds a vtk actor to screen.
2039  * \param[in] actor a pointer to the vtk actor object
2040  * \param[in] viewport the view port where the actor should be added to (default: all)
2041  */
2042  bool
2043  removeActorFromRenderer (const vtkSmartPointer<vtkProp> &actor,
2044  int viewport = 0);
2045 
2046  /** \brief Internal method. Creates a vtk actor from a vtk polydata object.
2047  * \param[in] data the vtk polydata object to create an actor for
2048  * \param[out] actor the resultant vtk actor object
2049  * \param[in] use_scalars set scalar properties to the mapper if it exists in the data. Default: true.
2050  */
2051  void
2052  createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data,
2054  bool use_scalars = true);
2055 
2056  /** \brief Internal method. Creates a vtk actor from a vtk polydata object.
2057  * \param[in] data the vtk polydata object to create an actor for
2058  * \param[out] actor the resultant vtk actor object
2059  * \param[in] use_scalars set scalar properties to the mapper if it exists in the data. Default: true.
2060  */
2061  void
2062  createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data,
2064  bool use_scalars = true);
2065 
2066  /** \brief Converts a PCL templated PointCloud object to a vtk polydata object.
2067  * \param[in] cloud the input PCL PointCloud dataset
2068  * \param[out] polydata the resultant polydata containing the cloud
2069  * \param[out] initcells a list of cell indices used for the conversion. This can be set once and then passed
2070  * around to speed up the conversion.
2071  */
2072  template <typename PointT> void
2073  convertPointCloudToVTKPolyData (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
2074  vtkSmartPointer<vtkPolyData> &polydata,
2075  vtkSmartPointer<vtkIdTypeArray> &initcells);
2076 
2077  /** \brief Converts a PCL templated PointCloud object to a vtk polydata object.
2078  * \param[in] geometry_handler the geometry handler object used to extract the XYZ data
2079  * \param[out] polydata the resultant polydata containing the cloud
2080  * \param[out] initcells a list of cell indices used for the conversion. This can be set once and then passed
2081  * around to speed up the conversion.
2082  */
2083  template <typename PointT> void
2084  convertPointCloudToVTKPolyData (const PointCloudGeometryHandler<PointT> &geometry_handler,
2085  vtkSmartPointer<vtkPolyData> &polydata,
2086  vtkSmartPointer<vtkIdTypeArray> &initcells);
2087 
2088  /** \brief Converts a PCL templated PointCloud object to a vtk polydata object.
2089  * \param[in] geometry_handler the geometry handler object used to extract the XYZ data
2090  * \param[out] polydata the resultant polydata containing the cloud
2091  * \param[out] initcells a list of cell indices used for the conversion. This can be set once and then passed
2092  * around to speed up the conversion.
2093  */
2094  void
2095  convertPointCloudToVTKPolyData (const GeometryHandlerConstPtr &geometry_handler,
2096  vtkSmartPointer<vtkPolyData> &polydata,
2097  vtkSmartPointer<vtkIdTypeArray> &initcells);
2098 
2099  /** \brief Updates a set of cells (vtkIdTypeArray) if the number of points in a cloud changes
2100  * \param[out] cells the vtkIdTypeArray object (set of cells) to update
2101  * \param[out] initcells a previously saved set of cells. If the number of points in the current cloud is
2102  * higher than the number of cells in \a cells, and initcells contains enough data, then a copy from it
2103  * will be made instead of regenerating the entire array.
2104  * \param[in] nr_points the number of points in the new cloud. This dictates how many cells we need to
2105  * generate
2106  */
2107  void
2108  updateCells (vtkSmartPointer<vtkIdTypeArray> &cells,
2110  vtkIdType nr_points);
2111 
2112  /** \brief Internal function which converts the information present in the geometric
2113  * and color handlers into VTK PolyData+Scalars, constructs a vtkActor object, and adds
2114  * all the required information to the internal cloud_actor_map_ object.
2115  * \param[in] geometry_handler the geometric handler that contains the XYZ data
2116  * \param[in] color_handler the color handler that contains the "RGB" (scalar) data
2117  * \param[in] id the point cloud object id
2118  * \param[in] viewport the view port where the Point Cloud should be added
2119  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
2120  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
2121  */
2122  template <typename PointT> bool
2123  fromHandlersToScreen (const PointCloudGeometryHandler<PointT> &geometry_handler,
2124  const PointCloudColorHandler<PointT> &color_handler,
2125  const std::string &id,
2126  int viewport,
2127  const Eigen::Vector4f& sensor_origin = Eigen::Vector4f (0, 0, 0, 0),
2128  const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternion<float> (1, 0, 0 ,0));
2129 
2130  /** \brief Internal function which converts the information present in the geometric
2131  * and color handlers into VTK PolyData+Scalars, constructs a vtkActor object, and adds
2132  * all the required information to the internal cloud_actor_map_ object.
2133  * \param[in] geometry_handler the geometric handler that contains the XYZ data
2134  * \param[in] color_handler the color handler that contains the "RGB" (scalar) data
2135  * \param[in] id the point cloud object id
2136  * \param[in] viewport the view port where the Point Cloud should be added
2137  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
2138  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
2139  */
2140  template <typename PointT> bool
2141  fromHandlersToScreen (const PointCloudGeometryHandler<PointT> &geometry_handler,
2142  const ColorHandlerConstPtr &color_handler,
2143  const std::string &id,
2144  int viewport,
2145  const Eigen::Vector4f& sensor_origin = Eigen::Vector4f (0, 0, 0, 0),
2146  const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternion<float> (1, 0, 0 ,0));
2147 
2148  /** \brief Internal function which converts the information present in the geometric
2149  * and color handlers into VTK PolyData+Scalars, constructs a vtkActor object, and adds
2150  * all the required information to the internal cloud_actor_map_ object.
2151  * \param[in] geometry_handler the geometric handler that contains the XYZ data
2152  * \param[in] color_handler the color handler that contains the "RGB" (scalar) data
2153  * \param[in] id the point cloud object id
2154  * \param[in] viewport the view port where the Point Cloud should be added
2155  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
2156  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
2157  */
2158  bool
2159  fromHandlersToScreen (const GeometryHandlerConstPtr &geometry_handler,
2160  const ColorHandlerConstPtr &color_handler,
2161  const std::string &id,
2162  int viewport,
2163  const Eigen::Vector4f& sensor_origin = Eigen::Vector4f (0, 0, 0, 0),
2164  const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternion<float> (1, 0, 0 ,0));
2165 
2166  /** \brief Internal function which converts the information present in the geometric
2167  * and color handlers into VTK PolyData+Scalars, constructs a vtkActor object, and adds
2168  * all the required information to the internal cloud_actor_map_ object.
2169  * \param[in] geometry_handler the geometric handler that contains the XYZ data
2170  * \param[in] color_handler the color handler that contains the "RGB" (scalar) data
2171  * \param[in] id the point cloud object id
2172  * \param[in] viewport the view port where the Point Cloud should be added
2173  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
2174  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
2175  */
2176  template <typename PointT> bool
2177  fromHandlersToScreen (const GeometryHandlerConstPtr &geometry_handler,
2178  const PointCloudColorHandler<PointT> &color_handler,
2179  const std::string &id,
2180  int viewport,
2181  const Eigen::Vector4f& sensor_origin = Eigen::Vector4f (0, 0, 0, 0),
2182  const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternion<float> (1, 0, 0 ,0));
2183 
2184  /** \brief Allocate a new polydata smartpointer. Internal
2185  * \param[out] polydata the resultant poly data
2186  */
2187  void
2188  allocVtkPolyData (vtkSmartPointer<vtkAppendPolyData> &polydata);
2189 
2190  /** \brief Allocate a new polydata smartpointer. Internal
2191  * \param[out] polydata the resultant poly data
2192  */
2193  void
2194  allocVtkPolyData (vtkSmartPointer<vtkPolyData> &polydata);
2195 
2196  /** \brief Allocate a new unstructured grid smartpointer. Internal
2197  * \param[out] polydata the resultant poly data
2198  */
2199  void
2201 
2202  /** \brief Transform the point cloud viewpoint to a transformation matrix
2203  * \param[in] origin the camera origin
2204  * \param[in] orientation the camera orientation
2205  * \param[out] transformation the camera transformation matrix
2206  */
2207  void
2208  getTransformationMatrix (const Eigen::Vector4f &origin,
2209  const Eigen::Quaternion<float> &orientation,
2210  Eigen::Matrix4f &transformation);
2211 
2212  /** \brief Fills a vtkTexture structure from pcl::TexMaterial.
2213  * \param[in] tex_mat texture material in PCL format
2214  * \param[out] vtk_tex texture material in VTK format
2215  * \return 0 on success and -1 else.
2216  * \note for now only image based textures are supported, image file must be in
2217  * tex_file attribute of \a tex_mat.
2218  */
2219  int
2220  textureFromTexMaterial (const pcl::TexMaterial& tex_mat,
2221  vtkTexture* vtk_tex) const;
2222 
2223  /** \brief Get camera file for camera parameter saving/restoring from command line.
2224  * Camera filename is calculated using sha1 value of all pathes of input .pcd files
2225  * \return empty string if failed.
2226  */
2227  std::string
2228  getUniqueCameraFile (int argc, char **argv);
2229 
2230  //There's no reason these conversion functions shouldn't be public and static so others can use them.
2231  public:
2232  /** \brief Convert Eigen::Matrix4f to vtkMatrix4x4
2233  * \param[in] m the input Eigen matrix
2234  * \param[out] vtk_matrix the resultant VTK matrix
2235  */
2236  static void
2237  convertToVtkMatrix (const Eigen::Matrix4f &m,
2238  vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
2239 
2240  /** \brief Convert origin and orientation to vtkMatrix4x4
2241  * \param[in] origin the point cloud origin
2242  * \param[in] orientation the point cloud orientation
2243  * \param[out] vtk_matrix the resultant VTK 4x4 matrix
2244  */
2245  static void
2246  convertToVtkMatrix (const Eigen::Vector4f &origin,
2247  const Eigen::Quaternion<float> &orientation,
2248  vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
2249 
2250  /** \brief Convert vtkMatrix4x4 to an Eigen4f
2251  * \param[in] vtk_matrix the original VTK 4x4 matrix
2252  * \param[out] m the resultant Eigen 4x4 matrix
2253  */
2254  static void
2255  convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix,
2256  Eigen::Matrix4f &m);
2257 
2258  };
2259  }
2260 }
2261 
2262 #include <pcl/visualization/impl/pcl_visualizer.hpp>
2263 
2264 #endif
2265 
static PCLVisualizerInteractorStyle * New()
vtkSmartPointer< PCLVisualizerInteractorStyle > getInteractorStyle()
Get a pointer to the current interactor style used.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
PointCloudGeometryHandler< pcl::PCLPointCloud2 > GeometryHandler
PointCloudColorHandler< pcl::PCLPointCloud2 > ColorHandler
bool updatePointCloud(const pcl::PointCloud< pcl::PointXYZL >::ConstPtr &cloud, const std::string &id="cloud")
Updates the XYZL data for an existing cloud object id on screen.
GeometryHandler::Ptr GeometryHandlerPtr
bool addPointCloud(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a PointXYZRGB Point Cloud to screen.
bool updatePointCloud(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &cloud, const std::string &id="cloud")
Updates the XYZRGB data for an existing cloud object id on screen.
boost::shared_ptr< const PCLVisualizer > ConstPtr
bool addPointCloud(const pcl::PointCloud< pcl::PointXYZL >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a PointXYZL Point Cloud to screen.
boost::shared_ptr< CoordinateActorMap > CoordinateActorMapPtr
Definition: actor_map.h:106
ColorHandler::ConstPtr ColorHandlerConstPtr
boost::shared_ptr< const PointCloudGeometryHandler< PointCloud > > ConstPtr
Camera class holds a set of camera parameters together with the window pos/size.
Definition: common.h:147
boost::shared_ptr< const PointCloudColorHandler< PointCloud > > ConstPtr
bool contains(const std::string &id) const
Check if the cloud, shape, or coordinate with the given id was already added to this vizualizer...
vtkSmartPointer< vtkRendererCollection > getRendererCollection()
Return a pointer to the underlying VTK Renderer Collection.
std::vector< PointCloud< PointT >, Eigen::aligned_allocator< PointCloud< PointT > > > CloudVectorType
Definition: point_cloud.h:427
/brief Class representing 3D point picking events.
Definition: bfgs.h:10
bool updatePointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud, const std::string &id="cloud")
Updates the XYZ data for an existing cloud object id on screen.
boost::shared_ptr< CloudActorMap > CloudActorMapPtr
Definition: actor_map.h:100
Base Handler class for PointCloud colors.
bool addPointCloud(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a PointXYZ Point Cloud to screen.
vtkSmartPointer< vtkRenderWindowInteractor > interactor_
The render window interactor.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
boost::signals2::connection registerKeyboardCallback(void(*callback)(const pcl::visualization::KeyboardEvent &, void *), void *cookie=NULL)
Register a callback function for keyboard events.
boost::signals2::connection registerAreaPickingCallback(void(T::*callback)(const pcl::visualization::AreaPickingEvent &, void *), T &instance, void *cookie=NULL)
Register a callback function for area picking events.
boost::shared_ptr< PCLVisualizer > Ptr
bool updateCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const pcl::Correspondences &correspondences, const std::string &id="correspondences", int viewport=0)
Update the specified correspondences to the display.
PCL Visualizer main class.
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
ShapeActorMapPtr getShapeActorMap()
Return a pointer to the ShapeActorMap this visualizer uses.
boost::shared_ptr< PointCloudGeometryHandler< PointCloud > > Ptr
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
PCL_EXPORTS void allocVtkUnstructuredGrid(vtkSmartPointer< vtkUnstructuredGrid > &polydata)
Allocate a new unstructured grid smartpointer.
PCLVisualizerInteractorStyle defines an unique, custom VTK based interactory style for PCL Visualizer...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
bool addCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const pcl::Correspondences &correspondences, const std::string &id="correspondences", int viewport=0)
Add the specified correspondences to the display.
boost::shared_ptr< PointCloudColorHandler< PointCloud > > Ptr
vtkSmartPointer< vtkRenderWindow > getRenderWindow()
Return a pointer to the underlying VTK Render Window used.
CloudActorMapPtr getCloudActorMap()
Return a pointer to the CloudActorMap this visualizer uses.
boost::signals2::connection registerKeyboardCallback(void(T::*callback)(const pcl::visualization::KeyboardEvent &, void *), T &instance, void *cookie=NULL)
Register a callback function for keyboard events.
Base handler class for PointCloud geometry.
/brief Class representing 3D area picking events.
GeometryHandler::ConstPtr GeometryHandlerConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.
/brief Class representing key hit/release events
boost::shared_ptr< ShapeActorMap > ShapeActorMapPtr
Definition: actor_map.h:103
boost::signals2::connection registerMouseCallback(void(*callback)(const pcl::visualization::MouseEvent &, void *), void *cookie=NULL)
Register a callback function for mouse events.
boost::signals2::connection registerMouseCallback(void(T::*callback)(const pcl::visualization::MouseEvent &, void *), T &instance, void *cookie=NULL)
Register a callback function for mouse events.
bool addPointCloud(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a PointXYZRGBA Point Cloud to screen.
bool updatePointCloud(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &cloud, const std::string &id="cloud")
Updates the XYZRGBA data for an existing cloud object id on screen.
boost::signals2::connection registerPointPickingCallback(void(T::*callback)(const pcl::visualization::PointPickingEvent &, void *), T &instance, void *cookie=NULL)
Register a callback function for point picking events.