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 §ion ); 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 |