Main MRPT website > C++ reference
MRPT logo
CKinect.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef mrpt_CKinect_H
29 #define mrpt_CKinect_H
30 
31 #include <mrpt/hwdrivers/CGenericSensor.h>
32 #include <mrpt/slam/CObservation3DRangeScan.h>
33 #include <mrpt/slam/CObservationIMU.h>
34 #include <mrpt/utils/TEnumType.h>
35 #include <mrpt/gui/CDisplayWindow.h>
36 
37 #include <mrpt/hwdrivers/link_pragmas.h>
38 
39 // MRPT implements a common interface to Kinect disregarding the
40 // actual underlying library. These macros defined in "mrpt/config.h"
41 // let us know which library is actually used:
42 // - MRPT_HAS_KINECT_CL_NUI = 0 or 1
43 // - MRPT_HAS_KINECT_FREENECT = 0 or 1
44 
45 // Depth of Kinect ranges:
46 #if MRPT_HAS_KINECT_FREENECT
47 # define MRPT_KINECT_DEPTH_10BIT
48 # define KINECT_RANGES_TABLE_LEN 1024
49 # define KINECT_RANGES_TABLE_MASK 0x03FF
50 #else // MRPT_HAS_KINECT_CL_NUI or none:
51 # define MRPT_KINECT_DEPTH_11BIT
52 # define KINECT_RANGES_TABLE_LEN 2048
53 # define KINECT_RANGES_TABLE_MASK 0x07FF
54 #endif
55 
56 
57 namespace mrpt
58 {
59  namespace hwdrivers
60  {
61  /** A class for grabing "range images", intensity images (either RGB or IR) and other information from an Xbox Kinect sensor.
62  *
63  * <h2>Configuration and usage:</h2> <hr>
64  * Data is returned as observations of type mrpt::slam::CObservation3DRangeScan (and mrpt::slam::CObservationIMU for accelerometers data).
65  * See those classes for documentation on their fields.
66  *
67  * As with any other CGenericSensor class, the normal sequence of methods to be called is:
68  * - CGenericSensor::loadConfig() - Or calls to the individual setXXX() to configure the sensor parameters.
69  * - CKinect::initialize() - to start the communication with the sensor.
70  * - call CKinect::getNextObservation() for getting the data.
71  *
72  * <h2>Calibration parameters</h2><hr>
73  * For an accurate transformation of depth images to 3D points, you'll have to calibrate your Kinect, and supply
74  * the following <b>threee pieces of information</b> (default calibration data will be used otherwise, but they'll be not optimal for all sensors!):
75  * - Camera parameters for the RGB camera. See CKinect::setCameraParamsIntensity()
76  * - Camera parameters for the depth camera. See CKinect::setCameraParamsDepth()
77  * - The 3D relative pose of the two cameras. See CKinect::setRelativePoseIntensityWrtDepth()
78  *
79  * See http://www.mrpt.org/Kinect_calibration for a procedure to calibrate Kinect sensors with an interactive GUI program.
80  *
81  * <h2>Coordinates convention</h2><hr>
82  * The origin of coordinates is the focal point of the depth camera, with the axes oriented as in the
83  * diagram shown in mrpt::slam::CObservation3DRangeScan. Notice in that picture that the RGB camera is
84  * assumed to have axes as usual in computer vision, which differ from those for the depth camera.
85  *
86  * The X,Y,Z axes used to report the data from accelerometers coincide with those of the depth camera
87  * (e.g. the camera standing on a table would have an ACC_Z=-9.8m/s2).
88  *
89  * Notice however that, for consistency with stereo cameras, when loading the calibration parameters from
90  * a configuration file, the left-to-right pose increment is expected as if both RGB & IR cameras had
91  * their +Z axes pointing forward, +X to the right, +Y downwards (just like it's the standard in stereo cameras
92  * and in computer vision literature). In other words: the pose stored in this class uses a different
93  * axes convention for the depth camera than in a stereo camera, so when a pose L2R is loaded from a calibration file
94  * it's actually converted like:
95  *
96  * L2R(this class convention) = CPose3D(0,0,0,-90deg,0deg,-90deg) (+) L2R(in the config file)
97  *
98  *
99  * <h2>Some general comments</h2><hr>
100  * - Depth is grabbed in 10bit depth, and a range N it's converted to meters as: range(m) = 0.1236 * tan(N/2842.5 + 1.1863)
101  * - This sensor can be also used from within rawlog-grabber to grab datasets within a robot with more sensors.
102  * - There is no built-in threading support, so if you use this class manually (not with-in rawlog-grabber),
103  * the ideal would be to create a thread and continuously request data from that thread (see mrpt::system::createThread ).
104  * - The intensity channel default to the RGB images, but it can be changed with setVideoChannel() to read the IR camera images (useful for calibrating).
105  * - There is a built-in support for an optional preview of the data on a window, so you don't need to even worry on creating a window to show them.
106  * - This class relies on an embedded version of libfreenect (you do NOT need to install it in your system). Thanks guys for the great job!
107  *
108  * <h2>Converting to 3D point cloud </h2><hr>
109  * You can convert the 3D observation into a 3D point cloud with this piece of code:
110  *
111  * \code
112  * mrpt::slam::CObservation3DRangeScan obs3D;
113  * mrpt::slam::CColouredPointsMap pntsMap;
114  * pntsMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage;
115  * pntsMap.loadFromRangeScan(obs3D);
116  * \endcode
117  *
118  * Then the point cloud mrpt::slam::CColouredPointsMap can be converted into an OpenGL object for
119  * rendering with mrpt::slam::CMetricMap::getAs3DObject() or alternatively with:
120  *
121  * \code
122  * mrpt::opengl::CPointCloudColouredPtr gl_points = mrpt::opengl::CPointCloudColoured::Create();
123  * gl_points->loadFromPointsMap(&pntsMap);
124  * \endcode
125  *
126  *
127  * <h2>Raw depth to range conversion</h2><hr>
128  * At construction, this class builds an internal array for converting raw 10 or 11bit depths into ranges in meters.
129  * Users can read that array or modify it (if you have a better calibration, for example) by calling CKinect::getRawDepth2RangeConversion().
130  * If you replace it, remember to set the first and last entries (index 0 and KINECT_RANGES_TABLE_LEN-1) to zero, to indicate that those are invalid ranges.
131  *
132  * <table width="100%" >
133  * <tr>
134  * <td align="center" >
135  * <img src="kinect_depth2range_10bit.png" > <br>
136  * R(d) = k3 * tan(d/k2 + k1); <br>
137  * k1 = 1.1863, k2 = 2842.5, k3 = 0.1236 <br>
138  * </td>
139  * <td align="center" >
140  * </td>
141  * </tr>
142  * </table>
143  *
144  *
145  * <h2>Platform-specific comments</h2><hr>
146  * For more details, refer to <a href="http://openkinect.org/wiki/Main_Page" >libfreenect</a> documentation:
147  * - Linux: You'll need root privileges to access Kinect. Or, install <code> MRPT/scripts/51-kinect.rules </code> in <code>/etc/udev/rules.d/</code> to allow access to all users.
148  * - Windows:
149  * - Since MRPT 0.9.4 you'll only need to install <a href="http://sourceforge.net/projects/libusb-win32/files/libusb-win32-releases/" >libusb-win32</a>: download and extract the latest libusb-win32-bin-x.x.x.x.zip
150  * - To install the drivers, read this: http://openkinect.org/wiki/Getting_Started#Windows
151  * - MacOS: (write me!)
152  *
153  *
154  * <h2>Format of parameters for loading from a .ini file</h2><hr>
155  *
156  * \code
157  * PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
158  * -------------------------------------------------------
159  * [supplied_section_name]
160  * sensorLabel = KINECT // A text description
161  * preview_window = false // Show a window with a preview of the grabbed data in real-time
162  *
163  * device_number = 0 // Device index to open (0:first Kinect, 1:second Kinect,...)
164  *
165  * grab_image = true // Grab the RGB image channel? (Default=true)
166  * grab_depth = true // Grab the depth channel? (Default=true)
167  * grab_3D_points = true // Grab the 3D point cloud? (Default=true) If disabled, points can be generated later on.
168  * grab_IMU = true // Grab the accelerometers? (Default=true)
169  *
170  * video_channel = VIDEO_CHANNEL_RGB // Optional. Can be: VIDEO_CHANNEL_RGB (default) or VIDEO_CHANNEL_IR
171  *
172  * pose_x=0 // Camera position in the robot (meters)
173  * pose_y=0
174  * pose_z=0
175  * pose_yaw=0 // Angles in degrees
176  * pose_pitch=0
177  * pose_roll=0
178  *
179  * // Optional: Set the initial tilt angle of Kinect: upon initialization, the motor is sent a command to
180  * // rotate to this angle (in degrees). Note: You must be aware of the tilt when interpreting the sensor readings.
181  * // If not present or set to "360", no motor command will be sent at start up.
182  * initial_tilt_angle = 0
183  *
184  * // Kinect sensor calibration:
185  * // See http://www.mrpt.org/Kinect_and_MRPT
186  *
187  * // Left/Depth camera
188  * [supplied_section_name_LEFT]
189  * rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore this section (it is not a separate device!)
190  *
191  * resolution = [640 488]
192  * cx = 314.649173
193  * cy = 240.160459
194  * fx = 572.882768
195  * fy = 542.739980
196  * dist = [-4.747169e-03 -4.357976e-03 0.000000e+00 0.000000e+00 0.000000e+00] // The order is: [K1 K2 T1 T2 K3]
197  *
198  * // Right/RGB camera
199  * [supplied_section_name_RIGHT]
200  * rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore this section (it is not a separate device!)
201  *
202  * resolution = [640 480]
203  * cx = 322.515987
204  * cy = 259.055966
205  * fx = 521.179233
206  * fy = 493.033034
207  * dist = [5.858325e-02 3.856792e-02 0.000000e+00 0.000000e+00 0.000000e+00] // The order is: [K1 K2 T1 T2 K3]
208  *
209  * // Relative pose of the right camera wrt to the left camera:
210  * // This assumes that both camera frames are such that +Z points
211  * // forwards, and +X and +Y to the right and downwards.
212  * // For the actual coordinates employed in 3D observations, see figure in mrpt::slam::CObservation3DRangeScan
213  * [supplied_section_name_LEFT2RIGHT_POSE]
214  * rawlog-grabber-ignore = true // Instructs rawlog-grabber to ignore this section (it is not a separate device!)
215  *
216  * pose_quaternion = [0.025575 -0.000609 -0.001462 0.999987 0.002038 0.004335 -0.001693]
217  *
218  * \endcode
219  *
220  * More references to read:
221  * - http://openkinect.org/wiki/Imaging_Information
222  * - http://nicolas.burrus.name/index.php/Research/KinectCalibration
223  * \ingroup mrpt_hwdrivers_grp
224  */
225  class HWDRIVERS_IMPEXP CKinect : public mrpt::hwdrivers::CGenericSensor
226  {
227  DEFINE_GENERIC_SENSOR(CKinect)
228 
229  public:
230  typedef float TDepth2RangeArray[KINECT_RANGES_TABLE_LEN]; //!< A typedef for an array that converts raw depth to ranges in meters.
231 
232  /** RGB or IR video channel identifiers \sa setVideoChannel */
233  enum TVideoChannel {
234  VIDEO_CHANNEL_RGB=0,
235  VIDEO_CHANNEL_IR
236  };
237 
238  CKinect(); //!< Default ctor
239  ~CKinect(); //!< Default ctor
240 
241  /** Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different parameters with the set*() methods.
242  * \exception This method must throw an exception with a descriptive message if some critical error is found.
243  */
244  virtual void initialize();
245 
246  /** To be called at a high rate (>XX Hz), this method populates the internal buffer of received observations.
247  * This method is mainly intended for usage within rawlog-grabber or similar programs.
248  * For an alternative, see getNextObservation()
249  * \exception This method must throw an exception with a descriptive message if some critical error is found.
250  * \sa getNextObservation
251  */
252  virtual void doProcess();
253 
254  /** The main data retrieving function, to be called after calling loadConfig() and initialize().
255  * \param out_obs The output retrieved observation (only if there_is_obs=true).
256  * \param there_is_obs If set to false, there was no new observation.
257  * \param hardware_error True on hardware/comms error.
258  *
259  * \sa doProcess
260  */
261  void getNextObservation(
262  mrpt::slam::CObservation3DRangeScan &out_obs,
263  bool &there_is_obs,
264  bool &hardware_error );
265 
266  /** \overload
267  * \note This method also grabs data from the accelerometers, returning them in out_obs_imu
268  */
269  void getNextObservation(
270  mrpt::slam::CObservation3DRangeScan &out_obs,
271  mrpt::slam::CObservationIMU &out_obs_imu,
272  bool &there_is_obs,
273  bool &hardware_error );
274 
275  /** Set the path where to save off-rawlog image files (this class DOES take into account this path).
276  * An empty string (the default value at construction) means to save images embedded in the rawlog, instead of on separate files.
277  * \exception std::exception If the directory doesn't exists and cannot be created.
278  */
279  virtual void setPathForExternalImages( const std::string &directory );
280 
281 
282  /** @name Sensor parameters (alternative to \a loadConfig ) and manual control
283  @{ */
284 
285  /** Try to open the camera (set all the parameters before calling this) - users may also call initialize(), which in turn calls this method.
286  * Raises an exception upon error.
287  * \exception std::exception A textual description of the error.
288  */
289  void open();
290 
291  bool isOpen() const; //!< Whether there is a working connection to the sensor
292 
293  /** Close the conection to the sensor (not need to call it manually unless desired for some reason,
294  * since it's called at destructor) */
295  void close();
296 
297  /** Changes the video channel to open (RGB or IR) - you can call this method before start grabbing or in the middle of streaming and the video source will change on the fly.
298  Default is RGB channel. */
299  void setVideoChannel(const TVideoChannel vch);
300  /** Return the current video channel (RGB or IR) \sa setVideoChannel */
301  inline TVideoChannel getVideoChannel() const { return m_video_channel; }
302 
303  /** Set the sensor index to open (if there're several sensors attached to the computer); default=0 -> the first one. */
304  inline void setDeviceIndexToOpen(int index) { m_user_device_number=index; }
305  inline int getDeviceIndexToOpen() const { return m_user_device_number; }
306 
307  /** Change tilt angle \note Sensor must be open first. */
308  void setTiltAngleDegrees(double angle);
309  double getTiltAngleDegrees();
310 
311  /** Default: disabled */
312  inline void enablePreviewRGB(bool enable=true) { m_preview_window = enable; }
313  inline void disablePreviewRGB() { m_preview_window = false; }
314  inline bool isPreviewRGBEnabled() const { return m_preview_window; }
315 
316  /** If preview is enabled, show only one image out of N (default: 1=show all) */
317  inline void setPreviewDecimation(size_t decimation_factor ) { m_preview_window_decimation = decimation_factor; }
318  inline size_t getPreviewDecimation() const { return m_preview_window_decimation; }
319 
320  /** Get the maximum range (meters) that can be read in the observation field "rangeImage" */
321  inline double getMaxRange() const { return m_maxRange; }
322 
323  /** Get the row count in the camera images, loaded automatically upon camera open(). */
324  inline size_t getRowCount() const { return m_cameraParamsRGB.nrows; }
325  /** Get the col count in the camera images, loaded automatically upon camera open(). */
326  inline size_t getColCount() const { return m_cameraParamsRGB.ncols; }
327 
328  /** Get a const reference to the depth camera calibration parameters */
329  inline const mrpt::utils::TCamera & getCameraParamsIntensity() const { return m_cameraParamsRGB; }
330  inline void setCameraParamsIntensity(const mrpt::utils::TCamera &p) { m_cameraParamsRGB=p; }
331 
332  /** Get a const reference to the depth camera calibration parameters */
333  inline const mrpt::utils::TCamera & getCameraParamsDepth() const { return m_cameraParamsDepth; }
334  inline void setCameraParamsDepth(const mrpt::utils::TCamera &p) { m_cameraParamsDepth=p; }
335 
336  /** Set the pose of the intensity camera wrt the depth camera \sa See mrpt::slam::CObservation3DRangeScan for a 3D diagram of this pose */
337  inline void setRelativePoseIntensityWrtDepth(const mrpt::poses::CPose3D &p) { m_relativePoseIntensityWRTDepth=p; }
338  inline const mrpt::poses::CPose3D &getRelativePoseIntensityWrtDepth() const { return m_relativePoseIntensityWRTDepth; }
339 
340  /** Get a reference to the array that convert raw depth values (10 or 11 bit) into ranges in meters, so it can be read or replaced by the user.
341  * If you replace it, remember to set the first and last entries (index 0 and KINECT_RANGES_TABLE_LEN-1) to zero, to indicate that those are invalid ranges.
342  */
343  inline TDepth2RangeArray & getRawDepth2RangeConversion() { return m_range2meters; }
344  inline const TDepth2RangeArray & getRawDepth2RangeConversion() const { return m_range2meters; }
345 
346  /** Enable/disable the grabbing of the RGB channel */
347  inline void enableGrabRGB(bool enable=true) { m_grab_image=enable; }
348  inline bool isGrabRGBEnabled() const { return m_grab_image; }
349 
350  /** Enable/disable the grabbing of the depth channel */
351  inline void enableGrabDepth(bool enable=true) { m_grab_depth=enable; }
352  inline bool isGrabDepthEnabled() const { return m_grab_depth; }
353 
354  /** Enable/disable the grabbing of the inertial data */
355  inline void enableGrabAccelerometers(bool enable=true) { m_grab_IMU=enable; }
356  inline bool isGrabAccelerometersEnabled() const { return m_grab_IMU; }
357 
358  /** Enable/disable the grabbing of the 3D point clouds */
359  inline void enableGrab3DPoints(bool enable=true) { m_grab_3D_points=enable; }
360  inline bool isGrab3DPointsEnabled() const { return m_grab_3D_points; }
361 
362  /** @} */
363 
364 
365 #if MRPT_HAS_KINECT_FREENECT
366  // Auxiliary getters/setters (we can't declare the libfreenect callback as friend since we
367  // want to avoid including the API headers here).
368  inline mrpt::slam::CObservation3DRangeScan & internal_latest_obs() { return m_latest_obs; }
369  inline volatile uint32_t & internal_tim_latest_depth() { return m_tim_latest_depth; }
370  inline volatile uint32_t & internal_tim_latest_rgb() { return m_tim_latest_rgb; }
371  inline mrpt::synch::CCriticalSection & internal_latest_obs_cs() { return m_latest_obs_cs; }
372 #endif
373 
374  protected:
375  /** Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes)
376  * \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
377  */
378  virtual void loadConfig_sensorSpecific(
379  const mrpt::utils::CConfigFileBase &configSource,
380  const std::string &section );
381 
382  mrpt::poses::CPose3D m_sensorPoseOnRobot;
383 
384  bool m_preview_window; //!< Show preview window while grabbing
385  size_t m_preview_window_decimation; //!< If preview is enabled, only show 1 out of N images.
386  size_t m_preview_decim_counter_range, m_preview_decim_counter_rgb;
387  mrpt::gui::CDisplayWindowPtr m_win_range, m_win_int;
388 
389 #if MRPT_HAS_KINECT_FREENECT
390  void *m_f_ctx; //!< The "freenect_context", or NULL if closed
391  void *m_f_dev; //!< The "freenect_device", or NULL if closed
392 
393  // Data fields for use with the callback function:
394  mrpt::slam::CObservation3DRangeScan m_latest_obs;
395  volatile uint32_t m_tim_latest_depth, m_tim_latest_rgb; // 0 = not updated
396  mrpt::synch::CCriticalSection m_latest_obs_cs;
397 #endif
398 
399 #if MRPT_HAS_KINECT_CL_NUI
400  void *m_clnui_cam; //!< The "CLNUICamera" or NULL if closed
401  void *m_clnui_motor; //!< The "CLNUIMotor" or NULL if closed
402 #endif
403 
404  mrpt::utils::TCamera m_cameraParamsRGB; //!< Params for the RGB camera
405  mrpt::utils::TCamera m_cameraParamsDepth; //!< Params for the Depth camera
406  mrpt::poses::CPose3D m_relativePoseIntensityWRTDepth; //!< See mrpt::slam::CObservation3DRangeScan for a diagram of this pose
407 
408  int m_initial_tilt_angle; //!< Set Kinect tilt to an initial deegre (it should be take in account in the sensor pose by the user)
409 
410  double m_maxRange; //!< Sensor max range (meters)
411 
412  int m_user_device_number; //!< Number of device to open (0:first,...)
413 
414  bool m_grab_image, m_grab_depth, m_grab_3D_points, m_grab_IMU ; //!< Default: all true
415 
416  TVideoChannel m_video_channel; //!< The video channel to open: RGB or IR
417 
418  private:
419  std::vector<uint8_t> m_buf_depth, m_buf_rgb; //!< Temporary buffers for image grabbing.
420  TDepth2RangeArray m_range2meters; //!< The table raw depth -> range in meters
421 
422  void calculate_range2meters(); //!< Compute m_range2meters at construction
423 
424  public:
425  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
426 
427  }; // End of class
428  } // End of NS
429 
430  // Specializations MUST occur at the same namespace:
431  namespace utils
432  {
433  template <>
434  struct TEnumTypeFiller<hwdrivers::CKinect::TVideoChannel>
435  {
436  typedef hwdrivers::CKinect::TVideoChannel enum_t;
437  static void fill(bimap<enum_t,std::string> &m_map)
438  {
439  m_map.insert(hwdrivers::CKinect::VIDEO_CHANNEL_RGB, "VIDEO_CHANNEL_RGB");
440  m_map.insert(hwdrivers::CKinect::VIDEO_CHANNEL_IR, "VIDEO_CHANNEL_IR");
441  }
442  };
443  } // End of namespace
444 
445 } // End of NS
446 
447 
448 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013