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