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