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
1152  * \param[in] property the property type
1153  * \param[in] value the value to be set
1154  * \param[in] id the point cloud object id (default: cloud)
1155  * \param[in] viewport the view port where the Point Cloud's rendering properties should be modified (default: all)
1156  * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
1157  */
1158  bool
1159  setPointCloudRenderingProperties (int property, double value,
1160  const std::string &id = "cloud", int viewport = 0);
1161 
1162  /** \brief Get the rendering properties of a PointCloud
1163  * \param[in] property the property type
1164  * \param[in] value the resultant property value
1165  * \param[in] id the point cloud object id (default: cloud)
1166  * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
1167  */
1168  bool
1169  getPointCloudRenderingProperties (int property, double &value,
1170  const std::string &id = "cloud");
1171 
1172  /** \brief Set whether the point cloud is selected or not
1173  * \param[in] selected whether the cloud is selected or not (true = selected)
1174  * \param[in] id the point cloud object id (default: cloud)
1175  */
1176  bool
1177  setPointCloudSelected (const bool selected, const std::string &id = "cloud" );
1178 
1179  /** \brief Set the rendering properties of a shape
1180  * \param[in] property the property type
1181  * \param[in] value the value to be set
1182  * \param[in] id the shape object id
1183  * \param[in] viewport the view port where the shape's properties should be modified (default: all)
1184  * \note When using \ref addPolygonMesh you you should use \ref setPointCloudRenderingProperties
1185  * \note The list of properties can be found in \ref pcl::visualization::LookUpTableRepresentationProperties.
1186  */
1187  bool
1188  setShapeRenderingProperties (int property, double value,
1189  const std::string &id, int viewport = 0);
1190 
1191  /** \brief Set the rendering properties of a shape (3x values - e.g., RGB)
1192  * \param[in] property the property type
1193  * \param[in] val1 the first value to be set
1194  * \param[in] val2 the second value to be set
1195  * \param[in] val3 the third value to be set
1196  * \param[in] id the shape object id
1197  * \param[in] viewport the view port where the shape's properties should be modified (default: all)
1198  * \note When using \ref addPolygonMesh you you should use \ref setPointCloudRenderingProperties
1199  */
1200  bool
1201  setShapeRenderingProperties (int property, double val1, double val2, double val3,
1202  const std::string &id, int viewport = 0);
1203 
1204  /** \brief Returns true when the user tried to close the window */
1205  bool
1206  wasStopped () const;
1207 
1208  /** \brief Set the stopped flag back to false */
1209  void
1210  resetStoppedFlag ();
1211 
1212  /** \brief Stop the interaction and close the visualizaton window. */
1213  void
1214  close ();
1215 
1216  /** \brief Create a new viewport from [xmin,ymin] -> [xmax,ymax].
1217  * \param[in] xmin the minimum X coordinate for the viewport (0.0 <= 1.0)
1218  * \param[in] ymin the minimum Y coordinate for the viewport (0.0 <= 1.0)
1219  * \param[in] xmax the maximum X coordinate for the viewport (0.0 <= 1.0)
1220  * \param[in] ymax the maximum Y coordinate for the viewport (0.0 <= 1.0)
1221  * \param[in] viewport the id of the new viewport
1222  *
1223  * \note If no renderer for the current window exists, one will be created, and
1224  * the viewport will be set to 0 ('all'). In case one or multiple renderers do
1225  * exist, the viewport ID will be set to the total number of renderers - 1.
1226  */
1227  void
1228  createViewPort (double xmin, double ymin, double xmax, double ymax, int &viewport);
1229 
1230  /** \brief Create a new separate camera for the given viewport.
1231  * \param[in] viewport the viewport to create a new camera for.
1232  */
1233  void
1234  createViewPortCamera (const int viewport);
1235 
1236  /** \brief Add a polygon (polyline) that represents the input point cloud (connects all
1237  * points in order)
1238  * \param[in] cloud the point cloud dataset representing the polygon
1239  * \param[in] r the red channel of the color that the polygon should be rendered with
1240  * \param[in] g the green channel of the color that the polygon should be rendered with
1241  * \param[in] b the blue channel of the color that the polygon should be rendered with
1242  * \param[in] id (optional) the polygon id/name (default: "polygon")
1243  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1244  */
1245  template <typename PointT> bool
1246  addPolygon (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1247  double r, double g, double b,
1248  const std::string &id = "polygon", int viewport = 0);
1249 
1250  /** \brief Add a polygon (polyline) that represents the input point cloud (connects all
1251  * points in order)
1252  * \param[in] cloud the point cloud dataset representing the polygon
1253  * \param[in] id the polygon id/name (default: "polygon")
1254  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1255  */
1256  template <typename PointT> bool
1257  addPolygon (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
1258  const std::string &id = "polygon",
1259  int viewport = 0);
1260 
1261  /** \brief Add a planar polygon that represents the input point cloud (connects all points in order)
1262  * \param[in] polygon the polygon to draw
1263  * \param[in] r the red channel of the color that the polygon should be rendered with
1264  * \param[in] g the green channel of the color that the polygon should be rendered with
1265  * \param[in] b the blue channel of the color that the polygon should be rendered with
1266  * \param[in] id the polygon id/name (default: "polygon")
1267  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1268  */
1269  template <typename PointT> bool
1270  addPolygon (const pcl::PlanarPolygon<PointT> &polygon,
1271  double r, double g, double b,
1272  const std::string &id = "polygon",
1273  int viewport = 0);
1274 
1275  /** \brief Add a line segment from two points
1276  * \param[in] pt1 the first (start) point on the line
1277  * \param[in] pt2 the second (end) point on the line
1278  * \param[in] id the line id/name (default: "line")
1279  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1280  */
1281  template <typename P1, typename P2> bool
1282  addLine (const P1 &pt1, const P2 &pt2, const std::string &id = "line",
1283  int viewport = 0);
1284 
1285  /** \brief Add a line segment from two points
1286  * \param[in] pt1 the first (start) point on the line
1287  * \param[in] pt2 the second (end) point on the line
1288  * \param[in] r the red channel of the color that the line should be rendered with
1289  * \param[in] g the green channel of the color that the line should be rendered with
1290  * \param[in] b the blue channel of the color that the line should be rendered with
1291  * \param[in] id the line id/name (default: "line")
1292  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1293  */
1294  template <typename P1, typename P2> bool
1295  addLine (const P1 &pt1, const P2 &pt2, double r, double g, double b,
1296  const std::string &id = "line", int viewport = 0);
1297 
1298  /** \brief Add a line arrow segment between two points, and display the distance between them
1299  *
1300  * Arrow heads are attached to both end points of the arrow.
1301  *
1302  * \param[in] pt1 the first (start) point on the line
1303  * \param[in] pt2 the second (end) point on the line
1304  * \param[in] r the red channel of the color that the line should be rendered with
1305  * \param[in] g the green channel of the color that the line should be rendered with
1306  * \param[in] b the blue channel of the color that the line should be rendered with
1307  * \param[in] id the arrow id/name (default: "arrow")
1308  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1309  */
1310  template <typename P1, typename P2> bool
1311  addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b,
1312  const std::string &id = "arrow", int viewport = 0);
1313 
1314  /** \brief Add a line arrow segment between two points, and (optianally) display the distance between them
1315  *
1316  * Arrow head is attached on the **start** point (\c pt1) of the arrow.
1317  *
1318  * \param[in] pt1 the first (start) point on the line
1319  * \param[in] pt2 the second (end) point on the line
1320  * \param[in] r the red channel of the color that the line should be rendered with
1321  * \param[in] g the green channel of the color that the line should be rendered with
1322  * \param[in] b the blue channel of the color that the line should be rendered with
1323  * \param[in] display_length true if the length should be displayed on the arrow as text
1324  * \param[in] id the line id/name (default: "arrow")
1325  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1326  */
1327  template <typename P1, typename P2> bool
1328  addArrow (const P1 &pt1, const P2 &pt2, double r, double g, double b, bool display_length,
1329  const std::string &id = "arrow", int viewport = 0);
1330 
1331  /** \brief Add a line arrow segment between two points, and display the distance between them in a given color
1332  *
1333  * Arrow heads are attached to both end points of the arrow.
1334  *
1335  * \param[in] pt1 the first (start) point on the line
1336  * \param[in] pt2 the second (end) point on the line
1337  * \param[in] r_line the red channel of the color that the line should be rendered with
1338  * \param[in] g_line the green channel of the color that the line should be rendered with
1339  * \param[in] b_line the blue channel of the color that the line should be rendered with
1340  * \param[in] r_text the red channel of the color that the text should be rendered with
1341  * \param[in] g_text the green channel of the color that the text should be rendered with
1342  * \param[in] b_text the blue channel of the color that the text should be rendered with
1343  * \param[in] id the line id/name (default: "arrow")
1344  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1345  */
1346  template <typename P1, typename P2> bool
1347  addArrow (const P1 &pt1, const P2 &pt2,
1348  double r_line, double g_line, double b_line,
1349  double r_text, double g_text, double b_text,
1350  const std::string &id = "arrow", int viewport = 0);
1351 
1352 
1353  /** \brief Add a sphere shape from a point and a radius
1354  * \param[in] center the center of the sphere
1355  * \param[in] radius the radius of the sphere
1356  * \param[in] id the sphere id/name (default: "sphere")
1357  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1358  */
1359  template <typename PointT> bool
1360  addSphere (const PointT &center, double radius, const std::string &id = "sphere",
1361  int viewport = 0);
1362 
1363  /** \brief Add a sphere shape from a point and a radius
1364  * \param[in] center the center of the sphere
1365  * \param[in] radius the radius of the sphere
1366  * \param[in] r the red channel of the color that the sphere should be rendered with
1367  * \param[in] g the green channel of the color that the sphere should be rendered with
1368  * \param[in] b the blue channel of the color that the sphere should be rendered with
1369  * \param[in] id the sphere id/name (default: "sphere")
1370  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1371  */
1372  template <typename PointT> bool
1373  addSphere (const PointT &center, double radius, double r, double g, double b,
1374  const std::string &id = "sphere", int viewport = 0);
1375 
1376  /** \brief Update an existing sphere shape from a point and a radius
1377  * \param[in] center the center of the sphere
1378  * \param[in] radius the radius of the sphere
1379  * \param[in] r the red channel of the color that the sphere should be rendered with
1380  * \param[in] g the green channel of the color that the sphere should be rendered with
1381  * \param[in] b the blue channel of the color that the sphere should be rendered with
1382  * \param[in] id the sphere id/name (default: "sphere")
1383  */
1384  template <typename PointT> bool
1385  updateSphere (const PointT &center, double radius, double r, double g, double b,
1386  const std::string &id = "sphere");
1387 
1388  /** \brief Add a vtkPolydata as a mesh
1389  * \param[in] polydata vtkPolyData
1390  * \param[in] id the model id/name (default: "PolyData")
1391  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1392  */
1393  bool
1394  addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata,
1395  const std::string & id = "PolyData",
1396  int viewport = 0);
1397 
1398  /** \brief Add a vtkPolydata as a mesh
1399  * \param[in] polydata vtkPolyData
1400  * \param[in] transform transformation to apply
1401  * \param[in] id the model id/name (default: "PolyData")
1402  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1403  */
1404  bool
1405  addModelFromPolyData (vtkSmartPointer<vtkPolyData> polydata,
1407  const std::string &id = "PolyData",
1408  int viewport = 0);
1409 
1410  /** \brief Add a PLYmodel as a mesh
1411  * \param[in] filename of the ply file
1412  * \param[in] id the model id/name (default: "PLYModel")
1413  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1414  */
1415  bool
1416  addModelFromPLYFile (const std::string &filename,
1417  const std::string &id = "PLYModel",
1418  int viewport = 0);
1419 
1420  /** \brief Add a PLYmodel as a mesh and applies given transformation
1421  * \param[in] filename of the ply file
1422  * \param[in] transform transformation to apply
1423  * \param[in] id the model id/name (default: "PLYModel")
1424  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1425  */
1426  bool
1427  addModelFromPLYFile (const std::string &filename,
1429  const std::string &id = "PLYModel",
1430  int viewport = 0);
1431 
1432  /** \brief Add a cylinder from a set of given model coefficients
1433  * \param[in] coefficients the model coefficients (point_on_axis, axis_direction, radius)
1434  * \param[in] id the cylinder id/name (default: "cylinder")
1435  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1436  *
1437  * \code
1438  * // The following are given (or computed using sample consensus techniques)
1439  * // See SampleConsensusModelCylinder for more information.
1440  * // Eigen::Vector3f pt_on_axis, axis_direction;
1441  * // float radius;
1442  *
1443  * pcl::ModelCoefficients cylinder_coeff;
1444  * cylinder_coeff.values.resize (7); // We need 7 values
1445  * cylinder_coeff.values[0] = pt_on_axis.x ();
1446  * cylinder_coeff.values[1] = pt_on_axis.y ();
1447  * cylinder_coeff.values[2] = pt_on_axis.z ();
1448  *
1449  * cylinder_coeff.values[3] = axis_direction.x ();
1450  * cylinder_coeff.values[4] = axis_direction.y ();
1451  * cylinder_coeff.values[5] = axis_direction.z ();
1452  *
1453  * cylinder_coeff.values[6] = radius;
1454  *
1455  * addCylinder (cylinder_coeff);
1456  * \endcode
1457  */
1458  bool
1459  addCylinder (const pcl::ModelCoefficients &coefficients,
1460  const std::string &id = "cylinder",
1461  int viewport = 0);
1462 
1463  /** \brief Add a sphere from a set of given model coefficients
1464  * \param[in] coefficients the model coefficients (sphere center, radius)
1465  * \param[in] id the sphere id/name (default: "sphere")
1466  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1467  *
1468  * \code
1469  * // The following are given (or computed using sample consensus techniques)
1470  * // See SampleConsensusModelSphere for more information
1471  * // Eigen::Vector3f sphere_center;
1472  * // float radius;
1473  *
1474  * pcl::ModelCoefficients sphere_coeff;
1475  * sphere_coeff.values.resize (4); // We need 4 values
1476  * sphere_coeff.values[0] = sphere_center.x ();
1477  * sphere_coeff.values[1] = sphere_center.y ();
1478  * sphere_coeff.values[2] = sphere_center.z ();
1479  *
1480  * sphere_coeff.values[3] = radius;
1481  *
1482  * addSphere (sphere_coeff);
1483  * \endcode
1484  */
1485  bool
1486  addSphere (const pcl::ModelCoefficients &coefficients,
1487  const std::string &id = "sphere",
1488  int viewport = 0);
1489 
1490  /** \brief Add a line from a set of given model coefficients
1491  * \param[in] coefficients the model coefficients (point_on_line, direction)
1492  * \param[in] id the line id/name (default: "line")
1493  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1494  *
1495  * \code
1496  * // The following are given (or computed using sample consensus techniques)
1497  * // See SampleConsensusModelLine for more information
1498  * // Eigen::Vector3f point_on_line, line_direction;
1499  *
1500  * pcl::ModelCoefficients line_coeff;
1501  * line_coeff.values.resize (6); // We need 6 values
1502  * line_coeff.values[0] = point_on_line.x ();
1503  * line_coeff.values[1] = point_on_line.y ();
1504  * line_coeff.values[2] = point_on_line.z ();
1505  *
1506  * line_coeff.values[3] = line_direction.x ();
1507  * line_coeff.values[4] = line_direction.y ();
1508  * line_coeff.values[5] = line_direction.z ();
1509  *
1510  * addLine (line_coeff);
1511  * \endcode
1512  */
1513  bool
1514  addLine (const pcl::ModelCoefficients &coefficients,
1515  const std::string &id = "line",
1516  int viewport = 0);
1517 
1518  /** \brief Add a plane from a set of given model coefficients
1519  * \param[in] coefficients the model coefficients (a, b, c, d with ax+by+cz+d=0)
1520  * \param[in] id the plane id/name (default: "plane")
1521  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1522  *
1523  * \code
1524  * // The following are given (or computed using sample consensus techniques)
1525  * // See SampleConsensusModelPlane for more information
1526  * // Eigen::Vector4f plane_parameters;
1527  *
1528  * pcl::ModelCoefficients plane_coeff;
1529  * plane_coeff.values.resize (4); // We need 4 values
1530  * plane_coeff.values[0] = plane_parameters.x ();
1531  * plane_coeff.values[1] = plane_parameters.y ();
1532  * plane_coeff.values[2] = plane_parameters.z ();
1533  * plane_coeff.values[3] = plane_parameters.w ();
1534  *
1535  * addPlane (plane_coeff);
1536  * \endcode
1537  */
1538  bool
1539  addPlane (const pcl::ModelCoefficients &coefficients,
1540  const std::string &id = "plane",
1541  int viewport = 0);
1542 
1543  bool
1544  addPlane (const pcl::ModelCoefficients &coefficients, double x, double y, double z,
1545  const std::string &id = "plane",
1546  int viewport = 0);
1547  /** \brief Add a circle from a set of given model coefficients
1548  * \param[in] coefficients the model coefficients (x, y, radius)
1549  * \param[in] id the circle id/name (default: "circle")
1550  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1551  *
1552  * \code
1553  * // The following are given (or computed using sample consensus techniques)
1554  * // See SampleConsensusModelCircle2D for more information
1555  * // float x, y, radius;
1556  *
1557  * pcl::ModelCoefficients circle_coeff;
1558  * circle_coeff.values.resize (3); // We need 3 values
1559  * circle_coeff.values[0] = x;
1560  * circle_coeff.values[1] = y;
1561  * circle_coeff.values[2] = radius;
1562  *
1563  * vtkSmartPointer<vtkDataSet> data = pcl::visualization::create2DCircle (circle_coeff, z);
1564  * \endcode
1565  */
1566  bool
1567  addCircle (const pcl::ModelCoefficients &coefficients,
1568  const std::string &id = "circle",
1569  int viewport = 0);
1570 
1571  /** \brief Add a cone from a set of given model coefficients
1572  * \param[in] coefficients the model coefficients (see \ref pcl::visualization::createCone)
1573  * \param[in] id the cone id/name (default: "cone")
1574  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1575  */
1576  bool
1577  addCone (const pcl::ModelCoefficients &coefficients,
1578  const std::string &id = "cone",
1579  int viewport = 0);
1580 
1581  /** \brief Add a cube from a set of given model coefficients
1582  * \param[in] coefficients the model coefficients (see \ref pcl::visualization::createCube)
1583  * \param[in] id the cube id/name (default: "cube")
1584  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1585  */
1586  bool
1587  addCube (const pcl::ModelCoefficients &coefficients,
1588  const std::string &id = "cube",
1589  int viewport = 0);
1590 
1591  /** \brief Add a cube from a set of given model coefficients
1592  * \param[in] translation a translation to apply to the cube from 0,0,0
1593  * \param[in] rotation a quaternion-based rotation to apply to the cube
1594  * \param[in] width the cube's width
1595  * \param[in] height the cube's height
1596  * \param[in] depth the cube's depth
1597  * \param[in] id the cube id/name (default: "cube")
1598  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1599  */
1600  bool
1601  addCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation,
1602  double width, double height, double depth,
1603  const std::string &id = "cube",
1604  int viewport = 0);
1605 
1606  /** \brief Add a cube
1607  * \param[in] x_min the min X coordinate
1608  * \param[in] x_max the max X coordinate
1609  * \param[in] y_min the min Y coordinate
1610  * \param[in] y_max the max Y coordinate
1611  * \param[in] z_min the min Z coordinate
1612  * \param[in] z_max the max Z coordinate
1613  * \param[in] r how much red (0.0 -> 1.0)
1614  * \param[in] g how much green (0.0 -> 1.0)
1615  * \param[in] b how much blue (0.0 -> 1.0)
1616  * \param[in] id the cube id/name (default: "cube")
1617  * \param[in] viewport (optional) the id of the new viewport (default: 0)
1618  */
1619  bool
1620  addCube (float x_min, float x_max, float y_min, float y_max, float z_min, float z_max,
1621  double r = 1.0, double g = 1.0, double b = 1.0, const std::string &id = "cube", int viewport = 0);
1622 
1623  /** \brief Changes the visual representation for all actors to surface representation. */
1624  void
1625  setRepresentationToSurfaceForAllActors ();
1626 
1627  /** \brief Changes the visual representation for all actors to points representation. */
1628  void
1629  setRepresentationToPointsForAllActors ();
1630 
1631  /** \brief Changes the visual representation for all actors to wireframe representation. */
1632  void
1633  setRepresentationToWireframeForAllActors ();
1634 
1635  /** \brief Sets whether the 2D overlay text showing the framerate of the window is displayed or not.
1636  * \param[in] show_fps determines whether the fps text will be shown or not.
1637  */
1638  void
1639  setShowFPS (bool show_fps);
1640 
1641  /** \brief Renders a virtual scene as seen from the camera viewpoint and returns the rendered point cloud.
1642  * ATT: This method will only render the scene if only on viewport exists. Otherwise, returns an empty
1643  * point cloud and exits immediately.
1644  * \param[in] xres is the size of the window (X) used to render the scene
1645  * \param[in] yres is the size of the window (Y) used to render the scene
1646  * \param[in] cloud is the rendered point cloud
1647  */
1648  void
1649  renderView (int xres, int yres, pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
1650 
1651  /** \brief The purpose of this method is to render a CAD model added to the visualizer from different viewpoints
1652  * in order to simulate partial views of model. The viewpoint locations are the vertices of a tesselated sphere
1653  * build from an icosaheadron. The tesselation paremeter controls how many times the triangles of the original
1654  * icosahedron are divided to approximate the sphere and thus the number of partial view generated for a model,
1655  * with a tesselation_level of 0, 12 views are generated if use_vertices=true and 20 views if use_vertices=false
1656  *
1657  * \param[in] xres the size of the window (X) used to render the partial view of the object
1658  * \param[in] yres the size of the window (Y) used to render the partial view of the object
1659  * \param[in] cloud is a vector of pointcloud with XYZ information that represent the model as seen from the respective viewpoints.
1660  * \param[out] poses represent the transformation from object coordinates to camera coordinates for the respective viewpoint.
1661  * \param[out] enthropies are values between 0 and 1 representing which percentage of the model is seen from the respective viewpoint.
1662  * \param[in] tesselation_level represents the number of subdivisions applied to the triangles of original icosahedron.
1663  * \param[in] view_angle field of view of the virtual camera. Default: 45
1664  * \param[in] radius_sphere the tesselated sphere radius. Default: 1
1665  * \param[in] use_vertices if true, use the vertices of tesselated icosahedron (12,42,...) or if false, use the faces of tesselated
1666  * icosahedron (20,80,...). Default: true
1667  */
1668  void
1669  renderViewTesselatedSphere (
1670  int xres, int yres,
1672  std::vector<Eigen::Matrix4f,Eigen::aligned_allocator< Eigen::Matrix4f > > & poses, std::vector<float> & enthropies, int tesselation_level,
1673  float view_angle = 45, float radius_sphere = 1, bool use_vertices = true);
1674 
1675 
1676  /** \brief Initialize camera parameters with some default values. */
1677  void
1678  initCameraParameters ();
1679 
1680  /** \brief Search for camera parameters at the command line and set them internally.
1681  * \param[in] argc
1682  * \param[in] argv
1683  */
1684  bool
1685  getCameraParameters (int argc, char **argv);
1686 
1687  /** \brief Load camera parameters from a camera parameters file.
1688  * \param[in] file the name of the camera parameters file
1689  */
1690  bool
1691  loadCameraParameters (const std::string &file);
1692 
1693  /** \brief Checks whether the camera parameters were manually loaded.
1694  * \return True if valid "-cam" option is available in command line.
1695  * \sa cameraFileLoaded ()
1696  */
1697  bool
1698  cameraParamsSet () const;
1699 
1700  /** \brief Checks whether a camera file were automatically loaded.
1701  * \return True if a valid camera file is automatically loaded.
1702  * \note The camera file is saved by pressing "ctrl + s" during last run of the program
1703  * and restored automatically when the program runs this time.
1704  * \sa cameraParamsSet ()
1705  */
1706  bool
1707  cameraFileLoaded () const;
1708 
1709  /** \brief Get camera file for camera parameter saving/restoring.
1710  * \note This will be valid only when valid "-cam" option were available in command line
1711  * or a saved camera file were automatically loaded.
1712  * \sa cameraParamsSet (), cameraFileLoaded ()
1713  */
1714  std::string
1715  getCameraFile () const;
1716 
1717  /** \brief Update camera parameters and render. */
1718  void
1719  updateCamera ();
1720 
1721  /** \brief Reset camera parameters and render. */
1722  void
1723  resetCamera ();
1724 
1725  /** \brief Reset the camera direction from {0, 0, 0} to the center_{x, y, z} of a given dataset.
1726  * \param[in] id the point cloud object id (default: cloud)
1727  */
1728  void
1729  resetCameraViewpoint (const std::string &id = "cloud");
1730 
1731  /** \brief Set the camera pose given by position, viewpoint and up vector
1732  * \param[in] pos_x the x coordinate of the camera location
1733  * \param[in] pos_y the y coordinate of the camera location
1734  * \param[in] pos_z the z coordinate of the camera location
1735  * \param[in] view_x the x component of the view point of the camera
1736  * \param[in] view_y the y component of the view point of the camera
1737  * \param[in] view_z the z component of the view point of the camera
1738  * \param[in] up_x the x component of the view up direction of the camera
1739  * \param[in] up_y the y component of the view up direction of the camera
1740  * \param[in] up_z the y component of the view up direction of the camera
1741  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1742  */
1743  void
1744  setCameraPosition (double pos_x, double pos_y, double pos_z,
1745  double view_x, double view_y, double view_z,
1746  double up_x, double up_y, double up_z, int viewport = 0);
1747 
1748  /** \brief Set the camera location and viewup according to the given arguments
1749  * \param[in] pos_x the x coordinate of the camera location
1750  * \param[in] pos_y the y coordinate of the camera location
1751  * \param[in] pos_z the z coordinate of the camera location
1752  * \param[in] up_x the x component of the view up direction of the camera
1753  * \param[in] up_y the y component of the view up direction of the camera
1754  * \param[in] up_z the z component of the view up direction of the camera
1755  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1756  */
1757  void
1758  setCameraPosition (double pos_x, double pos_y, double pos_z,
1759  double up_x, double up_y, double up_z, int viewport = 0);
1760 
1761  /** \brief Set the camera parameters via an intrinsics and and extrinsics matrix
1762  * \note This assumes that the pixels are square and that the center of the image is at the center of the sensor.
1763  * \param[in] intrinsics the intrinsics that will be used to compute the VTK camera parameters
1764  * \param[in] extrinsics the extrinsics that will be used to compute the VTK camera parameters
1765  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1766  */
1767  void
1768  setCameraParameters (const Eigen::Matrix3f &intrinsics, const Eigen::Matrix4f &extrinsics, int viewport = 0);
1769 
1770  /** \brief Set the camera parameters by given a full camera data structure.
1771  * \param[in] camera camera structure containing all the camera parameters.
1772  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1773  */
1774  void
1775  setCameraParameters (const Camera &camera, int viewport = 0);
1776 
1777  /** \brief Set the camera clipping distances.
1778  * \param[in] near the near clipping distance (no objects closer than this to the camera will be drawn)
1779  * \param[in] far the far clipping distance (no objects further away than this to the camera will be drawn)
1780  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1781  */
1782  void
1783  setCameraClipDistances (double near, double far, int viewport = 0);
1784 
1785  /** \brief Set the camera vertical field of view.
1786  * \param[in] fovy vertical field of view in radians
1787  * \param[in] viewport the viewport to modify camera of (0 modifies all cameras)
1788  */
1789  void
1790  setCameraFieldOfView (double fovy, int viewport = 0);
1791 
1792  /** \brief Get the current camera parameters. */
1793  void
1794  getCameras (std::vector<Camera>& cameras);
1795 
1796 
1797  /** \brief Get the current viewing pose. */
1798  Eigen::Affine3f
1799  getViewerPose (int viewport = 0);
1800 
1801  /** \brief Save the current rendered image to disk, as a PNG screenshot.
1802  * \param[in] file the name of the PNG file
1803  */
1804  void
1805  saveScreenshot (const std::string &file);
1806 
1807  /** \brief Save the camera parameters to disk, as a .cam file.
1808  * \param[in] file the name of the .cam file
1809  */
1810  void
1811  saveCameraParameters (const std::string &file);
1812 
1813  /** \brief Get camera parameters and save them to a pcl::visualization::Camera.
1814  * \param[out] camera the name of the pcl::visualization::Camera
1815  */
1816  void
1817  getCameraParameters (Camera &camera);
1818 
1819  /** \brief Return a pointer to the underlying VTK Render Window used. */
1822  {
1823  return (win_);
1824  }
1825 
1826  /** \brief Return a pointer to the underlying VTK Renderer Collection. */
1829  {
1830  return (rens_);
1831  }
1832 
1833  /** \brief Return a pointer to the CloudActorMap this visualizer uses. */
1836  {
1837  return (cloud_actor_map_);
1838  }
1839 
1840  /** \brief Return a pointer to the ShapeActorMap this visualizer uses. */
1843  {
1844  return (shape_actor_map_);
1845  }
1846 
1847  /** \brief Set the position in screen coordinates.
1848  * \param[in] x where to move the window to (X)
1849  * \param[in] y where to move the window to (Y)
1850  */
1851  void
1852  setPosition (int x, int y);
1853 
1854  /** \brief Set the window size in screen coordinates.
1855  * \param[in] xw window size in horizontal (pixels)
1856  * \param[in] yw window size in vertical (pixels)
1857  */
1858  void
1859  setSize (int xw, int yw);
1860 
1861  /** \brief Use Vertex Buffer Objects renderers.
1862  * \param[in] use_vbos set to true to use VBOs
1863  */
1864  void
1865  setUseVbos (bool use_vbos);
1866 
1867  /** \brief Set the ID of a cloud or shape to be used for LUT display
1868  * \param[in] id The id of the cloud/shape look up table to be displayed
1869  * The look up table is displayed by pressing 'u' in the PCLVisualizer */
1870  void
1871  setLookUpTableID (const std::string id);
1872 
1873  /** \brief Create the internal Interactor object. */
1874  void
1875  createInteractor ();
1876 
1877  /** \brief Set up our unique PCL interactor style for a given vtkRenderWindowInteractor object
1878  * attached to a given vtkRenderWindow
1879  * \param[in,out] iren the vtkRenderWindowInteractor object to set up
1880  * \param[in,out] win a vtkRenderWindow object that the interactor is attached to
1881  */
1882  void
1883  setupInteractor (vtkRenderWindowInteractor *iren,
1884  vtkRenderWindow *win);
1885 
1886  /** \brief Set up PCLVisualizer with custom interactor style for a given vtkRenderWindowInteractor object
1887  * attached to a given vtkRenderWindow
1888  * \param[in,out] iren the vtkRenderWindowInteractor object to set up
1889  * \param[in,out] win a vtkRenderWindow object that the interactor is attached to
1890  * \param[in,out] style a vtkInteractorStyle object
1891  */
1892  void
1893  setupInteractor (vtkRenderWindowInteractor *iren,
1894  vtkRenderWindow *win,
1895  vtkInteractorStyle *style);
1896 
1897  /** \brief Get a pointer to the current interactor style used. */
1900  {
1901  return (style_);
1902  }
1903  protected:
1904  /** \brief The render window interactor. */
1905 #if ((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4))
1907 #else
1909 #endif
1910  private:
1911  struct ExitMainLoopTimerCallback : public vtkCommand
1912  {
1913  static ExitMainLoopTimerCallback* New ()
1914  {
1915  return (new ExitMainLoopTimerCallback);
1916  }
1917  virtual void
1918  Execute (vtkObject*, unsigned long event_id, void*);
1919 
1920  int right_timer_id;
1921  PCLVisualizer* pcl_visualizer;
1922  };
1923 
1924  struct ExitCallback : public vtkCommand
1925  {
1926  static ExitCallback* New ()
1927  {
1928  return (new ExitCallback);
1929  }
1930  virtual void
1931  Execute (vtkObject*, unsigned long event_id, void*);
1932 
1933  PCLVisualizer* pcl_visualizer;
1934  };
1935 
1936  //////////////////////////////////////////////////////////////////////////////////////////////
1937  struct FPSCallback : public vtkCommand
1938  {
1939  static FPSCallback *New () { return (new FPSCallback); }
1940 
1941  FPSCallback () : actor (), pcl_visualizer (), decimated () {}
1942  FPSCallback (const FPSCallback& src) : vtkCommand (), actor (src.actor), pcl_visualizer (src.pcl_visualizer), decimated (src.decimated) {}
1943  FPSCallback& operator = (const FPSCallback& src) { actor = src.actor; pcl_visualizer = src.pcl_visualizer; decimated = src.decimated; return (*this); }
1944 
1945  virtual void
1946  Execute (vtkObject*, unsigned long event_id, void*);
1947 
1948  vtkTextActor *actor;
1949  PCLVisualizer* pcl_visualizer;
1950  bool decimated;
1951  };
1952 
1953  /** \brief The FPSCallback object for the current visualizer. */
1954  vtkSmartPointer<FPSCallback> update_fps_;
1955 
1956 #if !((VTK_MAJOR_VERSION == 5) && (VTK_MINOR_VERSION <= 4))
1957  /** \brief Set to false if the interaction loop is running. */
1958  bool stopped_;
1959 
1960  /** \brief Global timer ID. Used in destructor only. */
1961  int timer_id_;
1962 #endif
1963  /** \brief Callback object enabling us to leave the main loop, when a timer fires. */
1964  vtkSmartPointer<ExitMainLoopTimerCallback> exit_main_loop_timer_callback_;
1965  vtkSmartPointer<ExitCallback> exit_callback_;
1966 
1967  /** \brief The collection of renderers used. */
1969 
1970  /** \brief The render window. */
1972 
1973  /** \brief The render window interactor style. */
1975 
1976  /** \brief Internal list with actor pointers and name IDs for point clouds. */
1977  CloudActorMapPtr cloud_actor_map_;
1978 
1979  /** \brief Internal list with actor pointers and name IDs for shapes. */
1980  ShapeActorMapPtr shape_actor_map_;
1981 
1982  /** \brief Internal list with actor pointers and viewpoint for coordinates. */
1983  CoordinateActorMapPtr coordinate_actor_map_;
1984 
1985  /** \brief Internal pointer to widget which contains a set of axes */
1987 
1988  /** \brief Boolean that holds whether or not the camera parameters were manually initialized */
1989  bool camera_set_;
1990 
1991  /** \brief Boolean that holds whether or not a camera file were automatically loaded */
1992  bool camera_file_loaded_;
1993 
1994  /** \brief Boolean that holds whether or not to use the vtkVertexBufferObjectMapper*/
1995  bool use_vbos_;
1996 
1997  /** \brief Internal method. Removes a vtk actor from the screen.
1998  * \param[in] actor a pointer to the vtk actor object
1999  * \param[in] viewport the view port where the actor should be removed from (default: all)
2000  */
2001  bool
2002  removeActorFromRenderer (const vtkSmartPointer<vtkLODActor> &actor,
2003  int viewport = 0);
2004 
2005  /** \brief Internal method. Removes a vtk actor from the screen.
2006  * \param[in] actor a pointer to the vtk actor object
2007  * \param[in] viewport the view port where the actor should be removed from (default: all)
2008  */
2009  bool
2010  removeActorFromRenderer (const vtkSmartPointer<vtkActor> &actor,
2011  int viewport = 0);
2012 
2013  /** \brief Internal method. Adds a vtk actor to screen.
2014  * \param[in] actor a pointer to the vtk actor object
2015  * \param[in] viewport port where the actor should be added to (default: 0/all)
2016  *
2017  * \note If viewport is set to 0, the actor will be added to all existing
2018  * renders. To select a specific viewport use an integer between 1 and N.
2019  */
2020  void
2021  addActorToRenderer (const vtkSmartPointer<vtkProp> &actor,
2022  int viewport = 0);
2023 
2024  /** \brief Internal method. Adds a vtk actor to screen.
2025  * \param[in] actor a pointer to the vtk actor object
2026  * \param[in] viewport the view port where the actor should be added to (default: all)
2027  */
2028  bool
2029  removeActorFromRenderer (const vtkSmartPointer<vtkProp> &actor,
2030  int viewport = 0);
2031 
2032  /** \brief Internal method. Creates a vtk actor from a vtk polydata object.
2033  * \param[in] data the vtk polydata object to create an actor for
2034  * \param[out] actor the resultant vtk actor object
2035  * \param[in] use_scalars set scalar properties to the mapper if it exists in the data. Default: true.
2036  */
2037  void
2038  createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data,
2040  bool use_scalars = true);
2041 
2042  /** \brief Internal method. Creates a vtk actor from a vtk polydata object.
2043  * \param[in] data the vtk polydata object to create an actor for
2044  * \param[out] actor the resultant vtk actor object
2045  * \param[in] use_scalars set scalar properties to the mapper if it exists in the data. Default: true.
2046  */
2047  void
2048  createActorFromVTKDataSet (const vtkSmartPointer<vtkDataSet> &data,
2050  bool use_scalars = true);
2051 
2052  /** \brief Converts a PCL templated PointCloud object to a vtk polydata object.
2053  * \param[in] cloud the input PCL PointCloud dataset
2054  * \param[out] polydata the resultant polydata containing the cloud
2055  * \param[out] initcells a list of cell indices used for the conversion. This can be set once and then passed
2056  * around to speed up the conversion.
2057  */
2058  template <typename PointT> void
2059  convertPointCloudToVTKPolyData (const typename pcl::PointCloud<PointT>::ConstPtr &cloud,
2060  vtkSmartPointer<vtkPolyData> &polydata,
2061  vtkSmartPointer<vtkIdTypeArray> &initcells);
2062 
2063  /** \brief Converts a PCL templated PointCloud object to a vtk polydata object.
2064  * \param[in] geometry_handler the geometry handler object used to extract the XYZ data
2065  * \param[out] polydata the resultant polydata containing the cloud
2066  * \param[out] initcells a list of cell indices used for the conversion. This can be set once and then passed
2067  * around to speed up the conversion.
2068  */
2069  template <typename PointT> void
2070  convertPointCloudToVTKPolyData (const PointCloudGeometryHandler<PointT> &geometry_handler,
2071  vtkSmartPointer<vtkPolyData> &polydata,
2072  vtkSmartPointer<vtkIdTypeArray> &initcells);
2073 
2074  /** \brief Converts a PCL templated PointCloud object to a vtk polydata object.
2075  * \param[in] geometry_handler the geometry handler object used to extract the XYZ data
2076  * \param[out] polydata the resultant polydata containing the cloud
2077  * \param[out] initcells a list of cell indices used for the conversion. This can be set once and then passed
2078  * around to speed up the conversion.
2079  */
2080  void
2081  convertPointCloudToVTKPolyData (const GeometryHandlerConstPtr &geometry_handler,
2082  vtkSmartPointer<vtkPolyData> &polydata,
2083  vtkSmartPointer<vtkIdTypeArray> &initcells);
2084 
2085  /** \brief Updates a set of cells (vtkIdTypeArray) if the number of points in a cloud changes
2086  * \param[out] cells the vtkIdTypeArray object (set of cells) to update
2087  * \param[out] initcells a previously saved set of cells. If the number of points in the current cloud is
2088  * higher than the number of cells in \a cells, and initcells contains enough data, then a copy from it
2089  * will be made instead of regenerating the entire array.
2090  * \param[in] nr_points the number of points in the new cloud. This dictates how many cells we need to
2091  * generate
2092  */
2093  void
2094  updateCells (vtkSmartPointer<vtkIdTypeArray> &cells,
2096  vtkIdType nr_points);
2097 
2098  /** \brief Internal function which converts the information present in the geometric
2099  * and color handlers into VTK PolyData+Scalars, constructs a vtkActor object, and adds
2100  * all the required information to the internal cloud_actor_map_ object.
2101  * \param[in] geometry_handler the geometric handler that contains the XYZ data
2102  * \param[in] color_handler the color handler that contains the "RGB" (scalar) data
2103  * \param[in] id the point cloud object id
2104  * \param[in] viewport the view port where the Point Cloud should be added
2105  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
2106  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
2107  */
2108  template <typename PointT> bool
2109  fromHandlersToScreen (const PointCloudGeometryHandler<PointT> &geometry_handler,
2110  const PointCloudColorHandler<PointT> &color_handler,
2111  const std::string &id,
2112  int viewport,
2113  const Eigen::Vector4f& sensor_origin = Eigen::Vector4f (0, 0, 0, 0),
2114  const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternion<float> (1, 0, 0 ,0));
2115 
2116  /** \brief Internal function which converts the information present in the geometric
2117  * and color handlers into VTK PolyData+Scalars, constructs a vtkActor object, and adds
2118  * all the required information to the internal cloud_actor_map_ object.
2119  * \param[in] geometry_handler the geometric handler that contains the XYZ data
2120  * \param[in] color_handler the color handler that contains the "RGB" (scalar) data
2121  * \param[in] id the point cloud object id
2122  * \param[in] viewport the view port where the Point Cloud should be added
2123  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
2124  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
2125  */
2126  template <typename PointT> bool
2127  fromHandlersToScreen (const PointCloudGeometryHandler<PointT> &geometry_handler,
2128  const ColorHandlerConstPtr &color_handler,
2129  const std::string &id,
2130  int viewport,
2131  const Eigen::Vector4f& sensor_origin = Eigen::Vector4f (0, 0, 0, 0),
2132  const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternion<float> (1, 0, 0 ,0));
2133 
2134  /** \brief Internal function which converts the information present in the geometric
2135  * and color handlers into VTK PolyData+Scalars, constructs a vtkActor object, and adds
2136  * all the required information to the internal cloud_actor_map_ object.
2137  * \param[in] geometry_handler the geometric handler that contains the XYZ data
2138  * \param[in] color_handler the color handler that contains the "RGB" (scalar) data
2139  * \param[in] id the point cloud object id
2140  * \param[in] viewport the view port where the Point Cloud should be added
2141  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
2142  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
2143  */
2144  bool
2145  fromHandlersToScreen (const GeometryHandlerConstPtr &geometry_handler,
2146  const ColorHandlerConstPtr &color_handler,
2147  const std::string &id,
2148  int viewport,
2149  const Eigen::Vector4f& sensor_origin = Eigen::Vector4f (0, 0, 0, 0),
2150  const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternion<float> (1, 0, 0 ,0));
2151 
2152  /** \brief Internal function which converts the information present in the geometric
2153  * and color handlers into VTK PolyData+Scalars, constructs a vtkActor object, and adds
2154  * all the required information to the internal cloud_actor_map_ object.
2155  * \param[in] geometry_handler the geometric handler that contains the XYZ data
2156  * \param[in] color_handler the color handler that contains the "RGB" (scalar) data
2157  * \param[in] id the point cloud object id
2158  * \param[in] viewport the view port where the Point Cloud should be added
2159  * \param[in] sensor_origin the origin of the cloud data in global coordinates (defaults to 0,0,0)
2160  * \param[in] sensor_orientation the orientation of the cloud data in global coordinates (defaults to 1,0,0,0)
2161  */
2162  template <typename PointT> bool
2163  fromHandlersToScreen (const GeometryHandlerConstPtr &geometry_handler,
2164  const PointCloudColorHandler<PointT> &color_handler,
2165  const std::string &id,
2166  int viewport,
2167  const Eigen::Vector4f& sensor_origin = Eigen::Vector4f (0, 0, 0, 0),
2168  const Eigen::Quaternion<float>& sensor_orientation = Eigen::Quaternion<float> (1, 0, 0 ,0));
2169 
2170  /** \brief Allocate a new polydata smartpointer. Internal
2171  * \param[out] polydata the resultant poly data
2172  */
2173  void
2174  allocVtkPolyData (vtkSmartPointer<vtkAppendPolyData> &polydata);
2175 
2176  /** \brief Allocate a new polydata smartpointer. Internal
2177  * \param[out] polydata the resultant poly data
2178  */
2179  void
2180  allocVtkPolyData (vtkSmartPointer<vtkPolyData> &polydata);
2181 
2182  /** \brief Allocate a new unstructured grid smartpointer. Internal
2183  * \param[out] polydata the resultant poly data
2184  */
2185  void
2187 
2188  /** \brief Transform the point cloud viewpoint to a transformation matrix
2189  * \param[in] origin the camera origin
2190  * \param[in] orientation the camera orientation
2191  * \param[out] transformation the camera transformation matrix
2192  */
2193  void
2194  getTransformationMatrix (const Eigen::Vector4f &origin,
2195  const Eigen::Quaternion<float> &orientation,
2196  Eigen::Matrix4f &transformation);
2197 
2198  /** \brief Fills a vtkTexture structure from pcl::TexMaterial.
2199  * \param[in] tex_mat texture material in PCL format
2200  * \param[out] vtk_tex texture material in VTK format
2201  * \return 0 on success and -1 else.
2202  * \note for now only image based textures are supported, image file must be in
2203  * tex_file attribute of \a tex_mat.
2204  */
2205  int
2206  textureFromTexMaterial (const pcl::TexMaterial& tex_mat,
2207  vtkTexture* vtk_tex) const;
2208 
2209  /** \brief Get camera file for camera parameter saving/restoring from command line.
2210  * Camera filename is calculated using sha1 value of all pathes of input .pcd files
2211  * \return empty string if failed.
2212  */
2213  std::string
2214  getUniqueCameraFile (int argc, char **argv);
2215 
2216  //There's no reason these conversion functions shouldn't be public and static so others can use them.
2217  public:
2218  /** \brief Convert Eigen::Matrix4f to vtkMatrix4x4
2219  * \param[in] m the input Eigen matrix
2220  * \param[out] vtk_matrix the resultant VTK matrix
2221  */
2222  static void
2223  convertToVtkMatrix (const Eigen::Matrix4f &m,
2224  vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
2225 
2226  /** \brief Convert origin and orientation to vtkMatrix4x4
2227  * \param[in] origin the point cloud origin
2228  * \param[in] orientation the point cloud orientation
2229  * \param[out] vtk_matrix the resultant VTK 4x4 matrix
2230  */
2231  static void
2232  convertToVtkMatrix (const Eigen::Vector4f &origin,
2233  const Eigen::Quaternion<float> &orientation,
2234  vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
2235 
2236  /** \brief Convert vtkMatrix4x4 to an Eigen4f
2237  * \param[in] vtk_matrix the original VTK 4x4 matrix
2238  * \param[out] m the resultant Eigen 4x4 matrix
2239  */
2240  static void
2241  convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix,
2242  Eigen::Matrix4f &m);
2243 
2244  };
2245  }
2246 }
2247 
2248 #include <pcl/visualization/impl/pcl_visualizer.hpp>
2249 
2250 #endif
2251 
static PCLVisualizerInteractorStyle * New()
vtkSmartPointer< PCLVisualizerInteractorStyle > getInteractorStyle()
Get a pointer to the current interactor style used.
PointCloudGeometryHandler< pcl::PCLPointCloud2 > GeometryHandler
PointCloudColorHandler< pcl::PCLPointCloud2 > ColorHandler
std::vector< PointCloud< PointT >, Eigen::aligned_allocator< PointCloud< PointT > > > CloudVectorType
Definition: point_cloud.h:427
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:140
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.
/brief Class representing 3D point picking events.
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.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
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::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.
bool removePolygonMesh(const std::string &id="polygon", int viewport=0)
Removes a PolygonMesh from screen, based on a given ID.
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< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
boost::shared_ptr< ::pcl::PCLPointCloud2 const > ConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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...
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.