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 #pragma once
32 
33 #include <pcl/memory.h>
34 #include <pcl/pcl_exports.h>
35 
36 // Template frame wrappers
37 #include <pcl/io/image.h>
38 #include <pcl/io/image_depth.h>
39 #include <pcl/io/image_ir.h>
40 
41 #include <pcl/io/io_exception.h>
42 #include <pcl/io/openni2/openni2_video_mode.h>
43 
44 #include "openni.h"
45 
46 #include <cstdint>
47 #include <functional>
48 #include <memory>
49 #include <string>
50 #include <vector>
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     {
67       using DepthImage = pcl::io::DepthImage;
68       using IRImage = pcl::io::IRImage;
69       using Image = pcl::io::Image;
70 
71       class OpenNI2FrameListener;
72 
73       class PCL_EXPORTS OpenNI2Device
74       {
75         public:
76           using Ptr = shared_ptr<OpenNI2Device>;
77           using ConstPtr = shared_ptr<const OpenNI2Device>;
78 
79           using ImageCallbackFunction = std::function<void(Image::Ptr, void* cookie) >;
80           using DepthImageCallbackFunction = std::function<void(DepthImage::Ptr, void* cookie) >;
81           using IRImageCallbackFunction = std::function<void(IRImage::Ptr, void* cookie) >;
82           using CallbackHandle = unsigned;
83 
84           using StreamCallbackFunction = std::function<void(openni::VideoStream& stream)>;
85 
86           OpenNI2Device (const std::string& device_URI);
87           virtual ~OpenNI2Device ();
88 
89           const std::string
90           getUri () const;
91           const std::string
92           getVendor () const;
93           const std::string
94           getName () const;
95           std::uint16_t
96           getUsbVendorId () const;
97           std::uint16_t
98           getUsbProductId () const;
99 
100           const std::string
101           getStringID () const;
102 
103           bool
104           isValid () const;
105 
106           bool
107           hasIRSensor () const;
108           bool
109           hasColorSensor () const;
110           bool
111           hasDepthSensor () const;
112 
113           void
114           startIRStream ();
115           void
116           startColorStream ();
117           void
118           startDepthStream ();
119 
120           void
121           stopAllStreams ();
122 
123           void
124           stopIRStream ();
125           void
126           stopColorStream ();
127           void
128           stopDepthStream ();
129 
130           bool
131           isIRStreamStarted ();
132           bool
133           isColorStreamStarted ();
134           bool
135           isDepthStreamStarted ();
136 
137           bool
138           isImageRegistrationModeSupported () const;
139           void
140           setImageRegistrationMode (bool enabled);
141           bool
142           isDepthRegistered () const;
143 
144           const OpenNI2VideoMode
145           getIRVideoMode ();
146           const OpenNI2VideoMode
147           getColorVideoMode ();
148           const OpenNI2VideoMode
149           getDepthVideoMode ();
150 
151           const std::vector<OpenNI2VideoMode>&
152           getSupportedIRVideoModes () const;
153           const std::vector<OpenNI2VideoMode>&
154           getSupportedColorVideoModes () const;
155           const std::vector<OpenNI2VideoMode>&
156           getSupportedDepthVideoModes () const;
157 
158           bool
159           isIRVideoModeSupported (const OpenNI2VideoMode& video_mode) const;
160           bool
161           isColorVideoModeSupported (const OpenNI2VideoMode& video_mode) const;
162           bool
163           isDepthVideoModeSupported (const OpenNI2VideoMode& video_mode) const;
164 
165           bool
166           findCompatibleIRMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const;
167           bool
168           findCompatibleColorMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const;
169           bool
170           findCompatibleDepthMode (const OpenNI2VideoMode& requested_mode, OpenNI2VideoMode& actual_mode) const;
171 
172           void
173           setIRVideoMode (const OpenNI2VideoMode& video_mode);
174           void
175           setColorVideoMode (const OpenNI2VideoMode& video_mode);
176           void
177           setDepthVideoMode (const OpenNI2VideoMode& video_mode);
178 
179           OpenNI2VideoMode
180           getDefaultIRMode () const;
181           OpenNI2VideoMode
182           getDefaultColorMode () const;
183           OpenNI2VideoMode
184           getDefaultDepthMode () const;
185 
186           float
187           getIRFocalLength () const;
188           float
189           getColorFocalLength () const;
190           float
191           getDepthFocalLength () const;
192 
193           // Baseline between sensors. Returns 0 if this value does not exist.
194           float
195           getBaseline();
196 
197           // Value of pixels in shadow or that have no valid measurement
198           std::uint64_t
199           getShadowValue();
200 
201           void
202           setAutoExposure (bool enable);
203           void
204           setAutoWhiteBalance (bool enable);
205 
206           inline bool
isSynchronized()207           isSynchronized ()
208           {
209             return (openni_device_->getDepthColorSyncEnabled ());
210           }
211 
212           inline bool
isSynchronizationSupported()213           isSynchronizationSupported ()
214           {
215             return (true); // Not sure how to query this from the hardware
216           }
217 
218           inline bool
isFile()219           isFile()
220           {
221             return (openni_device_->isFile());
222           }
223 
224           void
225           setSynchronization (bool enableSync);
226 
227           bool
228           getAutoExposure () const;
229           bool
230           getAutoWhiteBalance () const;
231 
232           void
233           setUseDeviceTimer (bool enable);
234 
235           /** \brief Get absolute number of depth frames in the current stream.
236           * This function returns 0 if the current device is not a file stream or
237           * if the current mode has no depth stream.
238           */
239           int
240           getDepthFrameCount ();
241 
242           /** \brief Get absolute number of color frames in the current stream.
243           * This function returns 0 if the current device is not a file stream or
244           * if the current mode has no color stream.
245           */
246           int
247           getColorFrameCount ();
248 
249           /** \brief Get absolute number of ir frames in the current stream.
250           * This function returns 0 if the current device is not a file stream or
251           * if the current mode has no ir stream.
252           */
253           int
254           getIRFrameCount ();
255 
256           /** \brief Set the playback speed if the device is an recorded stream.
257           * If setting the device playback speed fails, because the device is no recorded stream or
258           * any other reason this function returns false. Otherwise true is returned.
259           * \param[in] speed The playback speed factor 1.0 means the same speed as recorded,
260           * 0.5 half the speed, 2.0 double speed and so on.
261           * \return True on success, false otherwise.
262           */
263           bool
264           setPlaybackSpeed (double speed);
265 
266           /************************************************************************************/
267           // Callbacks from openni::VideoStream to grabber. Internal interface
268           void
269           setColorCallback (StreamCallbackFunction color_callback);
270           void
271           setDepthCallback (StreamCallbackFunction depth_callback);
272           void
273           setIRCallback (StreamCallbackFunction ir_callback);
274 
275         protected:
276           void shutdown ();
277 
278           std::shared_ptr<openni::VideoStream>
279           getIRVideoStream () const;
280           std::shared_ptr<openni::VideoStream>
281           getColorVideoStream () const;
282           std::shared_ptr<openni::VideoStream>
283           getDepthVideoStream () const;
284 
285 
286           void
287           processColorFrame (openni::VideoStream& stream);
288           void
289           processDepthFrame (openni::VideoStream& stream);
290           void
291           processIRFrame (openni::VideoStream& stream);
292 
293 
294           bool
295           findCompatibleVideoMode (const std::vector<OpenNI2VideoMode>& supportedModes,
296             const OpenNI2VideoMode& output_mode, OpenNI2VideoMode& mode) const;
297 
298           bool
299           resizingSupported (std::size_t input_width, std::size_t input_height, std::size_t output_width, std::size_t output_height) const;
300 
301           // Members
302 
303           std::shared_ptr<openni::Device> openni_device_;
304           std::shared_ptr<openni::DeviceInfo> device_info_;
305 
306           std::shared_ptr<OpenNI2FrameListener> ir_frame_listener;
307           std::shared_ptr<OpenNI2FrameListener> color_frame_listener;
308           std::shared_ptr<OpenNI2FrameListener> depth_frame_listener;
309 
310           mutable std::shared_ptr<openni::VideoStream> ir_video_stream_;
311           mutable std::shared_ptr<openni::VideoStream> color_video_stream_;
312           mutable std::shared_ptr<openni::VideoStream> depth_video_stream_;
313 
314           mutable std::vector<OpenNI2VideoMode> ir_video_modes_;
315           mutable std::vector<OpenNI2VideoMode> color_video_modes_;
316           mutable std::vector<OpenNI2VideoMode> depth_video_modes_;
317 
318           bool ir_video_started_;
319           bool color_video_started_;
320           bool depth_video_started_;
321 
322           /** \brief distance between the projector and the IR camera in meters*/
323           float baseline_;
324           /** the value for shadow (occluded pixels) */
325           std::uint64_t shadow_value_;
326           /** the value for pixels without a valid disparity measurement */
327           std::uint64_t no_sample_value_;
328       };
329 
330       PCL_EXPORTS std::ostream& operator<< (std::ostream& stream, const OpenNI2Device& device);
331 
332     } // namespace
333   }
334 }
335