Main MRPT website > C++ reference
MRPT logo

CKinect.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef mrpt_CKinect_H
00029 #define mrpt_CKinect_H
00030 
00031 #include <mrpt/hwdrivers/CGenericSensor.h>
00032 #include <mrpt/slam/CObservation3DRangeScan.h>
00033 #include <mrpt/slam/CObservationIMU.h>
00034 
00035 #include <mrpt/gui/CDisplayWindow.h>
00036 
00037 #include <mrpt/hwdrivers/link_pragmas.h>
00038 
00039 // MRPT implements a common interface to Kinect disregarding the
00040 //  actual underlying library. These macros are defined for internal
00041 //  MRPT usage to know which library to use:
00042 #if MRPT_HAS_KINECT
00043 #       if defined(_MSC_VER)
00044 #               define MRPT_KINECT_WITH_CLNUI        1
00045 #               define MRPT_KINECT_WITH_LIBFREENECT  0
00046 #       else
00047 #               define MRPT_KINECT_WITH_CLNUI        0
00048 #               define MRPT_KINECT_WITH_LIBFREENECT  1
00049 #       endif
00050 #else
00051 #               define MRPT_KINECT_WITH_CLNUI        0
00052 #               define MRPT_KINECT_WITH_LIBFREENECT  0
00053 #endif
00054 
00055 // Depth of Kinect ranges:
00056 #if MRPT_KINECT_WITH_LIBFREENECT
00057 #       define MRPT_KINECT_DEPTH_10BIT
00058 #       define KINECT_RANGES_TABLE_LEN    1024
00059 #       define KINECT_RANGES_TABLE_MASK   0x03FF
00060 #else //  MRPT_KINECT_WITH_CLNUI or none:
00061 #       define MRPT_KINECT_DEPTH_11BIT
00062 #       define KINECT_RANGES_TABLE_LEN    2048
00063 #       define KINECT_RANGES_TABLE_MASK   0x07FF
00064 #endif
00065 
00066 
00067 namespace mrpt
00068 {
00069         namespace hwdrivers
00070         {
00071                 /** A class for grabing "range images", intensity images and other information from an Xbox Kinect sensor.
00072                   *
00073                   *  <h2>Configuration and usage:</h2> <hr>
00074                   * Data is returned as observations of type mrpt::slam::CObservation3DRangeScan (and mrpt::slam::CObservationIMU for accelerometers data).
00075                   *  See those classes for documentation on their fields.
00076                   *
00077                   * As with any other CGenericSensor class, the normal sequence of methods to be called is:
00078                   *   - CGenericSensor::loadConfig() - Or calls to the individual setXXX() to configure the sensor parameters.
00079                   *   - CKinect::initialize() - to start the communication with the sensor.
00080                   *   - call CKinect::getNextObservation() for getting the data.
00081                   *
00082                   * <h2>Calibration parameters</h2><hr>
00083                   *  For an accurate transformation of depth images to 3D points, you'll have to calibrate your Kinect, and supply
00084                   *   the following <b>threee pieces of information</b> (default calibration data will be used otherwise, but they'll be not optimal for all sensors!):
00085                   *    - Camera parameters for the RGB camera. See CKinect::setCameraParamsIntensity()
00086                   *    - Camera parameters for the depth camera. See CKinect::setCameraParamsDepth()
00087                   *    - The 3D relative pose of the two cameras. See CKinect::setRelativePoseIntensityWrtDepth()
00088                   *
00089                   * <h2>Coordinates convention</h2><hr>
00090                   *   The origin of coordinates is the focal point of the depth camera, with the axes oriented as in the
00091                   *   diagram shown in mrpt::slam::CObservation3DRangeScan. Notice in that picture that the RGB camera is
00092                   *   assumed to have axes as usual in computer vision, which differ from those for the depth camera.
00093                   *
00094                   *   The X,Y,Z axes used to report the data from accelerometers coincide with those of the depth camera
00095                   *    (e.g. the camera standing on a table would have an ACC_Z=-9.8m/s2).
00096                   *
00097                   * <h2>Some general comments</h2><hr>
00098                   *             - 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)
00099                   *             - This sensor can be also used from within rawlog-grabber to grab datasets within a robot with more sensors.
00100                   *             - There is no built-in threading support, so if you use this class manually (not with-in rawlog-grabber),
00101                   *                     the ideal would be to create a thread and continuously request data from that thread (see mrpt::system::createThread ).
00102                   *             - 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.
00103                   *             - This class relies on an embedded version of libfreenect (you don't have to install it in your system). Thanks guys for the great job!
00104                   *
00105                   * <h2>Converting to 3D point cloud </h2><hr>
00106                   *   You can convert the 3D observation into a 3D point cloud with this piece of code:
00107                   *
00108                   * \code
00109                   * mrpt::slam::CObservation3DRangeScan  obs3D;
00110                   * mrpt::slam::CColouredPointsMap       pntsMap;
00111                   * pntsMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage;
00112                   * pntsMap.loadFromRangeScan(obs3D);
00113                   * \endcode
00114                   *
00115                   *   Then the point cloud mrpt::slam::CColouredPointsMap can be converted into an OpenGL object for
00116                   *    rendering with mrpt::slam::CMetricMap::getAs3DObject() or alternatively with:
00117                   *
00118                   *  \code
00119                   *    mrpt::opengl::CPointCloudColouredPtr gl_points = mrpt::opengl::CPointCloudColoured::Create();
00120                   *    gl_points->loadFromPointsMap(&pntsMap);
00121                   *  \endcode
00122                   *
00123                   *
00124                   * <h2>Raw depth to range conversion</h2><hr>
00125                   *  At construction, this class builds an internal array for converting raw 10 or 11bit depths into ranges in meters.
00126                   *   Users can read that array or modify it (if you have a better calibration, for example) by calling CKinect::getRawDepth2RangeConversion().
00127                   *   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.
00128                   *
00129                   *  <table width="100%" >
00130                   *  <tr>
00131                   *  <td align="center" >
00132                   *   <img src="kinect_depth2range_10bit.png" > <br>
00133                   *    R(d) = k3 * tan(d/k2 + k1); <br>
00134                   *    k1 = 1.1863,  k2 = 2842.5, k3 = 0.1236 <br>
00135                   *  </td>
00136                   *  <td align="center" >
00137                   *  </td>
00138                   *  </tr>
00139                   *  </table>
00140                   *
00141                   *
00142                   * <h2>Platform-specific comments</h2><hr>
00143                   *   For more details, refer to <a href="http://openkinect.org/wiki/Main_Page" >libfreenect</a> documentation:
00144                   *             - 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.
00145                   *             - Windows: Install CL NUI SDK: http://codelaboratories.com/nui
00146                   *             - MacOS: (write me!)
00147                   *
00148                   *
00149                   * <h2>Format of parameters for loading from a .ini file</h2><hr>
00150                   *
00151                   *  \code
00152                   *  PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
00153                   * -------------------------------------------------------
00154                   *   [supplied_section_name]
00155                   *    sensorLabel     = KINECT       // A text description
00156                   *    preview_window  = false        // Show a window with a preview of the grabbed data in real-time
00157                   *
00158                   *    device_number   = 0           // Device index to open (0:first Kinect, 1:second Kinect,...)
00159                   *
00160                   *    grab_image      = true        // Grab the RGB image channel? (Default=true)
00161                   *    grab_depth      = true        // Grab the depth channel? (Default=true)
00162                   *    grab_3D_points  = true        // Grab the 3D point cloud? (Default=true) If disabled, points can be generated later on.
00163                   *    grab_IMU        = true        // Grab the accelerometers? (Default=true)
00164                   *
00165                   *    // Calibration matrix of the RGB camera:
00166                   *    rgb_cx        = 328.9427     // (cx,cy): Optical center, pixels
00167                   *    rgb_cy        = 267.4807
00168                   *    rgb_fx        = 529.2151     // (fx,fy): Focal distance, pixels
00169                   *    rgb_fy        = 525.5639
00170                   *
00171                   *    // Calibration matrix of the Depth camera:
00172                   *    d_cx          = 339.3078     // (cx,cy): Optical center, pixels
00173                   *    d_cy          = 242.7391
00174                   *    d_fx          = 594.2143     // (fx,fy): Focal distance, pixels
00175                   *    d_fy          = 591.0405
00176                   *
00177                   *    // The relative pose of the RGB camera wrt the depth camera.
00178                   *    //  (See mrpt::slam::CObservation3DRangeScan for a 3D diagram of this pose)
00179                   *    relativePoseIntensityWRTDepth  =  [0 -0.02 0 -90 0 -90]   //  [x(m) y(m) z(m) yaw(deg) pitch(deg) roll(deg)]
00180                   *
00181                   *    pose_x=0 // Camera position in the robot (meters)
00182                   *    pose_y=0
00183                   *    pose_z=0
00184                   *    pose_yaw=0       // Angles in degrees
00185                   *    pose_pitch=0
00186                   *    pose_roll=0
00187                   *
00188                   *  \endcode
00189                   *
00190                   *  More references to read:
00191                   *             - http://openkinect.org/wiki/Imaging_Information
00192                   *             - http://nicolas.burrus.name/index.php/Research/KinectCalibration
00193                   */
00194                 class HWDRIVERS_IMPEXP  CKinect : public mrpt::hwdrivers::CGenericSensor
00195                 {
00196                         DEFINE_GENERIC_SENSOR(CKinect)
00197 
00198                 public:
00199                         typedef float TDepth2RangeArray[KINECT_RANGES_TABLE_LEN]; //!< A typedef for an array that converts raw depth to ranges in meters.
00200 
00201 
00202                         CKinect();       //!< Default ctor
00203                         ~CKinect();      //!< Default ctor
00204 
00205                         /** Initializes the 3D camera - should be invoked after calling loadConfig() or setting the different parameters with the set*() methods.
00206                           *  \exception This method must throw an exception with a descriptive message if some critical error is found.
00207                           */
00208                         virtual void initialize();
00209 
00210                         /** To be called  at a high rate (>XX Hz), this method populates the internal buffer of received observations.
00211                           *  This method is mainly intended for usage within rawlog-grabber or similar programs.
00212                           *  For an alternative, see getNextObservation()
00213                           *  \exception This method must throw an exception with a descriptive message if some critical error is found.
00214                           * \sa getNextObservation
00215                           */
00216                         virtual void doProcess();
00217 
00218                         /** The main data retrieving function, to be called after calling loadConfig() and initialize().
00219                           *  \param out_obs The output retrieved observation (only if there_is_obs=true).
00220                           *  \param there_is_obs If set to false, there was no new observation.
00221                           *  \param hardware_error True on hardware/comms error.
00222                           *
00223                           * \sa doProcess
00224                           */
00225                         void getNextObservation(
00226                                 mrpt::slam::CObservation3DRangeScan &out_obs,
00227                                 bool &there_is_obs,
00228                                 bool &hardware_error );
00229 
00230                         /** \overload
00231                           * \note This method also grabs data from the accelerometers, returning them in out_obs_imu
00232                           */
00233                         void getNextObservation(
00234                                 mrpt::slam::CObservation3DRangeScan &out_obs,
00235                                 mrpt::slam::CObservationIMU         &out_obs_imu,
00236                                 bool &there_is_obs,
00237                                 bool &hardware_error );
00238 
00239                         /**  Set the path where to save off-rawlog image files (this class DOES take into account this path).
00240                           *  An  empty string (the default value at construction) means to save images embedded in the rawlog, instead of on separate files.
00241                           * \exception std::exception If the directory doesn't exists and cannot be created.
00242                           */
00243                         virtual void setPathForExternalImages( const std::string &directory );
00244 
00245 
00246                         /** @name Sensor parameters (alternative to \a loadConfig ) and manual control
00247                             @{ */
00248 
00249                         /** Try to open the camera (set all the parameters first!) - users may also call initialize(), which in turn calls this.
00250                           *  Raises an exception upon error.
00251                           * \exception std::exception A textual description of the error.
00252                           */
00253                         void open();
00254 
00255                         bool isOpen() const; //!< Whether there is a working connection to the sensor
00256 
00257                         /** Close the conection to the sensor (not need to call it manually unless desired for some reason,
00258                           * since it's called at destructor) */
00259                         void close();
00260 
00261                         /** Set the sensor index to open (if there're several sensors attached to the computer); default=0 -> the first one. */
00262                         inline void setDeviceIndexToOpen(int index) { m_user_device_number=index; }
00263                         inline int getDeviceIndexToOpen() const { return m_user_device_number; }
00264 
00265                         /** Change tilt angle \note Sensor must be open first. */
00266                         void setTiltAngleDegrees(double angle);
00267                         double getTiltAngleDegrees();
00268 
00269                         /** Default: disabled */
00270                         inline void enablePreviewRGB(bool enable=true) { m_preview_window = enable; }
00271                         inline void disablePreviewRGB() { m_preview_window = false; }
00272                         inline bool isPreviewRGBEnabled() const { return m_preview_window; }
00273 
00274                         /** If preview is enabled, show only one image out of N (default: 1=show all) */
00275                         inline void setPreviewDecimation(size_t decimation_factor ) { m_preview_window_decimation = decimation_factor; }
00276                         inline size_t getPreviewDecimation() const { return m_preview_window_decimation; }
00277 
00278                         /** Get the maximum range (meters) that can be read in the observation field "rangeImage" */
00279                         inline double getMaxRange() const { return m_maxRange; }
00280 
00281                         /** Get the row count in the camera images, loaded automatically upon camera open(). */
00282                         inline size_t getRowCount() const { return m_cameraParamsRGB.nrows; }
00283                         /** Get the col count in the camera images, loaded automatically upon camera open(). */
00284                         inline size_t getColCount() const { return m_cameraParamsRGB.ncols; }
00285 
00286                         /** Get a const reference to the depth camera calibration parameters */
00287                         inline const mrpt::utils::TCamera  & getCameraParamsIntensity() const { return m_cameraParamsRGB; }
00288                         inline void setCameraParamsIntensity(const mrpt::utils::TCamera  &p) { m_cameraParamsRGB=p; }
00289 
00290                         /** Get a const reference to the depth camera calibration parameters */
00291                         inline const mrpt::utils::TCamera  & getCameraParamsDepth() const { return m_cameraParamsDepth; }
00292                         inline void setCameraParamsDepth(const mrpt::utils::TCamera  &p) { m_cameraParamsDepth=p; }
00293 
00294                         /** Set the pose of the intensity camera wrt the depth camera \sa See mrpt::slam::CObservation3DRangeScan for a 3D diagram of this pose */
00295                         inline void setRelativePoseIntensityWrtDepth(const mrpt::poses::CPose3D &p) { m_relativePoseIntensityWRTDepth=p; }
00296                         inline const mrpt::poses::CPose3D &getRelativePoseIntensityWrtDepth() const { return m_relativePoseIntensityWRTDepth; }
00297 
00298                         /** 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.
00299                           *  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.
00300                           */
00301                         inline       TDepth2RangeArray & getRawDepth2RangeConversion()       { return m_range2meters; }
00302                         inline const TDepth2RangeArray & getRawDepth2RangeConversion() const { return m_range2meters; }
00303 
00304                         /** Enable/disable the grabbing of the RGB channel */
00305                         inline void enableGrabRGB(bool enable=true) { m_grab_image=enable; }
00306                         inline bool isGrabRGBEnabled() const { return m_grab_image; }
00307 
00308                         /** Enable/disable the grabbing of the depth channel */
00309                         inline void enableGrabDepth(bool enable=true) { m_grab_depth=enable; }
00310                         inline bool isGrabDepthEnabled() const { return m_grab_depth; }
00311 
00312                         /** Enable/disable the grabbing of the inertial data */
00313                         inline void enableGrabAccelerometers(bool enable=true) { m_grab_IMU=enable; }
00314                         inline bool isGrabAccelerometersEnabled() const { return m_grab_IMU; }
00315 
00316                         /** Enable/disable the grabbing of the 3D point clouds */
00317                         inline void enableGrab3DPoints(bool enable=true) { m_grab_3D_points=enable; }
00318                         inline bool isGrab3DPointsEnabled() const { return m_grab_3D_points; }
00319 
00320                         /** @} */
00321 
00322 
00323 #if MRPT_KINECT_WITH_LIBFREENECT
00324                         // Auxiliary getters/setters (we can't declare the libfreenect callback as friend since we
00325                         //   want to avoid including the API headers here).
00326                         inline CObservation3DRangeScan & internal_latest_obs() { return m_latest_obs; }
00327                         inline volatile uint32_t & internal_tim_latest_depth() { return m_tim_latest_depth; }
00328                         inline volatile uint32_t & internal_tim_latest_rgb()   { return m_tim_latest_rgb; }
00329 #endif
00330 
00331                 protected:
00332                         /** 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)
00333                           *  \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
00334                           */
00335                         virtual void  loadConfig_sensorSpecific(
00336                                 const mrpt::utils::CConfigFileBase &configSource,
00337                                 const std::string                       &section );
00338 
00339                         mrpt::poses::CPose3D    m_sensorPoseOnRobot;
00340 
00341                         bool            m_preview_window; //!< Show preview window while grabbing
00342                         size_t          m_preview_window_decimation; //!< If preview is enabled, only show 1 out of N images.
00343                         size_t      m_preview_decim_counter_range, m_preview_decim_counter_rgb;
00344                         mrpt::gui::CDisplayWindowPtr  m_win_range, m_win_int;
00345 
00346 #if MRPT_KINECT_WITH_LIBFREENECT
00347                         void *m_f_ctx;  //!< The "freenect_context", or NULL if closed
00348                         void *m_f_dev;  //!< The "freenect_device", or NULL if closed
00349 
00350                         // Data fields for use with the callback function:
00351                         CObservation3DRangeScan  m_latest_obs;
00352                         volatile uint32_t                 m_tim_latest_depth, m_tim_latest_rgb; // 0 = not updated
00353 #endif
00354 
00355 #if MRPT_KINECT_WITH_CLNUI
00356                         void *m_clnui_cam;   //!< The "CLNUICamera" or NULL if closed
00357                         void *m_clnui_motor; //!< The "CLNUIMotor" or NULL if closed
00358 #endif
00359 
00360                         mrpt::utils::TCamera    m_cameraParamsRGB;  //!< Params for the RGB camera
00361                         mrpt::utils::TCamera    m_cameraParamsDepth;  //!< Params for the Depth camera
00362                         mrpt::poses::CPose3D    m_relativePoseIntensityWRTDepth; //!< See mrpt::slam::CObservation3DRangeScan for a diagram of this pose
00363 
00364                         double  m_maxRange; //!< Sensor max range (meters)
00365 
00366                         int  m_user_device_number; //!< Number of device to open (0:first,...)
00367 
00368                         bool  m_grab_image, m_grab_depth, m_grab_3D_points, m_grab_IMU ; //!< Default: all true
00369 
00370                 private:
00371                         std::vector<uint8_t> m_buf_depth, m_buf_rgb; //!< Temporary buffers for image grabbing.
00372                         TDepth2RangeArray   m_range2meters; //!< The table raw depth -> range in meters
00373 
00374                         void calculate_range2meters(); //!< Compute m_range2meters at construction
00375 
00376                 public:
00377                         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00378 
00379                 };      // End of class
00380 
00381         } // End of NS
00382 } // End of NS
00383 
00384 
00385 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011