Point Cloud Library (PCL)
1.11.1
|
42 #include <pcl/pcl_config.h>
47 #include <pcl/point_cloud.h>
48 #include <pcl/io/eigen.h>
49 #include <pcl/io/boost.h>
50 #include <pcl/io/grabber.h>
51 #include <pcl/io/openni_camera/openni_driver.h>
52 #include <pcl/io/openni_camera/openni_device_kinect.h>
53 #include <pcl/io/openni_camera/openni_image.h>
54 #include <pcl/io/openni_camera/openni_depth_image.h>
55 #include <pcl/io/openni_camera/openni_ir_image.h>
58 #include <pcl/common/synchronizer.h>
74 using Ptr = shared_ptr<OpenNIGrabber>;
75 using ConstPtr = shared_ptr<const OpenNIGrabber>;
79 OpenNI_Default_Mode = 0,
86 OpenNI_QQVGA_25Hz = 7,
87 OpenNI_QQVGA_30Hz = 8,
109 const Mode& depth_mode = OpenNI_Default_Mode,
110 const Mode& image_mode = OpenNI_Default_Mode);
125 isRunning () const override;
128 getName () const override;
132 getFramesPerSecond () const override;
139 std::vector<std::pair<
int, XnMapOutputMode> >
140 getAvailableDepthModes () const;
143 std::vector<std::pair<
int, XnMapOutputMode> >
144 getAvailableImageModes () const;
155 setRGBCameraIntrinsics (const
double rgb_focal_length_x,
156 const
double rgb_focal_length_y,
157 const
double rgb_principal_point_x,
158 const
double rgb_principal_point_y)
160 rgb_focal_length_x_ = rgb_focal_length_x;
161 rgb_focal_length_y_ = rgb_focal_length_y;
162 rgb_principal_point_x_ = rgb_principal_point_x;
163 rgb_principal_point_y_ = rgb_principal_point_y;
174 double &rgb_focal_length_y,
175 double &rgb_principal_point_x,
176 double &rgb_principal_point_y)
const
178 rgb_focal_length_x = rgb_focal_length_x_;
179 rgb_focal_length_y = rgb_focal_length_y_;
180 rgb_principal_point_x = rgb_principal_point_x_;
181 rgb_principal_point_y = rgb_principal_point_y_;
194 rgb_focal_length_x_ = rgb_focal_length_y_ = rgb_focal_length;
207 rgb_focal_length_x_ = rgb_focal_length_x;
208 rgb_focal_length_y_ = rgb_focal_length_y;
218 rgb_focal_length_x = rgb_focal_length_x_;
219 rgb_focal_length_y = rgb_focal_length_y_;
232 const double depth_focal_length_y,
233 const double depth_principal_point_x,
234 const double depth_principal_point_y)
236 depth_focal_length_x_ = depth_focal_length_x;
237 depth_focal_length_y_ = depth_focal_length_y;
238 depth_principal_point_x_ = depth_principal_point_x;
239 depth_principal_point_y_ = depth_principal_point_y;
250 double &depth_focal_length_y,
251 double &depth_principal_point_x,
252 double &depth_principal_point_y)
const
254 depth_focal_length_x = depth_focal_length_x_;
255 depth_focal_length_y = depth_focal_length_y_;
256 depth_principal_point_x = depth_principal_point_x_;
257 depth_principal_point_y = depth_principal_point_y_;
268 depth_focal_length_x_ = depth_focal_length_y_ = depth_focal_length;
281 depth_focal_length_x_ = depth_focal_length_x;
282 depth_focal_length_y_ = depth_focal_length_y;
292 depth_focal_length_x = depth_focal_length_x_;
293 depth_focal_length_y = depth_focal_length_y_;
305 std::size_t size)
const
308 auto openni_device = this->getDevice ();
314 for (std::size_t i=0; i<size; ++i)
316 *depth_data_it = openni_device->shiftToDepth(*shift_data_it);
328 onInit (
const std::string& device_id,
const Mode& depth_mode,
const Mode& image_mode);
450 bool operator () (
const XnMapOutputMode& mode1,
const XnMapOutputMode & mode2)
const
452 if (mode1.nXRes < mode2.nXRes)
454 if (mode1.nXRes > mode2.nXRes)
456 if (mode1.nYRes < mode2.nYRes)
458 if (mode1.nYRes > mode2.nYRes)
460 return (mode1.nFPS < mode2.nFPS);
504 #endif // HAVE_OPENNI
std::map< int, XnMapOutputMode > config2xn_map_
void setRGBFocalLength(const double rgb_focal_length)
Set the RGB image focal length (fx = fy).
bool mapConfigMode2XnMode(int mode, XnMapOutputMode &xnmode) const
Map config modes.
Defines all the PCL and non-PCL macros used.
virtual void checkIRStreamRequired()
Check if the IR image stream is required or not.
void(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &) sig_cb_openni_point_cloud_rgb
pcl::PointCloud< pcl::PointXYZ >::Ptr convertToXYZPointCloud(const openni_wrapper::DepthImage::Ptr &depth) const
Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ>
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_
void stopSynchronization()
Stop synchronization.
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).
virtual void checkDepthStreamRequired()
Check if the depth stream is required or not.
virtual void checkImageStreamRequired()
Check if the RGB image stream is required or not.
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
virtual void imageCallback(openni_wrapper::Image::Ptr image, void *cookie)
RGB image callback.
virtual void depthCallback(openni_wrapper::DepthImage::Ptr depth_image, void *cookie)
Depth image callback.
void(const pcl::PointCloud< pcl::PointXYZI >::ConstPtr &) sig_cb_openni_point_cloud_i
virtual void irCallback(openni_wrapper::IRImage::Ptr ir_image, void *cookie)
IR image callback.
Grabber interface for PCL 1.x device drivers.
virtual void irDepthImageCallback(const openni_wrapper::IRImage::Ptr &image, const openni_wrapper::DepthImage::Ptr &depth_image)
IR + Depth image callback.
pcl::PointCloud< PointT >::Ptr convertToXYZRGBPointCloud(const openni_wrapper::Image::Ptr &image, const openni_wrapper::DepthImage::Ptr &depth_image) const
Convert a Depth + RGB image pair to a pcl::PointCloud<PointT>
shared_ptr< const OpenNIGrabber > ConstPtr
boost::signals2::signal< sig_cb_openni_image > * image_signal_
pcl::shared_ptr< Image > Ptr
void signalsChanged() override
Process changed signals.
void(const openni_wrapper::IRImage::Ptr &) sig_cb_openni_ir_image
boost::signals2::signal< sig_cb_openni_point_cloud > * point_cloud_signal_
void setupDevice(const std::string &device_id, const Mode &depth_mode, const Mode &image_mode)
Sets up an OpenNI device.
OpenNIGrabber(const std::string &device_id="", const Mode &depth_mode=OpenNI_Default_Mode, const Mode &image_mode=OpenNI_Default_Mode)
Constructor.
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 startSynchronization()
Start synchronization.
void getRGBFocalLength(double &rgb_focal_length_x, double &rgb_focal_length_y) const
Return the RGB focal length parameters (fx, fy)
virtual void checkImageAndDepthSynchronizationRequired()
Check if the RGB and Depth images are required to be synchronized or not.
#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_
pcl::PointCloud< pcl::PointXYZI >::Ptr convertToXYZIPointCloud(const openni_wrapper::IRImage::Ptr &image, const openni_wrapper::DepthImage::Ptr &depth_image) const
Convert a Depth + Intensity image pair to a pcl::PointCloud<pcl::PointXYZI>
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
void onInit(const std::string &device_id, const Mode &depth_mode, const Mode &image_mode)
On initialization processing.
virtual void imageDepthImageCallback(const openni_wrapper::Image::Ptr &image, const openni_wrapper::DepthImage::Ptr &depth_image)
RGB + Depth image callback.
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
~OpenNIGrabber() noexcept
virtual Destructor inherited from the Grabber interface.
Synchronizer< openni_wrapper::Image::Ptr, openni_wrapper::DepthImage::Ptr > rgb_sync_
boost::shared_array< unsigned char > rgb_array_
void updateModeMaps()
Update mode maps.
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 pcl::shared pointer to the openni_wrapper::OpenNIDevice object.
boost::signals2::signal< sig_cb_openni_image_depth_image > * image_depth_image_signal_
Defines functions, macros and traits for allocating and using memory.
unsigned depth_buffer_size_
std::string rgb_frame_id_
openni_wrapper::OpenNIDevice::CallbackHandle ir_callback_handle
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