40 #include <pcl/pcl_config.h>
43 #ifndef PCL_IO_OPENNI2_GRABBER_H_
44 #define PCL_IO_OPENNI2_GRABBER_H_
46 #include <pcl/io/eigen.h>
47 #include <pcl/io/boost.h>
48 #include <pcl/io/grabber.h>
49 #include <pcl/io/openni2/openni2_device_manager.h>
50 #include <pcl/io/openni2/openni2_device.h>
53 #include <pcl/common/synchronizer.h>
55 #include <pcl/io/image.h>
56 #include <pcl/io/image_rgb24.h>
57 #include <pcl/io/image_yuv422.h>
58 #include <pcl/io/image_depth.h>
59 #include <pcl/io/image_ir.h>
75 class PCL_EXPORTS OpenNI2Grabber :
public Grabber
78 typedef boost::shared_ptr<OpenNI2Grabber> Ptr;
79 typedef boost::shared_ptr<const OpenNI2Grabber> ConstPtr;
87 struct CameraParameters
90 double focal_length_x;
92 double focal_length_y;
94 double principal_point_x;
96 double principal_point_y;
98 CameraParameters (
double initValue)
99 : focal_length_x (initValue), focal_length_y (initValue),
100 principal_point_x (initValue), principal_point_y (initValue)
103 CameraParameters (
double fx,
double fy,
double cx,
double cy)
104 : focal_length_x (fx), focal_length_y (fy), principal_point_x (cx), principal_point_y (cy)
110 OpenNI_Default_Mode = 0,
111 OpenNI_SXGA_15Hz = 1,
114 OpenNI_QVGA_25Hz = 4,
115 OpenNI_QVGA_30Hz = 5,
116 OpenNI_QVGA_60Hz = 6,
117 OpenNI_QQVGA_25Hz = 7,
118 OpenNI_QQVGA_30Hz = 8,
119 OpenNI_QQVGA_60Hz = 9
123 typedef void (sig_cb_openni_image) (
const boost::shared_ptr<Image>&);
124 typedef void (sig_cb_openni_depth_image) (
const boost::shared_ptr<DepthImage>&);
125 typedef void (sig_cb_openni_ir_image) (
const boost::shared_ptr<IRImage>&);
126 typedef void (sig_cb_openni_image_depth_image) (
const boost::shared_ptr<Image>&,
const boost::shared_ptr<DepthImage>&,
float reciprocalFocalLength) ;
127 typedef void (sig_cb_openni_ir_depth_image) (
const boost::shared_ptr<IRImage>&,
const boost::shared_ptr<DepthImage>&,
float reciprocalFocalLength) ;
128 typedef void (sig_cb_openni_point_cloud) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZ> >&);
129 typedef void (sig_cb_openni_point_cloud_rgb) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGB> >&);
130 typedef void (sig_cb_openni_point_cloud_rgba) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZRGBA> >&);
131 typedef void (sig_cb_openni_point_cloud_i) (
const boost::shared_ptr<const pcl::PointCloud<pcl::PointXYZI> >&);
139 OpenNI2Grabber (
const std::string& device_id =
"",
140 const Mode& depth_mode = OpenNI_Default_Mode,
141 const Mode& image_mode = OpenNI_Default_Mode);
144 virtual ~OpenNI2Grabber () throw ();
163 getFramesPerSecond () const;
166 inline
boost::shared_ptr<
pcl::io::openni2::OpenNI2Device>
170 std::vector<
std::pair<
int,
pcl::io::openni2::OpenNI2VideoMode> >
171 getAvailableDepthModes () const;
174 std::vector<
std::pair<
int,
pcl::io::openni2::OpenNI2VideoMode> >
175 getAvailableImageModes () const;
186 setRGBCameraIntrinsics (const
double rgb_focal_length_x,
187 const
double rgb_focal_length_y,
188 const
double rgb_principal_point_x,
189 const
double rgb_principal_point_y)
191 rgb_parameters_ = CameraParameters (
192 rgb_focal_length_x, rgb_focal_length_y,
193 rgb_principal_point_x, rgb_principal_point_y);
203 getRGBCameraIntrinsics (
double &rgb_focal_length_x,
204 double &rgb_focal_length_y,
205 double &rgb_principal_point_x,
206 double &rgb_principal_point_y)
const
208 rgb_focal_length_x = rgb_parameters_.focal_length_x;
209 rgb_focal_length_y = rgb_parameters_.focal_length_y;
210 rgb_principal_point_x = rgb_parameters_.principal_point_x;
211 rgb_principal_point_y = rgb_parameters_.principal_point_y;
222 setRGBFocalLength (
const double rgb_focal_length)
224 rgb_parameters_.focal_length_x = rgb_focal_length;
225 rgb_parameters_.focal_length_y = rgb_focal_length;
236 setRGBFocalLength (
const double rgb_focal_length_x,
const double rgb_focal_length_y)
238 rgb_parameters_.focal_length_x = rgb_focal_length_x;
239 rgb_parameters_.focal_length_y = rgb_focal_length_y;
247 getRGBFocalLength (
double &rgb_focal_length_x,
double &rgb_focal_length_y)
const
249 rgb_focal_length_x = rgb_parameters_.focal_length_x;
250 rgb_focal_length_y = rgb_parameters_.focal_length_y;
262 setDepthCameraIntrinsics (
const double depth_focal_length_x,
263 const double depth_focal_length_y,
264 const double depth_principal_point_x,
265 const double depth_principal_point_y)
267 depth_parameters_ = CameraParameters (
268 depth_focal_length_x, depth_focal_length_y,
269 depth_principal_point_x, depth_principal_point_y);
279 getDepthCameraIntrinsics (
double &depth_focal_length_x,
280 double &depth_focal_length_y,
281 double &depth_principal_point_x,
282 double &depth_principal_point_y)
const
284 depth_focal_length_x = depth_parameters_.focal_length_x;
285 depth_focal_length_y = depth_parameters_.focal_length_y;
286 depth_principal_point_x = depth_parameters_.principal_point_x;
287 depth_principal_point_y = depth_parameters_.principal_point_y;
296 setDepthFocalLength (
const double depth_focal_length)
298 depth_parameters_.focal_length_x = depth_focal_length;
299 depth_parameters_.focal_length_y = depth_focal_length;
310 setDepthFocalLength (
const double depth_focal_length_x,
const double depth_focal_length_y)
312 depth_parameters_.focal_length_x = depth_focal_length_x;
313 depth_parameters_.focal_length_y = depth_focal_length_y;
321 getDepthFocalLength (
double &depth_focal_length_x,
double &depth_focal_length_y)
const
323 depth_focal_length_x = depth_parameters_.focal_length_x;
324 depth_focal_length_y = depth_parameters_.focal_length_y;
331 setupDevice (
const std::string& device_id,
const Mode& depth_mode,
const Mode& image_mode);
339 startSynchronization ();
343 stopSynchronization ();
381 checkImageAndDepthSynchronizationRequired ();
385 checkImageStreamRequired ();
389 checkDepthStreamRequired ();
393 checkIRStreamRequired ();
401 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ> >
416 boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI> >
420 std::vector<uint8_t> color_resize_buffer_;
421 std::vector<uint16_t> depth_resize_buffer_;
422 std::vector<uint16_t> ir_resize_buffer_;
426 processColorFrame (openni::VideoStream& stream);
429 processDepthFrame (openni::VideoStream& stream);
432 processIRFrame (openni::VideoStream& stream);
435 Synchronizer<pcl::io::openni2::Image::Ptr, pcl::io::openni2::DepthImage::Ptr > rgb_sync_;
436 Synchronizer<pcl::io::openni2::IRImage::Ptr, pcl::io::openni2::DepthImage::Ptr > ir_sync_;
439 boost::shared_ptr<pcl::io::openni2::OpenNI2Device> device_;
441 std::string rgb_frame_id_;
442 std::string depth_frame_id_;
443 unsigned image_width_;
444 unsigned image_height_;
445 unsigned depth_width_;
446 unsigned depth_height_;
448 bool image_required_;
449 bool depth_required_;
453 boost::signals2::signal<sig_cb_openni_image>* image_signal_;
454 boost::signals2::signal<sig_cb_openni_depth_image>* depth_image_signal_;
455 boost::signals2::signal<sig_cb_openni_ir_image>* ir_image_signal_;
456 boost::signals2::signal<sig_cb_openni_image_depth_image>* image_depth_image_signal_;
457 boost::signals2::signal<sig_cb_openni_ir_depth_image>* ir_depth_image_signal_;
458 boost::signals2::signal<sig_cb_openni_point_cloud>* point_cloud_signal_;
459 boost::signals2::signal<sig_cb_openni_point_cloud_i>* point_cloud_i_signal_;
460 boost::signals2::signal<sig_cb_openni_point_cloud_rgb>* point_cloud_rgb_signal_;
461 boost::signals2::signal<sig_cb_openni_point_cloud_rgba>* point_cloud_rgba_signal_;
465 bool operator () (
const openni::VideoMode& mode1,
const openni::VideoMode & mode2)
const
467 if (mode1.getResolutionX () < mode2.getResolutionX ())
469 else if (mode1.getResolutionX () > mode2.getResolutionX ())
471 else if (mode1.getResolutionY () < mode2.getResolutionY ())
473 else if (mode1.getResolutionY () > mode2.getResolutionY ())
475 else if (mode1.getFps () < mode2.getFps () )
483 std::map<int, pcl::io::openni2::OpenNI2VideoMode> config2oni_map_;
491 CameraParameters rgb_parameters_;
492 CameraParameters depth_parameters_;
495 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
498 boost::shared_ptr<pcl::io::openni2::OpenNI2Device>
499 OpenNI2Grabber::getDevice ()
const
507 #endif // PCL_IO_OPENNI2_GRABBER_H_
508 #endif // HAVE_OPENNI2
Class containing just a reference to IR meta data.
boost::shared_ptr< IRImage > Ptr
pcl::io::DepthImage DepthImage
boost::shared_ptr< PointCloud< PointT > > Ptr
boost::shared_ptr< DepthImage > Ptr
boost::shared_ptr< Image > Ptr
Image interface class providing an interface to fill a RGB or Grayscale image buffer.
This class provides methods to fill a depth or disparity image.
PointCloud represents the base class in PCL for storing collections of 3D points. ...