39 #include <pcl/pcl_config.h> 42 #ifndef __PCL_IO_OPENNI_GRABBER__ 43 #define __PCL_IO_OPENNI_GRABBER__ 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> 55 #include <pcl/common/synchronizer.h> 72 typedef boost::shared_ptr<OpenNIGrabber>
Ptr;
73 typedef boost::shared_ptr<const OpenNIGrabber>
ConstPtr;
77 OpenNI_Default_Mode = 0,
84 OpenNI_QQVGA_25Hz = 7,
85 OpenNI_QQVGA_30Hz = 8,
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> >&);
107 const Mode& depth_mode = OpenNI_Default_Mode,
108 const Mode& image_mode = OpenNI_Default_Mode);
130 getFramesPerSecond () const;
137 std::vector<
std::pair<
int, XnMapOutputMode> >
138 getAvailableDepthModes () const;
141 std::vector<
std::pair<
int, XnMapOutputMode> >
142 getAvailableImageModes () const;
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)
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;
172 double &rgb_focal_length_y,
173 double &rgb_principal_point_x,
174 double &rgb_principal_point_y)
const 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_;
192 rgb_focal_length_x_ = rgb_focal_length_y_ = rgb_focal_length;
205 rgb_focal_length_x_ = rgb_focal_length_x;
206 rgb_focal_length_y_ = rgb_focal_length_y;
216 rgb_focal_length_x = rgb_focal_length_x_;
217 rgb_focal_length_y = rgb_focal_length_y_;
230 const double depth_focal_length_y,
231 const double depth_principal_point_x,
232 const double depth_principal_point_y)
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;
248 double &depth_focal_length_y,
249 double &depth_principal_point_x,
250 double &depth_principal_point_y)
const 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_;
266 depth_focal_length_x_ = depth_focal_length_y_ = depth_focal_length;
279 depth_focal_length_x_ = depth_focal_length_x;
280 depth_focal_length_y_ = depth_focal_length_y;
290 depth_focal_length_x = depth_focal_length_x_;
291 depth_focal_length_y = depth_focal_length_y_;
301 const uint16_t* shift_data_ptr,
302 uint16_t* depth_data_ptr,
303 std::size_t size)
const 306 boost::shared_ptr<openni_wrapper::OpenNIDevice> openni_device =
309 const uint16_t* shift_data_it = shift_data_ptr;
310 uint16_t* depth_data_it = depth_data_ptr;
313 for (std::size_t i=0; i<size; ++i)
315 *depth_data_it = openni_device->shiftToDepth(*shift_data_it);
327 onInit (
const std::string& device_id,
const Mode& depth_mode,
const Mode& image_mode);
331 setupDevice (
const std::string& device_id,
const Mode& depth_mode,
const Mode& image_mode);
339 startSynchronization ();
343 stopSynchronization ();
347 mapConfigMode2XnMode (
int mode, XnMapOutputMode &xnmode)
const;
352 imageCallback (boost::shared_ptr<openni_wrapper::Image> image,
void* cookie);
356 depthCallback (boost::shared_ptr<openni_wrapper::DepthImage> depth_image,
void* cookie);
360 irCallback (boost::shared_ptr<openni_wrapper::IRImage> ir_image,
void* cookie);
364 imageDepthImageCallback (
const boost::shared_ptr<openni_wrapper::Image> &image,
365 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image);
369 irDepthImageCallback (
const boost::shared_ptr<openni_wrapper::IRImage> &image,
370 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image);
380 checkImageAndDepthSynchronizationRequired ();
384 checkImageStreamRequired ();
388 checkDepthStreamRequired ();
392 checkIRStreamRequired ();
398 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> >
399 convertToXYZPointCloud (
const boost::shared_ptr<openni_wrapper::DepthImage> &depth)
const;
406 convertToXYZRGBPointCloud (
const boost::shared_ptr<openni_wrapper::Image> &image,
407 const boost::shared_ptr<openni_wrapper::DepthImage> &depth_image)
const;
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;
422 boost::shared_ptr<openni_wrapper::OpenNIDevice>
device_;
449 bool operator () (
const XnMapOutputMode& mode1,
const XnMapOutputMode & mode2)
const 451 if (mode1.nXRes < mode2.nXRes)
453 else if (mode1.nXRes > mode2.nXRes)
455 else if (mode1.nYRes < mode2.nYRes)
457 else if (mode1.nYRes > mode2.nYRes)
459 else if (mode1.nFPS < mode2.nFPS)
496 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
499 boost::shared_ptr<openni_wrapper::OpenNIDevice>
506 #endif // __PCL_IO_OPENNI_GRABBER__ 507 #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).
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.
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.
Grabber for OpenNI devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live) ...
boost::shared_ptr< PointCloud< PointT > > Ptr
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).