Point Cloud Library (PCL)  1.14.1-dev
openni2_grabber.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-2012, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  * Copyright (c) 2014, respective authors.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  * notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above
18  * copyright notice, this list of conditions and the following
19  * disclaimer in the documentation and/or other materials provided
20  * with the distribution.
21  * * Neither the name of the copyright holder(s) nor the names of its
22  * contributors may be used to endorse or promote products derived
23  * from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/memory.h>
43 #include <pcl/pcl_config.h>
44 #include <pcl/pcl_macros.h>
45 
46 #ifdef HAVE_OPENNI2
47 
48 #include <pcl/point_cloud.h>
49 #include <pcl/io/grabber.h>
50 #include <pcl/io/openni2/openni2_device.h>
51 #include <string>
52 #include <pcl/common/synchronizer.h>
53 
54 #include <pcl/io/image.h>
55 #include <pcl/io/image_rgb24.h>
56 #include <pcl/io/image_yuv422.h>
57 #include <pcl/io/image_depth.h>
58 #include <pcl/io/image_ir.h>
59 
60 namespace pcl
61 {
62  struct PointXYZ;
63  struct PointXYZRGB;
64  struct PointXYZRGBA;
65  struct PointXYZI;
66 
67  namespace io
68  {
69 
70  /** \brief Grabber for OpenNI 2 devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live)
71  * \ingroup io
72  */
73  class PCL_EXPORTS OpenNI2Grabber : public Grabber
74  {
75  public:
76  using Ptr = shared_ptr<OpenNI2Grabber>;
77  using ConstPtr = shared_ptr<const OpenNI2Grabber>;
78 
79  // Templated images
81  using IRImage = pcl::io::IRImage;
82  using Image = pcl::io::Image;
83 
84  /** \brief Basic camera parameters placeholder. */
85  struct CameraParameters
86  {
87  /** fx */
88  double focal_length_x;
89  /** fy */
90  double focal_length_y;
91  /** cx */
92  double principal_point_x;
93  /** cy */
94  double principal_point_y;
95 
96  CameraParameters (double initValue)
97  : focal_length_x (initValue), focal_length_y (initValue),
98  principal_point_x (initValue), principal_point_y (initValue)
99  {}
100 
101  CameraParameters (double fx, double fy, double cx, double cy)
102  : focal_length_x (fx), focal_length_y (fy), principal_point_x (cx), principal_point_y (cy)
103  { }
104  };
105 
106  enum Mode
107  {
108  OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz
109  OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect
110  OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect
111  OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion
112  OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion
113  OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect
114  OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion
115  OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN)
116  OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN)
117  OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN)
118  };
119 
120  //define callback signature typedefs
121  using sig_cb_openni_image = void (const Image::Ptr &);
122  using sig_cb_openni_depth_image = void (const DepthImage::Ptr &);
123  using sig_cb_openni_ir_image = void (const IRImage::Ptr &);
124  using sig_cb_openni_image_depth_image = void (const Image::Ptr &, const DepthImage::Ptr &, float) ;
125  using sig_cb_openni_ir_depth_image = void (const IRImage::Ptr &, const DepthImage::Ptr &, float) ;
126  using sig_cb_openni_point_cloud = void (const typename pcl::PointCloud<pcl::PointXYZ>::ConstPtr &);
127  using sig_cb_openni_point_cloud_rgb = void (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &);
128  using sig_cb_openni_point_cloud_rgba = void (const typename pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &);
129  using sig_cb_openni_point_cloud_i = void (const typename pcl::PointCloud<pcl::PointXYZI>::ConstPtr &);
130 
131  public:
132  /** \brief Constructor
133  * \param[in] device_id ID of the device, which might be a serial number, bus@address, URI or the index of the device.
134  * \param[in] depth_mode the mode of the depth stream
135  * \param[in] image_mode the mode of the image stream
136  * Depending on the value of \a device_id, the device is opened as follows:
137  * * If it corresponds to a file path, the device is opened with OpenNI2DeviceManager::getFileDevice
138  * * If it is an index of the form "#1234", the device is opened with OpenNI2DeviceManager::getDeviceByIndex
139  * * If it corresponds to an URI, the device is opened with OpenNI2DeviceManager::getDevice
140  * * If it is an empty string, the device is opened with OpenNI2DeviceManager::getAnyDevice
141  * * Otherwise a pcl::IOException instance is thrown
142  */
143  OpenNI2Grabber (const std::string& device_id = "",
144  const Mode& depth_mode = OpenNI_Default_Mode,
145  const Mode& image_mode = OpenNI_Default_Mode);
146 
147  /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
148  ~OpenNI2Grabber () noexcept override;
149 
150  /** \brief Start the data acquisition. */
151  void
152  start () override;
153 
154  /** \brief Stop the data acquisition. */
155  void
156  stop () override;
157 
158  /** \brief Check if the data acquisition is still running. */
159  bool
160  isRunning () const override;
161 
162  std::string
163  getName () const override;
164 
165  /** \brief Obtain the number of frames per second (FPS). */
166  float
167  getFramesPerSecond () const override;
168 
169  /** \brief Get a boost shared pointer to the \ref OpenNIDevice object. */
170  inline pcl::io::openni2::OpenNI2Device::Ptr
171  getDevice () const;
172 
173  /** \brief Obtain a list of the available depth modes that this device supports. */
174  std::vector<std::pair<int, pcl::io::openni2::OpenNI2VideoMode> >
175  getAvailableDepthModes () const;
176 
177  /** \brief Obtain a list of the available image modes that this device supports. */
178  std::vector<std::pair<int, pcl::io::openni2::OpenNI2VideoMode> >
179  getAvailableImageModes () const;
180 
181  /** \brief Set the RGB camera parameters (fx, fy, cx, cy)
182  * \param[in] rgb_focal_length_x the RGB focal length (fx)
183  * \param[in] rgb_focal_length_y the RGB focal length (fy)
184  * \param[in] rgb_principal_point_x the RGB principal point (cx)
185  * \param[in] rgb_principal_point_y the RGB principal point (cy)
186  * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them
187  * and the grabber will use the default values from the camera instead.
188  */
189  inline void
190  setRGBCameraIntrinsics (const double rgb_focal_length_x,
191  const double rgb_focal_length_y,
192  const double rgb_principal_point_x,
193  const double rgb_principal_point_y)
194  {
195  rgb_parameters_ = CameraParameters (
196  rgb_focal_length_x, rgb_focal_length_y,
197  rgb_principal_point_x, rgb_principal_point_y);
198  }
199 
200  /** \brief Get the RGB camera parameters (fx, fy, cx, cy)
201  * \param[out] rgb_focal_length_x the RGB focal length (fx)
202  * \param[out] rgb_focal_length_y the RGB focal length (fy)
203  * \param[out] rgb_principal_point_x the RGB principal point (cx)
204  * \param[out] rgb_principal_point_y the RGB principal point (cy)
205  */
206  inline void
207  getRGBCameraIntrinsics (double &rgb_focal_length_x,
208  double &rgb_focal_length_y,
209  double &rgb_principal_point_x,
210  double &rgb_principal_point_y) const
211  {
212  rgb_focal_length_x = rgb_parameters_.focal_length_x;
213  rgb_focal_length_y = rgb_parameters_.focal_length_y;
214  rgb_principal_point_x = rgb_parameters_.principal_point_x;
215  rgb_principal_point_y = rgb_parameters_.principal_point_y;
216  }
217 
218 
219  /** \brief Set the RGB image focal length (fx = fy).
220  * \param[in] rgb_focal_length the RGB focal length (assumes fx = fy)
221  * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it
222  * and the grabber will use the default values from the camera instead.
223  * These parameters will be used for XYZRGBA clouds.
224  */
225  inline void
226  setRGBFocalLength (const double rgb_focal_length)
227  {
228  rgb_parameters_.focal_length_x = rgb_focal_length;
229  rgb_parameters_.focal_length_y = rgb_focal_length;
230  }
231 
232  /** \brief Set the RGB image focal length
233  * \param[in] rgb_focal_length_x the RGB focal length (fx)
234  * \param[in] rgb_focal_ulength_y the RGB focal length (fy)
235  * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them
236  * and the grabber will use the default values from the camera instead.
237  * These parameters will be used for XYZRGBA clouds.
238  */
239  inline void
240  setRGBFocalLength (const double rgb_focal_length_x, const double rgb_focal_length_y)
241  {
242  rgb_parameters_.focal_length_x = rgb_focal_length_x;
243  rgb_parameters_.focal_length_y = rgb_focal_length_y;
244  }
245 
246  /** \brief Return the RGB focal length parameters (fx, fy)
247  * \param[out] rgb_focal_length_x the RGB focal length (fx)
248  * \param[out] rgb_focal_length_y the RGB focal length (fy)
249  */
250  inline void
251  getRGBFocalLength (double &rgb_focal_length_x, double &rgb_focal_length_y) const
252  {
253  rgb_focal_length_x = rgb_parameters_.focal_length_x;
254  rgb_focal_length_y = rgb_parameters_.focal_length_y;
255  }
256 
257  /** \brief Set the Depth camera parameters (fx, fy, cx, cy)
258  * \param[in] depth_focal_length_x the Depth focal length (fx)
259  * \param[in] depth_focal_length_y the Depth focal length (fy)
260  * \param[in] depth_principal_point_x the Depth principal point (cx)
261  * \param[in] depth_principal_point_y the Depth principal point (cy)
262  * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them
263  * and the grabber will use the default values from the camera instead.
264  */
265  inline void
266  setDepthCameraIntrinsics (const double depth_focal_length_x,
267  const double depth_focal_length_y,
268  const double depth_principal_point_x,
269  const double depth_principal_point_y)
270  {
271  depth_parameters_ = CameraParameters (
272  depth_focal_length_x, depth_focal_length_y,
273  depth_principal_point_x, depth_principal_point_y);
274  }
275 
276  /** \brief Get the Depth camera parameters (fx, fy, cx, cy)
277  * \param[out] depth_focal_length_x the Depth focal length (fx)
278  * \param[out] depth_focal_length_y the Depth focal length (fy)
279  * \param[out] depth_principal_point_x the Depth principal point (cx)
280  * \param[out] depth_principal_point_y the Depth principal point (cy)
281  */
282  inline void
283  getDepthCameraIntrinsics (double &depth_focal_length_x,
284  double &depth_focal_length_y,
285  double &depth_principal_point_x,
286  double &depth_principal_point_y) const
287  {
288  depth_focal_length_x = depth_parameters_.focal_length_x;
289  depth_focal_length_y = depth_parameters_.focal_length_y;
290  depth_principal_point_x = depth_parameters_.principal_point_x;
291  depth_principal_point_y = depth_parameters_.principal_point_y;
292  }
293 
294  /** \brief Set the Depth image focal length (fx = fy).
295  * \param[in] depth_focal_length the Depth focal length (assumes fx = fy)
296  * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it
297  * and the grabber will use the default values from the camera instead.
298  */
299  inline void
300  setDepthFocalLength (const double depth_focal_length)
301  {
302  depth_parameters_.focal_length_x = depth_focal_length;
303  depth_parameters_.focal_length_y = depth_focal_length;
304  }
305 
306 
307  /** \brief Set the Depth image focal length
308  * \param[in] depth_focal_length_x the Depth focal length (fx)
309  * \param[in] depth_focal_length_y the Depth focal length (fy)
310  * Setting the parameter to non-finite values (e.g., NaN, Inf) invalidates them
311  * and the grabber will use the default values from the camera instead.
312  */
313  inline void
314  setDepthFocalLength (const double depth_focal_length_x, const double depth_focal_length_y)
315  {
316  depth_parameters_.focal_length_x = depth_focal_length_x;
317  depth_parameters_.focal_length_y = depth_focal_length_y;
318  }
319 
320  /** \brief Return the Depth focal length parameters (fx, fy)
321  * \param[out] depth_focal_length_x the Depth focal length (fx)
322  * \param[out] depth_focal_length_y the Depth focal length (fy)
323  */
324  inline void
325  getDepthFocalLength (double &depth_focal_length_x, double &depth_focal_length_y) const
326  {
327  depth_focal_length_x = depth_parameters_.focal_length_x;
328  depth_focal_length_y = depth_parameters_.focal_length_y;
329  }
330 
331  protected:
332 
333  /** \brief Sets up an OpenNI device. */
334  void
335  setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode);
336 
337  /** \brief Update mode maps. */
338  void
339  updateModeMaps ();
340 
341  /** \brief Start synchronization. */
342  void
343  startSynchronization ();
344 
345  /** \brief Stop synchronization. */
346  void
347  stopSynchronization ();
348 
349  // TODO: rename to mapMode2OniMode
350  /** \brief Map config modes. */
351  bool
352  mapMode2XnMode (int mode, pcl::io::openni2::OpenNI2VideoMode& videoMode) const;
353 
354  // callback methods
355  /** \brief RGB image callback. */
356  virtual void
357  imageCallback (pcl::io::openni2::Image::Ptr image, void* cookie);
358 
359  /** \brief Depth image callback. */
360  virtual void
361  depthCallback (pcl::io::openni2::DepthImage::Ptr depth_image, void* cookie);
362 
363  /** \brief IR image callback. */
364  virtual void
365  irCallback (pcl::io::openni2::IRImage::Ptr ir_image, void* cookie);
366 
367  /** \brief RGB + Depth image callback. */
368  virtual void
369  imageDepthImageCallback (const pcl::io::openni2::Image::Ptr &image,
370  const pcl::io::openni2::DepthImage::Ptr &depth_image);
371 
372  /** \brief IR + Depth image callback. */
373  virtual void
374  irDepthImageCallback (const pcl::io::openni2::IRImage::Ptr &image,
375  const pcl::io::openni2::DepthImage::Ptr &depth_image);
376 
377  /** \brief Process changed signals. */
378  void
379  signalsChanged () override;
380 
381  // helper methods
382 
383  /** \brief Check if the RGB and Depth images are required to be synchronized or not. */
384  virtual void
385  checkImageAndDepthSynchronizationRequired ();
386 
387  /** \brief Check if the RGB image stream is required or not. */
388  virtual void
389  checkImageStreamRequired ();
390 
391  /** \brief Check if the depth stream is required or not. */
392  virtual void
393  checkDepthStreamRequired ();
394 
395  /** \brief Check if the IR image stream is required or not. */
396  virtual void
397  checkIRStreamRequired ();
398 
399 
400  // Point cloud conversion ///////////////////////////////////////////////
401 
402  /** \brief Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ>
403  * \param[in] depth the depth image to convert
404  */
406  convertToXYZPointCloud (const pcl::io::openni2::DepthImage::Ptr &depth);
407 
408  /** \brief Convert a Depth + RGB image pair to a pcl::PointCloud<PointT>
409  * \param[in] image the RGB image to convert
410  * \param[in] depth_image the depth image to convert
411  */
412  template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
413  convertToXYZRGBPointCloud (const pcl::io::openni2::Image::Ptr &image,
414  const pcl::io::openni2::DepthImage::Ptr &depth_image);
415 
416  /** \brief Convert a Depth + Intensity image pair to a pcl::PointCloud<pcl::PointXYZI>
417  * \param[in] image the IR image to convert
418  * \param[in] depth_image the depth image to convert
419  */
421  convertToXYZIPointCloud (const pcl::io::openni2::IRImage::Ptr &image,
422  const pcl::io::openni2::DepthImage::Ptr &depth_image);
423 
424  std::vector<std::uint8_t> color_resize_buffer_{};
425  std::vector<std::uint16_t> depth_resize_buffer_{};
426  std::vector<std::uint16_t> ir_resize_buffer_{};
427 
428  // Stream callbacks /////////////////////////////////////////////////////
429  void
430  processColorFrame (openni::VideoStream& stream);
431 
432  void
433  processDepthFrame (openni::VideoStream& stream);
434 
435  void
436  processIRFrame (openni::VideoStream& stream);
437 
438 
439  Synchronizer<pcl::io::openni2::Image::Ptr, pcl::io::openni2::DepthImage::Ptr > rgb_sync_;
440  Synchronizer<pcl::io::openni2::IRImage::Ptr, pcl::io::openni2::DepthImage::Ptr > ir_sync_;
441 
442  /** \brief The actual openni device. */
444 
445  std::string rgb_frame_id_;
446  std::string depth_frame_id_;
447  unsigned image_width_{0};
448  unsigned image_height_{0};
449  unsigned depth_width_{0};
450  unsigned depth_height_{0};
451 
452  bool image_required_{false};
453  bool depth_required_{false};
454  bool ir_required_{false};
455  bool sync_required_{false};
456 
457  boost::signals2::signal<sig_cb_openni_image>* image_signal_{};
458  boost::signals2::signal<sig_cb_openni_depth_image>* depth_image_signal_{};
459  boost::signals2::signal<sig_cb_openni_ir_image>* ir_image_signal_{};
460  boost::signals2::signal<sig_cb_openni_image_depth_image>* image_depth_image_signal_{};
461  boost::signals2::signal<sig_cb_openni_ir_depth_image>* ir_depth_image_signal_{};
462  boost::signals2::signal<sig_cb_openni_point_cloud>* point_cloud_signal_{};
463  boost::signals2::signal<sig_cb_openni_point_cloud_i>* point_cloud_i_signal_{};
464  boost::signals2::signal<sig_cb_openni_point_cloud_rgb>* point_cloud_rgb_signal_{};
465  boost::signals2::signal<sig_cb_openni_point_cloud_rgba>* point_cloud_rgba_signal_{};
466 
467  struct modeComp
468  {
469  bool operator () (const openni::VideoMode& mode1, const openni::VideoMode & mode2) const
470  {
471  if (mode1.getResolutionX () < mode2.getResolutionX ())
472  return true;
473  if (mode1.getResolutionX () > mode2.getResolutionX ())
474  return false;
475  if (mode1.getResolutionY () < mode2.getResolutionY ())
476  return true;
477  if (mode1.getResolutionY () > mode2.getResolutionY ())
478  return false;
479  return (mode1.getFps () < mode2.getFps ());
480  }
481  };
482 
483  // Mapping from config (enum) modes to native OpenNI modes
484  std::map<int, pcl::io::openni2::OpenNI2VideoMode> config2oni_map_;
485 
486  pcl::io::openni2::OpenNI2Device::CallbackHandle depth_callback_handle_{};
487  pcl::io::openni2::OpenNI2Device::CallbackHandle image_callback_handle_{};
489  bool running_{false};
490 
491  CameraParameters rgb_parameters_{std::numeric_limits<double>::quiet_NaN ()};
492  CameraParameters depth_parameters_{std::numeric_limits<double>::quiet_NaN ()};
493 
494  public:
496  };
497 
499  OpenNI2Grabber::getDevice () const
500  {
501  return device_;
502  }
503 
504  } // namespace
505 }
506 
507 #endif // HAVE_OPENNI2
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:414
This class provides methods to fill a depth or disparity image.
Definition: image_depth.h:55
shared_ptr< DepthImage > Ptr
Definition: image_depth.h:57
Class containing just a reference to IR meta data.
Definition: image_ir.h:55
shared_ptr< IRImage > Ptr
Definition: image_ir.h:57
Image interface class providing an interface to fill a RGB or Grayscale image buffer.
Definition: image.h:57
shared_ptr< Image > Ptr
Definition: image.h:59
shared_ptr< OpenNI2Device > Ptr
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:86
Defines functions, macros and traits for allocating and using memory.
float4 PointXYZRGB
Definition: internal.hpp:60
DeviceArray2D< uchar4 > Image
Definition: label_common.h:112
pcl::io::IRImage IRImage
pcl::io::DepthImage DepthImage
Defines all the PCL and non-PCL macros used.
#define PCL_EXPORTS
Definition: pcl_macros.h:325