Point Cloud Library (PCL)
1.10.1
|
41 #include <pcl/pcl_config.h>
46 #include <pcl/point_cloud.h>
47 #include <pcl/io/eigen.h>
48 #include <pcl/io/boost.h>
49 #include <pcl/io/grabber.h>
50 #include <pcl/io/openni_camera/openni_driver.h>
51 #include <pcl/io/openni_camera/openni_device_kinect.h>
52 #include <pcl/io/openni_camera/openni_image.h>
53 #include <pcl/io/openni_camera/openni_depth_image.h>
54 #include <pcl/io/openni_camera/openni_ir_image.h>
57 #include <pcl/common/synchronizer.h>
78 OpenNI_Default_Mode = 0,
85 OpenNI_QQVGA_25Hz = 7,
86 OpenNI_QQVGA_30Hz = 8,
108 const Mode& depth_mode = OpenNI_Default_Mode,
109 const Mode& image_mode = OpenNI_Default_Mode);
124 isRunning () const override;
127 getName () const override;
131 getFramesPerSecond () const override;
138 std::vector<std::pair<
int, XnMapOutputMode> >
139 getAvailableDepthModes () const;
142 std::vector<std::pair<
int, XnMapOutputMode> >
143 getAvailableImageModes () const;
154 setRGBCameraIntrinsics (const
double rgb_focal_length_x,
155 const
double rgb_focal_length_y,
156 const
double rgb_principal_point_x,
157 const
double rgb_principal_point_y)
159 rgb_focal_length_x_ = rgb_focal_length_x;
160 rgb_focal_length_y_ = rgb_focal_length_y;
161 rgb_principal_point_x_ = rgb_principal_point_x;
162 rgb_principal_point_y_ = rgb_principal_point_y;
173 double &rgb_focal_length_y,
174 double &rgb_principal_point_x,
175 double &rgb_principal_point_y)
const
177 rgb_focal_length_x = rgb_focal_length_x_;
178 rgb_focal_length_y = rgb_focal_length_y_;
179 rgb_principal_point_x = rgb_principal_point_x_;
180 rgb_principal_point_y = rgb_principal_point_y_;
193 rgb_focal_length_x_ = rgb_focal_length_y_ = rgb_focal_length;
206 rgb_focal_length_x_ = rgb_focal_length_x;
207 rgb_focal_length_y_ = rgb_focal_length_y;
217 rgb_focal_length_x = rgb_focal_length_x_;
218 rgb_focal_length_y = rgb_focal_length_y_;
231 const double depth_focal_length_y,
232 const double depth_principal_point_x,
233 const double depth_principal_point_y)
235 depth_focal_length_x_ = depth_focal_length_x;
236 depth_focal_length_y_ = depth_focal_length_y;
237 depth_principal_point_x_ = depth_principal_point_x;
238 depth_principal_point_y_ = depth_principal_point_y;
249 double &depth_focal_length_y,
250 double &depth_principal_point_x,
251 double &depth_principal_point_y)
const
253 depth_focal_length_x = depth_focal_length_x_;
254 depth_focal_length_y = depth_focal_length_y_;
255 depth_principal_point_x = depth_principal_point_x_;
256 depth_principal_point_y = depth_principal_point_y_;
267 depth_focal_length_x_ = depth_focal_length_y_ = depth_focal_length;
280 depth_focal_length_x_ = depth_focal_length_x;
281 depth_focal_length_y_ = depth_focal_length_y;
291 depth_focal_length_x = depth_focal_length_x_;
292 depth_focal_length_y = depth_focal_length_y_;
302 const std::uint16_t* shift_data_ptr,
303 std::uint16_t* depth_data_ptr,
304 std::size_t size)
const
307 auto openni_device = this->getDevice ();
309 const std::uint16_t* shift_data_it = shift_data_ptr;
310 std::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;
374 signalsChanged ()
override;
380 checkImageAndDepthSynchronizationRequired ();
384 checkImageStreamRequired ();
388 checkDepthStreamRequired ();
392 checkIRStreamRequired ();
449 bool operator () (
const XnMapOutputMode& mode1,
const XnMapOutputMode & mode2)
const
451 if (mode1.nXRes < mode2.nXRes)
453 if (mode1.nXRes > mode2.nXRes)
455 if (mode1.nYRes < mode2.nYRes)
457 if (mode1.nYRes > mode2.nYRes)
459 return (mode1.nFPS < mode2.nFPS);
503 #endif // HAVE_OPENNI
std::map< int, XnMapOutputMode > config2xn_map_
void setRGBFocalLength(const double rgb_focal_length)
Set the RGB image focal length (fx = fy).
Defines all the PCL and non-PCL macros used.
This file defines compatibility wrappers for low level I/O functions.
void(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &) sig_cb_openni_point_cloud_rgb
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)
boost::shared_array< unsigned short > depth_buffer_
boost::signals2::signal< sig_cb_openni_point_cloud_rgb > * point_cloud_rgb_signal_
double rgb_focal_length_y_
The RGB image focal length (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)
double depth_focal_length_x_
The depth image focal length (fx).
boost::signals2::signal< sig_cb_openni_point_cloud_rgba > * point_cloud_rgba_signal_
double rgb_focal_length_x_
The RGB image focal length (fx).
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_depth_image > * depth_image_signal_
pcl::shared_ptr< OpenNIDevice > Ptr
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &) sig_cb_openni_point_cloud_i
Grabber interface for PCL 1.x device drivers.
shared_ptr< const OpenNIGrabber > ConstPtr
boost::signals2::signal< sig_cb_openni_image > * image_signal_
pcl::shared_ptr< Image > Ptr
void(const openni_wrapper::IRImage::Ptr &) sig_cb_openni_ir_image
boost::signals2::signal< sig_cb_openni_point_cloud > * point_cloud_signal_
Synchronizer< openni_wrapper::IRImage::Ptr, openni_wrapper::DepthImage::Ptr > ir_sync_
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.
void(const openni_wrapper::Image::Ptr &) sig_cb_openni_image
std::string depth_frame_id_
void(const openni_wrapper::DepthImage::Ptr &) sig_cb_openni_depth_image
void getRGBFocalLength(double &rgb_focal_length_x, double &rgb_focal_length_y) const
Return the RGB focal length parameters (fx, fy)
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
double depth_focal_length_y_
The depth image focal length (fy).
double depth_principal_point_x_
The depth image principal point (cx).
boost::signals2::signal< sig_cb_openni_point_cloud_i > * point_cloud_i_signal_
void setDepthFocalLength(const double depth_focal_length)
Set the Depth image focal length (fx = fy).
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)
double rgb_principal_point_x_
The RGB image principal point (cx).
boost::signals2::signal< sig_cb_openni_ir_depth_image > * ir_depth_image_signal_
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_image > * ir_image_signal_
shared_ptr< PointCloud< PointT > > Ptr
void(const pcl::PointCloud< pcl::PointXYZRGBA >::ConstPtr &) sig_cb_openni_point_cloud_rgba
pcl::shared_ptr< IRImage > Ptr
pcl::shared_ptr< DepthImage > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
boost::shared_array< unsigned short > ir_buffer_
openni_wrapper::OpenNIDevice::Ptr device_
The actual openni device.
shared_ptr< OpenNIGrabber > Ptr
Grabber for OpenNI devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live)
double depth_principal_point_y_
The depth image principal point (cy).
void(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &) sig_cb_openni_point_cloud
openni_wrapper::OpenNIDevice::CallbackHandle depth_callback_handle
Synchronizer< openni_wrapper::Image::Ptr, openni_wrapper::DepthImage::Ptr > rgb_sync_
boost::shared_array< unsigned char > rgb_array_
void setRGBFocalLength(const double rgb_focal_length_x, const double rgb_focal_length_y)
Set the RGB image focal length.
openni_wrapper::OpenNIDevice::CallbackHandle image_callback_handle
double rgb_principal_point_y_
The RGB image principal point (cy).
openni_wrapper::OpenNIDevice::Ptr getDevice() const
Get a boost shared pointer to the pcl::openni_wrapper::OpenNIDevice object.
boost::signals2::signal< sig_cb_openni_image_depth_image > * image_depth_image_signal_
unsigned depth_buffer_size_
std::string rgb_frame_id_
openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
void(const openni_wrapper::Image::Ptr &, const openni_wrapper::DepthImage::Ptr &, float) sig_cb_openni_image_depth_image
void(const openni_wrapper::IRImage::Ptr &, const openni_wrapper::DepthImage::Ptr &, float) sig_cb_openni_ir_depth_image