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