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