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/openni2/openni2_device.h>
54 #include <pcl/common/synchronizer.h>
56 #include <pcl/io/image.h>
57 #include <pcl/io/image_rgb24.h>
58 #include <pcl/io/image_yuv422.h>
59 #include <pcl/io/image_depth.h>
60 #include <pcl/io/image_ir.h>
78 using Ptr = shared_ptr<OpenNI2Grabber>;
79 using ConstPtr = shared_ptr<const OpenNI2Grabber>;
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 using sig_cb_openni_image = void (
const Image::Ptr &);
125 using sig_cb_openni_ir_image = void (
const IRImage::Ptr &);
145 OpenNI2Grabber (
const std::string& device_id =
"",
146 const Mode& depth_mode = OpenNI_Default_Mode,
147 const Mode& image_mode = OpenNI_Default_Mode);
150 ~OpenNI2Grabber () throw ();
162 isRunning () const override;
165 getName () const override;
169 getFramesPerSecond () const override;
172 inline
pcl::io::openni2::OpenNI2Device::Ptr
176 std::vector<std::pair<
int,
pcl::io::openni2::OpenNI2VideoMode> >
177 getAvailableDepthModes () const;
180 std::vector<std::pair<
int,
pcl::io::openni2::OpenNI2VideoMode> >
181 getAvailableImageModes () const;
192 setRGBCameraIntrinsics (const
double rgb_focal_length_x,
193 const
double rgb_focal_length_y,
194 const
double rgb_principal_point_x,
195 const
double rgb_principal_point_y)
197 rgb_parameters_ = CameraParameters (
198 rgb_focal_length_x, rgb_focal_length_y,
199 rgb_principal_point_x, rgb_principal_point_y);
209 getRGBCameraIntrinsics (
double &rgb_focal_length_x,
210 double &rgb_focal_length_y,
211 double &rgb_principal_point_x,
212 double &rgb_principal_point_y)
const
214 rgb_focal_length_x = rgb_parameters_.focal_length_x;
215 rgb_focal_length_y = rgb_parameters_.focal_length_y;
216 rgb_principal_point_x = rgb_parameters_.principal_point_x;
217 rgb_principal_point_y = rgb_parameters_.principal_point_y;
228 setRGBFocalLength (
const double rgb_focal_length)
230 rgb_parameters_.focal_length_x = rgb_focal_length;
231 rgb_parameters_.focal_length_y = rgb_focal_length;
242 setRGBFocalLength (
const double rgb_focal_length_x,
const double rgb_focal_length_y)
244 rgb_parameters_.focal_length_x = rgb_focal_length_x;
245 rgb_parameters_.focal_length_y = rgb_focal_length_y;
253 getRGBFocalLength (
double &rgb_focal_length_x,
double &rgb_focal_length_y)
const
255 rgb_focal_length_x = rgb_parameters_.focal_length_x;
256 rgb_focal_length_y = rgb_parameters_.focal_length_y;
268 setDepthCameraIntrinsics (
const double depth_focal_length_x,
269 const double depth_focal_length_y,
270 const double depth_principal_point_x,
271 const double depth_principal_point_y)
273 depth_parameters_ = CameraParameters (
274 depth_focal_length_x, depth_focal_length_y,
275 depth_principal_point_x, depth_principal_point_y);
285 getDepthCameraIntrinsics (
double &depth_focal_length_x,
286 double &depth_focal_length_y,
287 double &depth_principal_point_x,
288 double &depth_principal_point_y)
const
290 depth_focal_length_x = depth_parameters_.focal_length_x;
291 depth_focal_length_y = depth_parameters_.focal_length_y;
292 depth_principal_point_x = depth_parameters_.principal_point_x;
293 depth_principal_point_y = depth_parameters_.principal_point_y;
302 setDepthFocalLength (
const double depth_focal_length)
304 depth_parameters_.focal_length_x = depth_focal_length;
305 depth_parameters_.focal_length_y = depth_focal_length;
316 setDepthFocalLength (
const double depth_focal_length_x,
const double depth_focal_length_y)
318 depth_parameters_.focal_length_x = depth_focal_length_x;
319 depth_parameters_.focal_length_y = depth_focal_length_y;
327 getDepthFocalLength (
double &depth_focal_length_x,
double &depth_focal_length_y)
const
329 depth_focal_length_x = depth_parameters_.focal_length_x;
330 depth_focal_length_y = depth_parameters_.focal_length_y;
337 setupDevice (
const std::string& device_id,
const Mode& depth_mode,
const Mode& image_mode);
345 startSynchronization ();
349 stopSynchronization ();
381 signalsChanged ()
override;
387 checkImageAndDepthSynchronizationRequired ();
391 checkImageStreamRequired ();
395 checkDepthStreamRequired ();
399 checkIRStreamRequired ();
426 std::vector<std::uint8_t> color_resize_buffer_;
427 std::vector<std::uint16_t> depth_resize_buffer_;
428 std::vector<std::uint16_t> ir_resize_buffer_;
432 processColorFrame (openni::VideoStream& stream);
435 processDepthFrame (openni::VideoStream& stream);
438 processIRFrame (openni::VideoStream& stream);
441 Synchronizer<pcl::io::openni2::Image::Ptr, pcl::io::openni2::DepthImage::Ptr > rgb_sync_;
442 Synchronizer<pcl::io::openni2::IRImage::Ptr, pcl::io::openni2::DepthImage::Ptr > ir_sync_;
447 std::string rgb_frame_id_;
448 std::string depth_frame_id_;
449 unsigned image_width_;
450 unsigned image_height_;
451 unsigned depth_width_;
452 unsigned depth_height_;
454 bool image_required_;
455 bool depth_required_;
459 boost::signals2::signal<sig_cb_openni_image>* image_signal_;
460 boost::signals2::signal<sig_cb_openni_depth_image>* depth_image_signal_;
461 boost::signals2::signal<sig_cb_openni_ir_image>* ir_image_signal_;
462 boost::signals2::signal<sig_cb_openni_image_depth_image>* image_depth_image_signal_;
463 boost::signals2::signal<sig_cb_openni_ir_depth_image>* ir_depth_image_signal_;
464 boost::signals2::signal<sig_cb_openni_point_cloud>* point_cloud_signal_;
465 boost::signals2::signal<sig_cb_openni_point_cloud_i>* point_cloud_i_signal_;
466 boost::signals2::signal<sig_cb_openni_point_cloud_rgb>* point_cloud_rgb_signal_;
467 boost::signals2::signal<sig_cb_openni_point_cloud_rgba>* point_cloud_rgba_signal_;
471 bool operator () (
const openni::VideoMode& mode1,
const openni::VideoMode & mode2)
const
473 if (mode1.getResolutionX () < mode2.getResolutionX ())
475 if (mode1.getResolutionX () > mode2.getResolutionX ())
477 if (mode1.getResolutionY () < mode2.getResolutionY ())
479 if (mode1.getResolutionY () > mode2.getResolutionY ())
481 return (mode1.getFps () < mode2.getFps ());
486 std::map<int, pcl::io::openni2::OpenNI2VideoMode> config2oni_map_;
494 CameraParameters rgb_parameters_;
495 CameraParameters depth_parameters_;
502 OpenNI2Grabber::getDevice ()
const
510 #endif // HAVE_OPENNI2