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