Point Cloud Library (PCL)  1.7.2
openni2_device.h
1 /*
2  * Copyright (c) 2013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  */
30 
31 #ifndef PCL_IO_OPENNI2_DEVICE_H_
32 #define PCL_IO_OPENNI2_DEVICE_H_
33 
34 #include <pcl/pcl_exports.h>
35 #include "openni.h"
36 #include "pcl/io/openni2/openni2_video_mode.h"
37 #include "pcl/io/io_exception.h"
38 
39 #include <boost/shared_ptr.hpp>
40 #include <boost/cstdint.hpp>
41 #include <boost/bind.hpp>
42 #include <boost/function.hpp>
43 #include <string>
44 #include <vector>
45 
46 // Template frame wrappers
47 #include <pcl/io/image.h>
48 #include <pcl/io/image_depth.h>
49 #include <pcl/io/image_ir.h>
50 
51 
52 
53 namespace openni
54 {
55  class Device;
56  class DeviceInfo;
57  class VideoStream;
58  class SensorInfo;
59 }
60 
61 namespace pcl
62 {
63  namespace io
64  {
65  namespace openni2
66  {
70 
72 
73  class PCL_EXPORTS OpenNI2Device
74  {
75  public:
76 
77  typedef boost::function<void(boost::shared_ptr<Image>, void* cookie) > ImageCallbackFunction;
78  typedef boost::function<void(boost::shared_ptr<DepthImage>, void* cookie) > DepthImageCallbackFunction;
79  typedef boost::function<void(boost::shared_ptr<IRImage>, void* cookie) > IRImageCallbackFunction;
80  typedef unsigned CallbackHandle;
81 
82  typedef boost::function<void(openni::VideoStream& stream)> StreamCallbackFunction;
83 
84  OpenNI2Device (const std::string& device_URI);
85  virtual ~OpenNI2Device ();
86 
87  const std::string
88  getUri () const;
89  const std::string
90  getVendor () const;
91  const std::string
92  getName () const;
93  uint16_t
94  getUsbVendorId () const;
95  uint16_t
96  getUsbProductId () const;
97 
98  const std::string
99  getStringID () const;
100 
101  bool
102  isValid () const;
103 
104  bool
105  hasIRSensor () const;
106  bool
107  hasColorSensor () const;
108  bool
109  hasDepthSensor () const;
110 
111  void
112  startIRStream ();
113  void
114  startColorStream ();
115  void
116  startDepthStream ();
117 
118  void
119  stopAllStreams ();
120 
121  void
122  stopIRStream ();
123  void
124  stopColorStream ();
125  void
126  stopDepthStream ();
127 
128  bool
129  isIRStreamStarted ();
130  bool
131  isColorStreamStarted ();
132  bool
133  isDepthStreamStarted ();
134 
135  bool
136  isImageRegistrationModeSupported () const;
137  void
138  setImageRegistrationMode (bool enabled);
139  bool
140  isDepthRegistered () const;
141 
142  const OpenNI2VideoMode
143  getIRVideoMode ();
144  const OpenNI2VideoMode
145  getColorVideoMode ();
146  const OpenNI2VideoMode
147  getDepthVideoMode ();
148 
149  const std::vector<OpenNI2VideoMode>&
150  getSupportedIRVideoModes () const;
151  const std::vector<OpenNI2VideoMode>&
152  getSupportedColorVideoModes () const;
153  const std::vector<OpenNI2VideoMode>&
154  getSupportedDepthVideoModes () const;
155 
156  bool
157  isIRVideoModeSupported (const OpenNI2VideoMode& video_mode) const;
158  bool
159  isColorVideoModeSupported (const OpenNI2VideoMode& video_mode) const;
160  bool
161  isDepthVideoModeSupported (const OpenNI2VideoMode& video_mode) const;
162 
163  bool
164  findCompatibleIRMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const;
165  bool
166  findCompatibleColorMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const;
167  bool
168  findCompatibleDepthMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const;
169 
170  void
171  setIRVideoMode (const OpenNI2VideoMode& video_mode);
172  void
173  setColorVideoMode (const OpenNI2VideoMode& video_mode);
174  void
175  setDepthVideoMode (const OpenNI2VideoMode& video_mode);
176 
178  getDefaultIRMode () const;
180  getDefaultColorMode () const;
182  getDefaultDepthMode () const;
183 
184  float
185  getIRFocalLength () const;
186  float
187  getColorFocalLength () const;
188  float
189  getDepthFocalLength () const;
190 
191  // Baseline between sensors. Returns 0 if this value does not exist.
192  float
193  getBaseline();
194 
195  // Value of pixels in shadow or that have no valid measurement
196  uint64_t
197  getShadowValue();
198 
199  void
200  setAutoExposure (bool enable);
201  void
202  setAutoWhiteBalance (bool enable);
203 
204  inline bool
206  {
207  return (openni_device_->getDepthColorSyncEnabled ());
208  }
209 
210  inline bool
212  {
213  return (true); // Not sure how to query this from the hardware
214  }
215 
216  inline bool
218  {
219  return (openni_device_->isFile());
220  }
221 
222  void
223  setSynchronization (bool enableSync);
224 
225  bool
226  getAutoExposure () const;
227  bool
228  getAutoWhiteBalance () const;
229 
230  void
231  setUseDeviceTimer (bool enable);
232 
233  /** \brief Get absolut number of depth frames in the current stream.
234  * This function returns 0 if the current device is not a file stream or
235  * if the current mode has no depth stream.
236  */
237  int
238  getDepthFrameCount ();
239 
240  /** \brief Get absolut number of color frames in the current stream.
241  * This function returns 0 if the current device is not a file stream or
242  * if the current mode has no color stream.
243  */
244  int
245  getColorFrameCount ();
246 
247  /** \brief Get absolut number of ir frames in the current stream.
248  * This function returns 0 if the current device is not a file stream or
249  * if the current mode has no ir stream.
250  */
251  int
252  getIRFrameCount ();
253 
254  /** \brief Set the playback speed if the device is an recorded stream.
255  * If setting the device playback speed fails, because the device is no recorded stream or
256  * any other reason this function returns false. Otherwise true is returned.
257  * \param[in] speed The playback speed factor 1.0 means the same speed as recorded,
258  * 0.5 half the speed, 2.0 double speed and so on.
259  * \return True on success, false otherwise.
260  */
261  bool
262  setPlaybackSpeed (double speed);
263 
264  /************************************************************************************/
265  // Callbacks from openni::VideoStream to grabber. Internal interface
266  void
267  setColorCallback (StreamCallbackFunction color_callback);
268  void
269  setDepthCallback (StreamCallbackFunction depth_callback);
270  void
271  setIRCallback (StreamCallbackFunction ir_callback);
272 
273  protected:
274  void shutdown ();
275 
276  boost::shared_ptr<openni::VideoStream>
277  getIRVideoStream () const;
278  boost::shared_ptr<openni::VideoStream>
279  getColorVideoStream () const;
280  boost::shared_ptr<openni::VideoStream>
281  getDepthVideoStream () const;
282 
283 
284  void
285  processColorFrame (openni::VideoStream& stream);
286  void
287  processDepthFrame (openni::VideoStream& stream);
288  void
289  processIRFrame (openni::VideoStream& stream);
290 
291 
292  bool
293  findCompatibleVideoMode (const std::vector<OpenNI2VideoMode> supportedModes,
294  const OpenNI2VideoMode& output_mode, OpenNI2VideoMode& mode) const;
295 
296  bool
297  resizingSupported (size_t input_width, size_t input_height, size_t output_width, size_t output_height) const;
298 
299  // Members
300 
301  boost::shared_ptr<openni::Device> openni_device_;
302  boost::shared_ptr<openni::DeviceInfo> device_info_;
303 
304  boost::shared_ptr<OpenNI2FrameListener> ir_frame_listener;
305  boost::shared_ptr<OpenNI2FrameListener> color_frame_listener;
306  boost::shared_ptr<OpenNI2FrameListener> depth_frame_listener;
307 
308  mutable boost::shared_ptr<openni::VideoStream> ir_video_stream_;
309  mutable boost::shared_ptr<openni::VideoStream> color_video_stream_;
310  mutable boost::shared_ptr<openni::VideoStream> depth_video_stream_;
311 
312  mutable std::vector<OpenNI2VideoMode> ir_video_modes_;
313  mutable std::vector<OpenNI2VideoMode> color_video_modes_;
314  mutable std::vector<OpenNI2VideoMode> depth_video_modes_;
315 
319 
320  /** \brief distance between the projector and the IR camera in meters*/
321  float baseline_;
322  /** the value for shadow (occluded pixels) */
323  uint64_t shadow_value_;
324  /** the value for pixels without a valid disparity measurement */
326  };
327 
328  PCL_EXPORTS std::ostream& operator<< (std::ostream& stream, const OpenNI2Device& device);
329 
330  } // namespace
331  }
332 }
333 
334 #endif // PCL_IO_OPENNI2_DEVICE_H_
uint64_t shadow_value_
the value for shadow (occluded pixels)
boost::shared_ptr< openni::DeviceInfo > device_info_
boost::shared_ptr< openni::VideoStream > color_video_stream_
uint64_t no_sample_value_
the value for pixels without a valid disparity measurement
std::vector< OpenNI2VideoMode > ir_video_modes_
std::vector< OpenNI2VideoMode > depth_video_modes_
float baseline_
distance between the projector and the IR camera in meters
Class containing just a reference to IR meta data.
Definition: image_ir.h:52
boost::function< void(boost::shared_ptr< Image >, void *cookie) > ImageCallbackFunction
boost::shared_ptr< openni::VideoStream > depth_video_stream_
boost::function< void(boost::shared_ptr< DepthImage >, void *cookie) > DepthImageCallbackFunction
boost::function< void(boost::shared_ptr< IRImage >, void *cookie) > IRImageCallbackFunction
boost::function< void(openni::VideoStream &stream)> StreamCallbackFunction
Image interface class providing an interface to fill a RGB or Grayscale image buffer.
Definition: image.h:56
boost::shared_ptr< openni::VideoStream > ir_video_stream_
boost::shared_ptr< OpenNI2FrameListener > color_frame_listener
std::vector< OpenNI2VideoMode > color_video_modes_
std::ostream & operator<<(std::ostream &os, const NarfKeypoint::Parameters &p)
This class provides methods to fill a depth or disparity image.
Definition: image_depth.h:56
boost::shared_ptr< openni::Device > openni_device_
boost::shared_ptr< OpenNI2FrameListener > depth_frame_listener
boost::shared_ptr< OpenNI2FrameListener > ir_frame_listener