Point Cloud Library (PCL)  1.7.1
openni_grabber.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009-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 
39 #include <pcl/pcl_config.h>
40 #ifdef HAVE_OPENNI
41 
42 #ifndef __PCL_IO_OPENNI_GRABBER__
43 #define __PCL_IO_OPENNI_GRABBER__
44 
45 #include <pcl/io/eigen.h>
46 #include <pcl/io/boost.h>
47 #include <pcl/io/grabber.h>
48 #include <pcl/io/openni_camera/openni_driver.h>
49 #include <pcl/io/openni_camera/openni_device_kinect.h>
50 #include <pcl/io/openni_camera/openni_image.h>
51 #include <pcl/io/openni_camera/openni_depth_image.h>
52 #include <pcl/io/openni_camera/openni_ir_image.h>
53 #include <string>
54 #include <deque>
55 #include <pcl/common/synchronizer.h>
56 
57 namespace pcl
58 {
59  struct PointXYZ;
60  struct PointXYZRGB;
61  struct PointXYZRGBA;
62  struct PointXYZI;
63  template <typename T> class PointCloud;
64 
65  /** \brief Grabber for OpenNI devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live)
66  * \author Nico Blodow <blodow@cs.tum.edu>, Suat Gedikli <gedikli@willowgarage.com>
67  * \ingroup io
68  */
69  class PCL_EXPORTS OpenNIGrabber : public Grabber
70  {
71  public:
72  typedef boost::shared_ptr<OpenNIGrabber> Ptr;
73  typedef boost::shared_ptr<const OpenNIGrabber> ConstPtr;
74 
75  typedef enum
76  {
77  OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz
78  OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect
79  OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect
80  OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion
81  OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion
82  OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect
83  OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion
84  OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN)
85  OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN)
86  OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN)
87  } Mode;
88 
89  //define callback signature typedefs
90  typedef void (sig_cb_openni_image) (const boost::shared_ptr<openni_wrapper::Image>&);
91  typedef void (sig_cb_openni_depth_image) (const boost::shared_ptr<openni_wrapper::DepthImage>&);
92  typedef void (sig_cb_openni_ir_image) (const boost::shared_ptr<openni_wrapper::IRImage>&);
93  typedef void (sig_cb_openni_image_depth_image) (const boost::shared_ptr<openni_wrapper::Image>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ;
94  typedef void (sig_cb_openni_ir_depth_image) (const boost::shared_ptr<openni_wrapper::IRImage>&, const boost::shared_ptr<openni_wrapper::DepthImage>&, float constant) ;
95  typedef void (sig_cb_openni_point_cloud) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
96  typedef void (sig_cb_openni_point_cloud_rgb) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGB> >&);
97  typedef void (sig_cb_openni_point_cloud_rgba) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
98  typedef void (sig_cb_openni_point_cloud_i) (const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
99 
100  public:
101  /** \brief Constructor
102  * \param[in] device_id ID of the device, which might be a serial number, bus@address or the index of the device.
103  * \param[in] depth_mode the mode of the depth stream
104  * \param[in] image_mode the mode of the image stream
105  */
106  OpenNIGrabber (const std::string& device_id = "",
107  const Mode& depth_mode = OpenNI_Default_Mode,
108  const Mode& image_mode = OpenNI_Default_Mode);
109 
110  /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
111  virtual ~OpenNIGrabber () throw ();
112 
113  /** \brief Start the data acquisition. */
114  virtual void
115  start ();
116 
117  /** \brief Stop the data acquisition. */
118  virtual void
119  stop ();
120 
121  /** \brief Check if the data acquisition is still running. */
122  virtual bool
123  isRunning () const;
124 
125  virtual std::string
126  getName () const;
127 
128  /** \brief Obtain the number of frames per second (FPS). */
129  virtual float
130  getFramesPerSecond () const;
131 
132  /** \brief Get a boost shared pointer to the \ref OpenNIDevice object. */
133  inline boost::shared_ptr<openni_wrapper::OpenNIDevice>
134  getDevice () const;
135 
136  /** \brief Obtain a list of the available depth modes that this device supports. */
137  std::vector<std::pair<int, XnMapOutputMode> >
138  getAvailableDepthModes () const;
139 
140  /** \brief Obtain a list of the available image modes that this device supports. */
141  std::vector<std::pair<int, XnMapOutputMode> >
142  getAvailableImageModes () const;
143 
144  /** \brief Set the RGB camera parameters (fx, fy, cx, cy)
145  * \param[in] rgb_focal_length_x the RGB focal length (fx)
146  * \param[in] rgb_focal_length_y the RGB focal length (fy)
147  * \param[in] rgb_principal_point_x the RGB principal point (cx)
148  * \param[in] rgb_principal_point_y the RGB principal point (cy)
149  * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them
150  * and the grabber will use the default values from the camera instead.
151  */
152  inline void
153  setRGBCameraIntrinsics (const double rgb_focal_length_x,
154  const double rgb_focal_length_y,
155  const double rgb_principal_point_x,
156  const double rgb_principal_point_y)
157  {
158  rgb_focal_length_x_ = rgb_focal_length_x;
159  rgb_focal_length_y_ = rgb_focal_length_y;
160  rgb_principal_point_x_ = rgb_principal_point_x;
161  rgb_principal_point_y_ = rgb_principal_point_y;
162  }
163 
164  /** \brief Get the RGB camera parameters (fx, fy, cx, cy)
165  * \param[out] rgb_focal_length_x the RGB focal length (fx)
166  * \param[out] rgb_focal_length_y the RGB focal length (fy)
167  * \param[out] rgb_principal_point_x the RGB principal point (cx)
168  * \param[out] rgb_principal_point_y the RGB principal point (cy)
169  */
170  inline void
171  getRGBCameraIntrinsics (double &rgb_focal_length_x,
172  double &rgb_focal_length_y,
173  double &rgb_principal_point_x,
174  double &rgb_principal_point_y) const
175  {
176  rgb_focal_length_x = rgb_focal_length_x_;
177  rgb_focal_length_y = rgb_focal_length_y_;
178  rgb_principal_point_x = rgb_principal_point_x_;
179  rgb_principal_point_y = rgb_principal_point_y_;
180  }
181 
182 
183  /** \brief Set the RGB image focal length (fx = fy).
184  * \param[in] rgb_focal_length the RGB focal length (assumes fx = fy)
185  * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it
186  * and the grabber will use the default values from the camera instead.
187  * These parameters will be used for XYZRGBA clouds.
188  */
189  inline void
190  setRGBFocalLength (const double rgb_focal_length)
191  {
192  rgb_focal_length_x_ = rgb_focal_length_y_ = rgb_focal_length;
193  }
194 
195  /** \brief Set the RGB image focal length
196  * \param[in] rgb_focal_length_x the RGB focal length (fx)
197  * \param[in] rgb_focal_ulength_y the RGB focal length (fy)
198  * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them
199  * and the grabber will use the default values from the camera instead.
200  * These parameters will be used for XYZRGBA clouds.
201  */
202  inline void
203  setRGBFocalLength (const double rgb_focal_length_x, const double rgb_focal_length_y)
204  {
205  rgb_focal_length_x_ = rgb_focal_length_x;
206  rgb_focal_length_y_ = rgb_focal_length_y;
207  }
208 
209  /** \brief Return the RGB focal length parameters (fx, fy)
210  * \param[out] rgb_focal_length_x the RGB focal length (fx)
211  * \param[out] rgb_focal_length_y the RGB focal length (fy)
212  */
213  inline void
214  getRGBFocalLength (double &rgb_focal_length_x, double &rgb_focal_length_y) const
215  {
216  rgb_focal_length_x = rgb_focal_length_x_;
217  rgb_focal_length_y = rgb_focal_length_y_;
218  }
219 
220  /** \brief Set the Depth camera parameters (fx, fy, cx, cy)
221  * \param[in] depth_focal_length_x the Depth focal length (fx)
222  * \param[in] depth_focal_length_y the Depth focal length (fy)
223  * \param[in] depth_principal_point_x the Depth principal point (cx)
224  * \param[in] depth_principal_point_y the Depth principal point (cy)
225  * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them
226  * and the grabber will use the default values from the camera instead.
227  */
228  inline void
229  setDepthCameraIntrinsics (const double depth_focal_length_x,
230  const double depth_focal_length_y,
231  const double depth_principal_point_x,
232  const double depth_principal_point_y)
233  {
234  depth_focal_length_x_ = depth_focal_length_x;
235  depth_focal_length_y_ = depth_focal_length_y;
236  depth_principal_point_x_ = depth_principal_point_x;
237  depth_principal_point_y_ = depth_principal_point_y;
238  }
239 
240  /** \brief Get the Depth camera parameters (fx, fy, cx, cy)
241  * \param[out] depth_focal_length_x the Depth focal length (fx)
242  * \param[out] depth_focal_length_y the Depth focal length (fy)
243  * \param[out] depth_principal_point_x the Depth principal point (cx)
244  * \param[out] depth_principal_point_y the Depth principal point (cy)
245  */
246  inline void
247  getDepthCameraIntrinsics (double &depth_focal_length_x,
248  double &depth_focal_length_y,
249  double &depth_principal_point_x,
250  double &depth_principal_point_y) const
251  {
252  depth_focal_length_x = depth_focal_length_x_;
253  depth_focal_length_y = depth_focal_length_y_;
254  depth_principal_point_x = depth_principal_point_x_;
255  depth_principal_point_y = depth_principal_point_y_;
256  }
257 
258  /** \brief Set the Depth image focal length (fx = fy).
259  * \param[in] depth_focal_length the Depth focal length (assumes fx = fy)
260  * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it
261  * and the grabber will use the default values from the camera instead.
262  */
263  inline void
264  setDepthFocalLength (const double depth_focal_length)
265  {
266  depth_focal_length_x_ = depth_focal_length_y_ = depth_focal_length;
267  }
268 
269 
270  /** \brief Set the Depth image focal length
271  * \param[in] depth_focal_length_x the Depth focal length (fx)
272  * \param[in] depth_focal_length_y the Depth focal length (fy)
273  * Setting the parameter to non-finite values (e.g., NaN, Inf) invalidates them
274  * and the grabber will use the default values from the camera instead.
275  */
276  inline void
277  setDepthFocalLength (const double depth_focal_length_x, const double depth_focal_length_y)
278  {
279  depth_focal_length_x_ = depth_focal_length_x;
280  depth_focal_length_y_ = depth_focal_length_y;
281  }
282 
283  /** \brief Return the Depth focal length parameters (fx, fy)
284  * \param[out] depth_focal_length_x the Depth focal length (fx)
285  * \param[out] depth_focal_length_y the Depth focal length (fy)
286  */
287  inline void
288  getDepthFocalLength (double &depth_focal_length_x, double &depth_focal_length_y) const
289  {
290  depth_focal_length_x = depth_focal_length_x_;
291  depth_focal_length_y = depth_focal_length_y_;
292  }
293 
294  /** \brief Convert vector of raw shift values to depth values
295  * \param[in] shift_data_ptr input shift data
296  * \param[out] depth_data_ptr generated depth data
297  * \param[in] size of shift and depth buffer
298  */
299  inline void
301  const uint16_t* shift_data_ptr,
302  uint16_t* depth_data_ptr,
303  std::size_t size) const
304  {
305  // get openni device instance
306  boost::shared_ptr<openni_wrapper::OpenNIDevice> openni_device =
307  this->getDevice ();
308 
309  const uint16_t* shift_data_it = shift_data_ptr;
310  uint16_t* depth_data_it = depth_data_ptr;
311 
312  // shift-to-depth lookup
313  for (std::size_t i=0; i<size; ++i)
314  {
315  *depth_data_it = openni_device->shiftToDepth(*shift_data_it);
316 
317  shift_data_it++;
318  depth_data_it++;
319  }
320 
321  }
322 
323 
324  protected:
325  /** \brief On initialization processing. */
326  void
327  onInit (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode);
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  /** \brief Map config modes. */
346  bool
347  mapConfigMode2XnMode (int mode, XnMapOutputMode &xnmode) const;
348 
349  // callback methods
350  /** \brief RGB image callback. */
351  virtual void
352  imageCallback (boost::shared_ptr<openni_wrapper::Image> image, void* cookie);
353 
354  /** \brief Depth image callback. */
355  virtual void
356  depthCallback (boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie);
357 
358  /** \brief IR image callback. */
359  virtual void
360  irCallback (boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie);
361 
362  /** \brief RGB + Depth image callback. */
363  virtual void
364  imageDepthImageCallback (const boost::shared_ptr<openni_wrapper::Image> &image,
365  const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image);
366 
367  /** \brief IR + Depth image callback. */
368  virtual void
369  irDepthImageCallback (const boost::shared_ptr<openni_wrapper::IRImage> &image,
370  const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image);
371 
372  /** \brief Process changed signals. */
373  virtual void
374  signalsChanged ();
375 
376  // helper methods
377 
378  /** \brief Check if the RGB and Depth images are required to be synchronized or not. */
379  virtual void
380  checkImageAndDepthSynchronizationRequired ();
381 
382  /** \brief Check if the RGB image stream is required or not. */
383  virtual void
384  checkImageStreamRequired ();
385 
386  /** \brief Check if the depth stream is required or not. */
387  virtual void
388  checkDepthStreamRequired ();
389 
390  /** \brief Check if the IR image stream is required or not. */
391  virtual void
392  checkIRStreamRequired ();
393 
394 
395  /** \brief Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ>
396  * \param[in] depth the depth image to convert
397  */
398  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> >
399  convertToXYZPointCloud (const boost::shared_ptr<openni_wrapper::DepthImage> &depth) const;
400 
401  /** \brief Convert a Depth + RGB image pair to a pcl::PointCloud<PointT>
402  * \param[in] image the RGB image to convert
403  * \param[in] depth_image the depth image to convert
404  */
405  template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
406  convertToXYZRGBPointCloud (const boost::shared_ptr<openni_wrapper::Image> &image,
407  const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const;
408 
409  /** \brief Convert a Depth + Intensity image pair to a pcl::PointCloud<pcl::PointXYZI>
410  * \param[in] image the IR image to convert
411  * \param[in] depth_image the depth image to convert
412  */
413  boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> >
414  convertToXYZIPointCloud (const boost::shared_ptr<openni_wrapper::IRImage> &image,
415  const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image) const;
416 
417 
418  Synchronizer<boost::shared_ptr<openni_wrapper::Image>, boost::shared_ptr<openni_wrapper::DepthImage> > rgb_sync_;
419  Synchronizer<boost::shared_ptr<openni_wrapper::IRImage>, boost::shared_ptr<openni_wrapper::DepthImage> > ir_sync_;
420 
421  /** \brief The actual openni device. */
422  boost::shared_ptr<openni_wrapper::OpenNIDevice> device_;
423 
424  std::string rgb_frame_id_;
425  std::string depth_frame_id_;
426  unsigned image_width_;
427  unsigned image_height_;
428  unsigned depth_width_;
429  unsigned depth_height_;
430 
435 
436  boost::signals2::signal<sig_cb_openni_image>* image_signal_;
437  boost::signals2::signal<sig_cb_openni_depth_image>* depth_image_signal_;
438  boost::signals2::signal<sig_cb_openni_ir_image>* ir_image_signal_;
439  boost::signals2::signal<sig_cb_openni_image_depth_image>* image_depth_image_signal_;
440  boost::signals2::signal<sig_cb_openni_ir_depth_image>* ir_depth_image_signal_;
441  boost::signals2::signal<sig_cb_openni_point_cloud>* point_cloud_signal_;
442  boost::signals2::signal<sig_cb_openni_point_cloud_i>* point_cloud_i_signal_;
443  boost::signals2::signal<sig_cb_openni_point_cloud_rgb>* point_cloud_rgb_signal_;
444  boost::signals2::signal<sig_cb_openni_point_cloud_rgba>* point_cloud_rgba_signal_;
445 
446  struct modeComp
447  {
448 
449  bool operator () (const XnMapOutputMode& mode1, const XnMapOutputMode & mode2) const
450  {
451  if (mode1.nXRes < mode2.nXRes)
452  return true;
453  else if (mode1.nXRes > mode2.nXRes)
454  return false;
455  else if (mode1.nYRes < mode2.nYRes)
456  return true;
457  else if (mode1.nYRes > mode2.nYRes)
458  return false;
459  else if (mode1.nFPS < mode2.nFPS)
460  return true;
461  else
462  return false;
463  }
464  } ;
465  std::map<int, XnMapOutputMode> config2xn_map_;
466 
470  bool running_;
471 
472  /** \brief The RGB image focal length (fx). */
474  /** \brief The RGB image focal length (fy). */
476  /** \brief The RGB image principal point (cx). */
478  /** \brief The RGB image principal point (cy). */
480  /** \brief The depth image focal length (fx). */
482  /** \brief The depth image focal length (fy). */
484  /** \brief The depth image principal point (cx). */
486  /** \brief The depth image principal point (cy). */
488 
489  public:
490  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
491  };
492 
493  boost::shared_ptr<openni_wrapper::OpenNIDevice>
495  {
496  return device_;
497  }
498 
499 } // namespace pcl
500 #endif // __PCL_IO_OPENNI_GRABBER__
501 #endif // HAVE_OPENNI
boost::signals2::signal< sig_cb_openni_depth_image > * depth_image_signal_
void setDepthFocalLength(const double depth_focal_length)
Set the Depth image focal length (fx = fy).
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:58
boost::signals2::signal< sig_cb_openni_point_cloud_i > * point_cloud_i_signal_
double rgb_focal_length_x_
The RGB image focal length (fx).
std::string rgb_frame_id_
void getRGBCameraIntrinsics(double &rgb_focal_length_x, double &rgb_focal_length_y, double &rgb_principal_point_x, double &rgb_principal_point_y) const
Get the RGB camera parameters (fx, fy, cx, cy)
Grabber for OpenNI devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live) ...
openni_wrapper::OpenNIDevice::CallbackHandle image_callback_handle
void getRGBFocalLength(double &rgb_focal_length_x, double &rgb_focal_length_y) const
Return the RGB focal length parameters (fx, fy)
double rgb_principal_point_x_
The RGB image principal point (cx).
boost::signals2::signal< sig_cb_openni_ir_depth_image > * ir_depth_image_signal_
double depth_focal_length_x_
The depth image focal length (fx).
void setDepthCameraIntrinsics(const double depth_focal_length_x, const double depth_focal_length_y, const double depth_principal_point_x, const double depth_principal_point_y)
Set the Depth camera parameters (fx, fy, cx, cy)
double depth_focal_length_y_
The depth image focal length (fy).
double depth_principal_point_x_
The depth image principal point (cx).
void setRGBFocalLength(const double rgb_focal_length_x, const double rgb_focal_length_y)
Set the RGB image focal length.
boost::signals2::signal< sig_cb_openni_image > * image_signal_
openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle
void getDepthFocalLength(double &depth_focal_length_x, double &depth_focal_length_y) const
Return the Depth focal length parameters (fx, fy)
boost::signals2::signal< sig_cb_openni_point_cloud_rgba > * point_cloud_rgba_signal_
boost::signals2::signal< sig_cb_openni_ir_image > * ir_image_signal_
Synchronizer< boost::shared_ptr< openni_wrapper::IRImage >, boost::shared_ptr< openni_wrapper::DepthImage > > ir_sync_
boost::shared_ptr< OpenNIGrabber > Ptr
void setDepthFocalLength(const double depth_focal_length_x, const double depth_focal_length_y)
Set the Depth image focal length.
boost::shared_ptr< const OpenNIGrabber > ConstPtr
boost::shared_ptr< openni_wrapper::OpenNIDevice > getDevice() const
Get a boost shared pointer to the OpenNIDevice object.
std::map< int, XnMapOutputMode > config2xn_map_
double depth_principal_point_y_
The depth image principal point (cy).
Synchronizer< boost::shared_ptr< openni_wrapper::Image >, boost::shared_ptr< openni_wrapper::DepthImage > > rgb_sync_
std::string depth_frame_id_
double rgb_focal_length_y_
The RGB image focal length (fy).
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
openni_wrapper::OpenNIDevice::CallbackHandle depth_callback_handle
void getDepthCameraIntrinsics(double &depth_focal_length_x, double &depth_focal_length_y, double &depth_principal_point_x, double &depth_principal_point_y) const
Get the Depth camera parameters (fx, fy, cx, cy)
boost::signals2::signal< sig_cb_openni_point_cloud > * point_cloud_signal_
double rgb_principal_point_y_
The RGB image principal point (cy).
/brief This template class synchronizes two data streams of different types.
Definition: synchronizer.h:49
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void setRGBFocalLength(const double rgb_focal_length)
Set the RGB image focal length (fx = fy).
boost::signals2::signal< sig_cb_openni_point_cloud_rgb > * point_cloud_rgb_signal_
boost::shared_ptr< openni_wrapper::OpenNIDevice > device_
The actual openni device.
void convertShiftToDepth(const uint16_t *shift_data_ptr, uint16_t *depth_data_ptr, std::size_t size) const
Convert vector of raw shift values to depth values.
boost::signals2::signal< sig_cb_openni_image_depth_image > * image_depth_image_signal_