41 #include <pcl/memory.h>
42 #include <pcl/pcl_config.h>
43 #include <pcl/pcl_macros.h>
47 #include <pcl/point_cloud.h>
48 #include <pcl/io/grabber.h>
49 #include <pcl/io/openni_camera/openni_driver.h>
50 #include <pcl/io/openni_camera/openni_device_kinect.h>
51 #include <pcl/io/openni_camera/openni_image.h>
52 #include <pcl/io/openni_camera/openni_depth_image.h>
53 #include <pcl/io/openni_camera/openni_ir_image.h>
55 #include <pcl/common/synchronizer.h>
56 #include <boost/shared_array.hpp>
72 using Ptr = shared_ptr<OpenNIGrabber>;
73 using ConstPtr = shared_ptr<const OpenNIGrabber>;
77 OpenNI_Default_Mode = 0,
84 OpenNI_QQVGA_25Hz = 7,
85 OpenNI_QQVGA_30Hz = 8,
107 const Mode& depth_mode = OpenNI_Default_Mode,
108 const Mode& image_mode = OpenNI_Default_Mode);
123 isRunning () const override;
126 getName () const override;
130 getFramesPerSecond () const override;
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 std::uint16_t* shift_data_ptr,
302 std::uint16_t* depth_data_ptr,
303 std::size_t size)
const
306 auto openni_device = this->getDevice ();
308 const std::uint16_t* shift_data_it = shift_data_ptr;
309 std::uint16_t* depth_data_it = depth_data_ptr;
312 for (std::size_t i=0; i<size; ++i)
314 *depth_data_it = openni_device->shiftToDepth(*shift_data_it);
326 onInit (
const std::string& device_id,
const Mode& depth_mode,
const Mode& image_mode);
330 setupDevice (
const std::string& device_id,
const Mode& depth_mode,
const Mode& image_mode);
338 startSynchronization ();
342 stopSynchronization ();
346 mapConfigMode2XnMode (
int mode, XnMapOutputMode &xnmode)
const;
373 signalsChanged ()
override;
379 checkImageAndDepthSynchronizationRequired ();
383 checkImageStreamRequired ();
387 checkDepthStreamRequired ();
391 checkIRStreamRequired ();
425 unsigned image_width_{0};
426 unsigned image_height_{0};
427 unsigned depth_width_{0};
428 unsigned depth_height_{0};
430 bool image_required_{
false};
431 bool depth_required_{
false};
432 bool ir_required_{
false};
433 bool sync_required_{
false};
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_{};
448 bool operator () (
const XnMapOutputMode& mode1,
const XnMapOutputMode & mode2)
const
450 if (mode1.nXRes < mode2.nXRes)
452 if (mode1.nXRes > mode2.nXRes)
454 if (mode1.nYRes < mode2.nYRes)
456 if (mode1.nYRes > mode2.nYRes)
458 return (mode1.nFPS < mode2.nFPS);
466 bool running_{
false};
468 mutable unsigned rgb_array_size_{0};
469 mutable unsigned depth_buffer_size_{0};
502 #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.
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)
Synchronizer< openni_wrapper::Image::Ptr, openni_wrapper::DepthImage::Ptr > rgb_sync_
shared_ptr< PointCloud< PointT > > Ptr
std::string depth_frame_id_
double depth_principal_point_y_
The depth image principal point (cy).
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &) sig_cb_openni_point_cloud
Synchronizer< openni_wrapper::IRImage::Ptr, openni_wrapper::DepthImage::Ptr > ir_sync_
double rgb_principal_point_y_
The RGB image principal point (cy).
double rgb_focal_length_x_
The RGB image focal length (fx).
openni_wrapper::OpenNIDevice::Ptr device_
The actual openni device.
Grabber interface for PCL 1.x device drivers.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Grabber for OpenNI devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live) ...
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &) sig_cb_openni_point_cloud_i
void(const openni_wrapper::DepthImage::Ptr &) sig_cb_openni_depth_image
void(const openni_wrapper::Image::Ptr &, const openni_wrapper::DepthImage::Ptr &, float) sig_cb_openni_image_depth_image
pcl::shared_ptr< IRImage > Ptr
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(const openni_wrapper::IRImage::Ptr &, const openni_wrapper::DepthImage::Ptr &, float) sig_cb_openni_ir_depth_image
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).
double depth_focal_length_x_
The depth image focal length (fx).
pcl::shared_ptr< OpenNIDevice > Ptr
std::map< int, XnMapOutputMode > config2xn_map_
shared_ptr< const OpenNIGrabber > ConstPtr
void(const openni_wrapper::Image::Ptr &) sig_cb_openni_image
pcl::shared_ptr< Image > Ptr
double rgb_focal_length_y_
The RGB image focal length (fy).
shared_ptr< OpenNIGrabber > Ptr
void setRGBFocalLength(const double rgb_focal_length)
Set the RGB image focal length (fx = fy).
pcl::shared_ptr< DepthImage > Ptr
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).
shared_ptr< const PointCloud< PointT > > ConstPtr
void getDepthFocalLength(double &depth_focal_length_x, double &depth_focal_length_y) const
Return the Depth focal length parameters (fx, fy)
void(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &) sig_cb_openni_point_cloud_rgb
double depth_focal_length_y_
The depth image focal length (fy).
std::string rgb_frame_id_
openni_wrapper::OpenNIDevice::Ptr getDevice() const
Get a pcl::shared pointer to the openni_wrapper::OpenNIDevice object.
void(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &) sig_cb_openni_point_cloud_rgba
void(const openni_wrapper::IRImage::Ptr &) sig_cb_openni_ir_image
void convertShiftToDepth(const std::uint16_t *shift_data_ptr, std::uint16_t *depth_data_ptr, std::size_t size) const
Convert vector of raw shift values to depth values.
double depth_principal_point_x_
The depth image principal point (cx).