Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
openni2_grabber.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2012, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 * Copyright (c) 2014, respective authors.
8 *
9 * All rights reserved.
10 *
11 * Redistribution and use in source and binary forms, with or without
12 * modification, are permitted provided that the following conditions
13 * are met:
14 *
15 * * Redistributions of source code must retain the above copyright
16 * notice, this list of conditions and the following disclaimer.
17 * * Redistributions in binary form must reproduce the above
18 * copyright notice, this list of conditions and the following
19 * disclaimer in the documentation and/or other materials provided
20 * with the distribution.
21 * * Neither the name of the copyright holder(s) nor the names of its
22 * contributors may be used to endorse or promote products derived
23 * from this software without specific prior written permission.
24 *
25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36 * POSSIBILITY OF SUCH DAMAGE.
37 *
38 */
39
40#pragma once
41
42#include <pcl/memory.h>
43#include <pcl/pcl_config.h>
44#include <pcl/pcl_macros.h>
45
46#ifdef HAVE_OPENNI2
47
48#include <pcl/point_cloud.h>
49#include <pcl/io/grabber.h>
50#include <pcl/io/openni2/openni2_device.h>
51#include <string>
52#include <pcl/common/synchronizer.h>
53
54#include <pcl/io/image.h>
55#include <pcl/io/image_rgb24.h>
56#include <pcl/io/image_yuv422.h>
57#include <pcl/io/image_depth.h>
58#include <pcl/io/image_ir.h>
59
60namespace pcl
61{
62 struct PointXYZ;
63 struct PointXYZRGB;
64 struct PointXYZRGBA;
65 struct PointXYZI;
66
67 namespace io
68 {
69
70 /** \brief Grabber for OpenNI 2 devices (i.e., Primesense PSDK, Microsoft Kinect, Asus XTion Pro/Live)
71 * \ingroup io
72 */
73 class PCL_EXPORTS OpenNI2Grabber : public Grabber
74 {
75 public:
76 using Ptr = shared_ptr<OpenNI2Grabber>;
77 using ConstPtr = shared_ptr<const OpenNI2Grabber>;
78
79 // Templated images
82 using Image = pcl::io::Image;
83
84 /** \brief Basic camera parameters placeholder. */
85 struct CameraParameters
86 {
87 /** fx */
88 double focal_length_x;
89 /** fy */
90 double focal_length_y;
91 /** cx */
92 double principal_point_x;
93 /** cy */
94 double principal_point_y;
95
96 CameraParameters (double initValue)
97 : focal_length_x (initValue), focal_length_y (initValue),
98 principal_point_x (initValue), principal_point_y (initValue)
99 {}
100
101 CameraParameters (double fx, double fy, double cx, double cy)
102 : focal_length_x (fx), focal_length_y (fy), principal_point_x (cx), principal_point_y (cy)
103 { }
104 };
105
106 enum Mode
107 {
108 OpenNI_Default_Mode = 0, // This can depend on the device. For now all devices (PSDK, Xtion, Kinect) its VGA@30Hz
109 OpenNI_SXGA_15Hz = 1, // Only supported by the Kinect
110 OpenNI_VGA_30Hz = 2, // Supported by PSDK, Xtion and Kinect
111 OpenNI_VGA_25Hz = 3, // Supportged by PSDK and Xtion
112 OpenNI_QVGA_25Hz = 4, // Supported by PSDK and Xtion
113 OpenNI_QVGA_30Hz = 5, // Supported by PSDK, Xtion and Kinect
114 OpenNI_QVGA_60Hz = 6, // Supported by PSDK and Xtion
115 OpenNI_QQVGA_25Hz = 7, // Not supported -> using software downsampling (only for integer scale factor and only NN)
116 OpenNI_QQVGA_30Hz = 8, // Not supported -> using software downsampling (only for integer scale factor and only NN)
117 OpenNI_QQVGA_60Hz = 9 // Not supported -> using software downsampling (only for integer scale factor and only NN)
118 };
119
120 //define callback signature typedefs
121 using sig_cb_openni_image = void (const Image::Ptr &);
122 using sig_cb_openni_depth_image = void (const DepthImage::Ptr &);
123 using sig_cb_openni_ir_image = void (const IRImage::Ptr &);
124 using sig_cb_openni_image_depth_image = void (const Image::Ptr &, const DepthImage::Ptr &, float) ;
125 using sig_cb_openni_ir_depth_image = void (const IRImage::Ptr &, const DepthImage::Ptr &, float) ;
126 using sig_cb_openni_point_cloud = void (const typename pcl::PointCloud<pcl::PointXYZ>::ConstPtr &);
127 using sig_cb_openni_point_cloud_rgb = void (const typename pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr &);
128 using sig_cb_openni_point_cloud_rgba = void (const typename pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &);
129 using sig_cb_openni_point_cloud_i = void (const typename pcl::PointCloud<pcl::PointXYZI>::ConstPtr &);
130
131 public:
132 /** \brief Constructor
133 * \param[in] device_id ID of the device, which might be a serial number, bus@address, URI or the index of the device.
134 * \param[in] depth_mode the mode of the depth stream
135 * \param[in] image_mode the mode of the image stream
136 * Depending on the value of \a device_id, the device is opened as follows:
137 * * If it corresponds to a file path, the device is opened with OpenNI2DeviceManager::getFileDevice
138 * * If it is an index of the form "#1234", the device is opened with OpenNI2DeviceManager::getDeviceByIndex
139 * * If it corresponds to an URI, the device is opened with OpenNI2DeviceManager::getDevice
140 * * If it is an empty string, the device is opened with OpenNI2DeviceManager::getAnyDevice
141 * * Otherwise a pcl::IOException instance is thrown
142 */
143 OpenNI2Grabber (const std::string& device_id = "",
144 const Mode& depth_mode = OpenNI_Default_Mode,
145 const Mode& image_mode = OpenNI_Default_Mode);
146
147 /** \brief virtual Destructor inherited from the Grabber interface. It never throws. */
148 ~OpenNI2Grabber () noexcept override;
149
150 /** \brief Start the data acquisition. */
151 void
152 start () override;
153
154 /** \brief Stop the data acquisition. */
155 void
156 stop () override;
157
158 /** \brief Check if the data acquisition is still running. */
159 bool
160 isRunning () const override;
161
162 std::string
163 getName () const override;
164
165 /** \brief Obtain the number of frames per second (FPS). */
166 float
167 getFramesPerSecond () const override;
168
169 /** \brief Get a boost shared pointer to the \ref OpenNIDevice object. */
170 inline pcl::io::openni2::OpenNI2Device::Ptr
171 getDevice () const;
172
173 /** \brief Obtain a list of the available depth modes that this device supports. */
174 std::vector<std::pair<int, pcl::io::openni2::OpenNI2VideoMode> >
175 getAvailableDepthModes () const;
176
177 /** \brief Obtain a list of the available image modes that this device supports. */
178 std::vector<std::pair<int, pcl::io::openni2::OpenNI2VideoMode> >
179 getAvailableImageModes () const;
180
181 /** \brief Set the RGB camera parameters (fx, fy, cx, cy)
182 * \param[in] rgb_focal_length_x the RGB focal length (fx)
183 * \param[in] rgb_focal_length_y the RGB focal length (fy)
184 * \param[in] rgb_principal_point_x the RGB principal point (cx)
185 * \param[in] rgb_principal_point_y the RGB principal point (cy)
186 * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them
187 * and the grabber will use the default values from the camera instead.
188 */
189 inline void
190 setRGBCameraIntrinsics (const double rgb_focal_length_x,
191 const double rgb_focal_length_y,
192 const double rgb_principal_point_x,
193 const double rgb_principal_point_y)
194 {
195 rgb_parameters_ = CameraParameters (
196 rgb_focal_length_x, rgb_focal_length_y,
197 rgb_principal_point_x, rgb_principal_point_y);
198 }
199
200 /** \brief Get the RGB camera parameters (fx, fy, cx, cy)
201 * \param[out] rgb_focal_length_x the RGB focal length (fx)
202 * \param[out] rgb_focal_length_y the RGB focal length (fy)
203 * \param[out] rgb_principal_point_x the RGB principal point (cx)
204 * \param[out] rgb_principal_point_y the RGB principal point (cy)
205 */
206 inline void
207 getRGBCameraIntrinsics (double &rgb_focal_length_x,
208 double &rgb_focal_length_y,
209 double &rgb_principal_point_x,
210 double &rgb_principal_point_y) const
211 {
212 rgb_focal_length_x = rgb_parameters_.focal_length_x;
213 rgb_focal_length_y = rgb_parameters_.focal_length_y;
214 rgb_principal_point_x = rgb_parameters_.principal_point_x;
215 rgb_principal_point_y = rgb_parameters_.principal_point_y;
216 }
217
218
219 /** \brief Set the RGB image focal length (fx = fy).
220 * \param[in] rgb_focal_length the RGB focal length (assumes fx = fy)
221 * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it
222 * and the grabber will use the default values from the camera instead.
223 * These parameters will be used for XYZRGBA clouds.
224 */
225 inline void
226 setRGBFocalLength (const double rgb_focal_length)
227 {
228 rgb_parameters_.focal_length_x = rgb_focal_length;
229 rgb_parameters_.focal_length_y = rgb_focal_length;
230 }
231
232 /** \brief Set the RGB image focal length
233 * \param[in] rgb_focal_length_x the RGB focal length (fx)
234 * \param[in] rgb_focal_ulength_y the RGB focal length (fy)
235 * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them
236 * and the grabber will use the default values from the camera instead.
237 * These parameters will be used for XYZRGBA clouds.
238 */
239 inline void
240 setRGBFocalLength (const double rgb_focal_length_x, const double rgb_focal_length_y)
241 {
242 rgb_parameters_.focal_length_x = rgb_focal_length_x;
243 rgb_parameters_.focal_length_y = rgb_focal_length_y;
244 }
245
246 /** \brief Return the RGB focal length parameters (fx, fy)
247 * \param[out] rgb_focal_length_x the RGB focal length (fx)
248 * \param[out] rgb_focal_length_y the RGB focal length (fy)
249 */
250 inline void
251 getRGBFocalLength (double &rgb_focal_length_x, double &rgb_focal_length_y) const
252 {
253 rgb_focal_length_x = rgb_parameters_.focal_length_x;
254 rgb_focal_length_y = rgb_parameters_.focal_length_y;
255 }
256
257 /** \brief Set the Depth camera parameters (fx, fy, cx, cy)
258 * \param[in] depth_focal_length_x the Depth focal length (fx)
259 * \param[in] depth_focal_length_y the Depth focal length (fy)
260 * \param[in] depth_principal_point_x the Depth principal point (cx)
261 * \param[in] depth_principal_point_y the Depth principal point (cy)
262 * Setting the parameters to non-finite values (e.g., NaN, Inf) invalidates them
263 * and the grabber will use the default values from the camera instead.
264 */
265 inline void
266 setDepthCameraIntrinsics (const double depth_focal_length_x,
267 const double depth_focal_length_y,
268 const double depth_principal_point_x,
269 const double depth_principal_point_y)
270 {
271 depth_parameters_ = CameraParameters (
272 depth_focal_length_x, depth_focal_length_y,
273 depth_principal_point_x, depth_principal_point_y);
274 }
275
276 /** \brief Get the Depth camera parameters (fx, fy, cx, cy)
277 * \param[out] depth_focal_length_x the Depth focal length (fx)
278 * \param[out] depth_focal_length_y the Depth focal length (fy)
279 * \param[out] depth_principal_point_x the Depth principal point (cx)
280 * \param[out] depth_principal_point_y the Depth principal point (cy)
281 */
282 inline void
283 getDepthCameraIntrinsics (double &depth_focal_length_x,
284 double &depth_focal_length_y,
285 double &depth_principal_point_x,
286 double &depth_principal_point_y) const
287 {
288 depth_focal_length_x = depth_parameters_.focal_length_x;
289 depth_focal_length_y = depth_parameters_.focal_length_y;
290 depth_principal_point_x = depth_parameters_.principal_point_x;
291 depth_principal_point_y = depth_parameters_.principal_point_y;
292 }
293
294 /** \brief Set the Depth image focal length (fx = fy).
295 * \param[in] depth_focal_length the Depth focal length (assumes fx = fy)
296 * Setting the parameter to a non-finite value (e.g., NaN, Inf) invalidates it
297 * and the grabber will use the default values from the camera instead.
298 */
299 inline void
300 setDepthFocalLength (const double depth_focal_length)
301 {
302 depth_parameters_.focal_length_x = depth_focal_length;
303 depth_parameters_.focal_length_y = depth_focal_length;
304 }
305
306
307 /** \brief Set the Depth image focal length
308 * \param[in] depth_focal_length_x the Depth focal length (fx)
309 * \param[in] depth_focal_length_y the Depth focal length (fy)
310 * Setting the parameter to non-finite values (e.g., NaN, Inf) invalidates them
311 * and the grabber will use the default values from the camera instead.
312 */
313 inline void
314 setDepthFocalLength (const double depth_focal_length_x, const double depth_focal_length_y)
315 {
316 depth_parameters_.focal_length_x = depth_focal_length_x;
317 depth_parameters_.focal_length_y = depth_focal_length_y;
318 }
319
320 /** \brief Return the Depth focal length parameters (fx, fy)
321 * \param[out] depth_focal_length_x the Depth focal length (fx)
322 * \param[out] depth_focal_length_y the Depth focal length (fy)
323 */
324 inline void
325 getDepthFocalLength (double &depth_focal_length_x, double &depth_focal_length_y) const
326 {
327 depth_focal_length_x = depth_parameters_.focal_length_x;
328 depth_focal_length_y = depth_parameters_.focal_length_y;
329 }
330
331 protected:
332
333 /** \brief Sets up an OpenNI device. */
334 void
335 setupDevice (const std::string& device_id, const Mode& depth_mode, const Mode& image_mode);
336
337 /** \brief Update mode maps. */
338 void
339 updateModeMaps ();
340
341 /** \brief Start synchronization. */
342 void
343 startSynchronization ();
344
345 /** \brief Stop synchronization. */
346 void
347 stopSynchronization ();
348
349 // TODO: rename to mapMode2OniMode
350 /** \brief Map config modes. */
351 bool
352 mapMode2XnMode (int mode, pcl::io::openni2::OpenNI2VideoMode& videoMode) const;
353
354 // callback methods
355 /** \brief RGB image callback. */
356 virtual void
357 imageCallback (pcl::io::openni2::Image::Ptr image, void* cookie);
358
359 /** \brief Depth image callback. */
360 virtual void
361 depthCallback (pcl::io::openni2::DepthImage::Ptr depth_image, void* cookie);
362
363 /** \brief IR image callback. */
364 virtual void
365 irCallback (pcl::io::openni2::IRImage::Ptr ir_image, void* cookie);
366
367 /** \brief RGB + Depth image callback. */
368 virtual void
369 imageDepthImageCallback (const pcl::io::openni2::Image::Ptr &image,
370 const pcl::io::openni2::DepthImage::Ptr &depth_image);
371
372 /** \brief IR + Depth image callback. */
373 virtual void
374 irDepthImageCallback (const pcl::io::openni2::IRImage::Ptr &image,
375 const pcl::io::openni2::DepthImage::Ptr &depth_image);
376
377 /** \brief Process changed signals. */
378 void
379 signalsChanged () override;
380
381 // helper methods
382
383 /** \brief Check if the RGB and Depth images are required to be synchronized or not. */
384 virtual void
385 checkImageAndDepthSynchronizationRequired ();
386
387 /** \brief Check if the RGB image stream is required or not. */
388 virtual void
389 checkImageStreamRequired ();
390
391 /** \brief Check if the depth stream is required or not. */
392 virtual void
393 checkDepthStreamRequired ();
394
395 /** \brief Check if the IR image stream is required or not. */
396 virtual void
397 checkIRStreamRequired ();
398
399
400 // Point cloud conversion ///////////////////////////////////////////////
401
402 /** \brief Convert a Depth image to a pcl::PointCloud<pcl::PointXYZ>
403 * \param[in] depth the depth image to convert
404 */
406 convertToXYZPointCloud (const pcl::io::openni2::DepthImage::Ptr &depth);
407
408 /** \brief Convert a Depth + RGB image pair to a pcl::PointCloud<PointT>
409 * \param[in] image the RGB image to convert
410 * \param[in] depth_image the depth image to convert
411 */
412 template <typename PointT> typename pcl::PointCloud<PointT>::Ptr
413 convertToXYZRGBPointCloud (const pcl::io::openni2::Image::Ptr &image,
414 const pcl::io::openni2::DepthImage::Ptr &depth_image);
415
416 /** \brief Convert a Depth + Intensity image pair to a pcl::PointCloud<pcl::PointXYZI>
417 * \param[in] image the IR image to convert
418 * \param[in] depth_image the depth image to convert
419 */
421 convertToXYZIPointCloud (const pcl::io::openni2::IRImage::Ptr &image,
422 const pcl::io::openni2::DepthImage::Ptr &depth_image);
423
424 std::vector<std::uint8_t> color_resize_buffer_;
425 std::vector<std::uint16_t> depth_resize_buffer_;
426 std::vector<std::uint16_t> ir_resize_buffer_;
427
428 // Stream callbacks /////////////////////////////////////////////////////
429 void
430 processColorFrame (openni::VideoStream& stream);
431
432 void
433 processDepthFrame (openni::VideoStream& stream);
434
435 void
436 processIRFrame (openni::VideoStream& stream);
437
438
439 Synchronizer<pcl::io::openni2::Image::Ptr, pcl::io::openni2::DepthImage::Ptr > rgb_sync_;
440 Synchronizer<pcl::io::openni2::IRImage::Ptr, pcl::io::openni2::DepthImage::Ptr > ir_sync_;
441
442 /** \brief The actual openni device. */
444
445 std::string rgb_frame_id_;
446 std::string depth_frame_id_;
447 unsigned image_width_;
448 unsigned image_height_;
449 unsigned depth_width_;
450 unsigned depth_height_;
451
452 bool image_required_;
453 bool depth_required_;
454 bool ir_required_;
455 bool sync_required_;
456
457 boost::signals2::signal<sig_cb_openni_image>* image_signal_;
458 boost::signals2::signal<sig_cb_openni_depth_image>* depth_image_signal_;
459 boost::signals2::signal<sig_cb_openni_ir_image>* ir_image_signal_;
460 boost::signals2::signal<sig_cb_openni_image_depth_image>* image_depth_image_signal_;
461 boost::signals2::signal<sig_cb_openni_ir_depth_image>* ir_depth_image_signal_;
462 boost::signals2::signal<sig_cb_openni_point_cloud>* point_cloud_signal_;
463 boost::signals2::signal<sig_cb_openni_point_cloud_i>* point_cloud_i_signal_;
464 boost::signals2::signal<sig_cb_openni_point_cloud_rgb>* point_cloud_rgb_signal_;
465 boost::signals2::signal<sig_cb_openni_point_cloud_rgba>* point_cloud_rgba_signal_;
466
467 struct modeComp
468 {
469 bool operator () (const openni::VideoMode& mode1, const openni::VideoMode & mode2) const
470 {
471 if (mode1.getResolutionX () < mode2.getResolutionX ())
472 return true;
473 if (mode1.getResolutionX () > mode2.getResolutionX ())
474 return false;
475 if (mode1.getResolutionY () < mode2.getResolutionY ())
476 return true;
477 if (mode1.getResolutionY () > mode2.getResolutionY ())
478 return false;
479 return (mode1.getFps () < mode2.getFps ());
480 }
481 };
482
483 // Mapping from config (enum) modes to native OpenNI modes
484 std::map<int, pcl::io::openni2::OpenNI2VideoMode> config2oni_map_;
485
489 bool running_;
490
491
492 CameraParameters rgb_parameters_;
493 CameraParameters depth_parameters_;
494
495 public:
497 };
498
500 OpenNI2Grabber::getDevice () const
501 {
502 return device_;
503 }
504
505 } // namespace
506}
507
508#endif // HAVE_OPENNI2
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
This class provides methods to fill a depth or disparity image.
Definition image_depth.h:55
shared_ptr< DepthImage > Ptr
Definition image_depth.h:57
Class containing just a reference to IR meta data.
Definition image_ir.h:55
shared_ptr< IRImage > Ptr
Definition image_ir.h:57
Image interface class providing an interface to fill a RGB or Grayscale image buffer.
Definition image.h:57
shared_ptr< Image > Ptr
Definition image.h:59
shared_ptr< OpenNI2Device > Ptr
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:63
Defines functions, macros and traits for allocating and using memory.
float4 PointXYZRGB
Definition internal.hpp:60
DeviceArray2D< uchar4 > Image
pcl::io::IRImage IRImage
pcl::io::DepthImage DepthImage
Defines all the PCL and non-PCL macros used.