Main MRPT website > C++ reference
MRPT logo

CObservation3DRangeScan.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 CObservation3DRangeScan_H
00029 #define CObservation3DRangeScan_H
00030 
00031 #include <mrpt/utils/CSerializable.h>
00032 #include <mrpt/utils/CImage.h>
00033 #include <mrpt/slam/CObservation.h>
00034 #include <mrpt/poses/CPose3D.h>
00035 #include <mrpt/poses/CPose2D.h>
00036 
00037 #include <mrpt/math/CPolygon.h>
00038 
00039 
00040 namespace mrpt
00041 {
00042 namespace slam
00043 {
00044 
00045         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CObservation3DRangeScan, CObservation,OBS_IMPEXP )
00046 
00047         /** Declares a class derived from "CObservation" that
00048          *      encapsules a 3D range scan measurement (e.g. from a time of flight range camera).
00049          *  This kind of observations can carry one or more of these data fields:
00050          *    - 3D point cloud (as float's instead of double's to save storage space - precision is not a problem in this case).
00051          *    - 2D range image (as a matrix): Each entry in the matrix "rangeImage(x,y)" contains the distance of the pixel (x,y), in meters.
00052          *    - 2D intensity image (as a CImage): A logarithmic A-law compression is used to convert the original 16bit intensity to a more standard 8bit graylevel.
00053          *    - 2D confidence image (as a CImage): For each pixel, a 0x00 and a 0xFF mean the lowest and highest confidence levels, respectively.
00054          *
00055          *  The coordinates of the 3D point cloud are in meters with respect to the depth camera origin of coordinates
00056          *    (in SwissRanger, the front face of the camera: a small offset ~1cm in front of the physical focal point),
00057          *    with the +X axis pointing forward, +Y pointing left-hand and +Z pointing up.
00058          *  The field CObservation3DRangeScan::relativePoseIntensityWRTDepth describes the change of coordinates from
00059          *    the depth camera to the intensity (RGB or grayscale) camera. In a SwissRanger camera both cameras coincide,
00060          *    so this pose is just a rotation (0,0,0,-90deg,0,-90deg). But in
00061          *    Microsoft Kinect there is also an offset, as shown in this figure:
00062          *
00063          *  <div align=center>
00064          *   <img src="CObservation3DRangeScan_figRefSystem.png">
00065          *  </div>
00066          *
00067          *  The 2D images and matrices are stored as common images, with an up->down rows order and left->right, as usual.
00068          *   Optionally, the intensity and confidence channels can be set to delayed-load images for off-rawlog storage so it saves
00069          *   memory by having loaded in memory just the needed images. See the methods load() and unload().
00070          *  Due to the intensive storage requirements of this kind of observations, this observation is the only one in MRPT
00071          *   for which it's recommended to always call "load()" and "unload()" before and after using the observation, *ONLY* when
00072          *   the observation was read from a rawlog dataset, in order to make sure that all the externally stored data fields are
00073          *   loaded and ready in memory.
00074          *
00075          *  Classes that grab observations of this type are:
00076          *              - mrpt::hwdrivers::CSwissRanger3DCamera
00077          *              - mrpt::hwdrivers::CKinect
00078          *
00079          *  There are two sets of calibration parameters (in some cameras, like SwissRanger, both are the same):
00080          *              - cameraParams: Projection parameters of the depth camera.
00081          *              - cameraParamsIntensity: Projection parameters of the intensity (gray-level or RGB) camera.
00082          *
00083          *  3D point clouds can be generated at any moment after grabbing with CObservation3DRangeScan::project3DPointsFromDepthImage()
00084          *
00085          *  \note Starting at serialization version 2 (MRPT 0.9.1+), the confidence channel is stored as an image instead of a matrix to optimize memory and disk space.
00086          *  \note Starting at serialization version 3 (MRPT 0.9.1+), the 3D point cloud and the rangeImage can both be stored externally to save rawlog space.
00087          *
00088          * \sa mrpt::hwdrivers::CSwissRanger3DCamera, CObservation
00089          */
00090         class OBS_IMPEXP CObservation3DRangeScan : public CObservation
00091         {
00092                 // This must be added to any CSerializable derived class:
00093                 DEFINE_SERIALIZABLE( CObservation3DRangeScan )
00094 
00095         protected:
00096                 bool                    m_points3D_external_stored; //!< If set to true, m_points3D_external_file is valid.
00097                 std::string             m_points3D_external_file;   //!< 3D points are in CImage::IMAGES_PATH_BASE+<this_file_name>
00098 
00099                 bool                    m_rangeImage_external_stored; //!< If set to true, m_rangeImage_external_file is valid.
00100                 std::string             m_rangeImage_external_file;   //!< rangeImage is in CImage::IMAGES_PATH_BASE+<this_file_name>
00101 
00102         public:
00103                 CObservation3DRangeScan( );                             //!< Default constructor
00104                 virtual ~CObservation3DRangeScan( );    //!< Destructor
00105 
00106                 /** @name Delayed-load manual control methods.
00107                     @{ */
00108                 /** Makes sure all images and other fields which may be externally stored are loaded in memory.
00109                   *  Note that for all CImages, calling load() is not required since the images will be automatically loaded upon first access, so load() shouldn't be needed to be called in normal cases by the user.
00110                   *  If all the data were alredy loaded or this object has no externally stored data fields, calling this method has no effects.
00111                   * \sa unload
00112                   */
00113                 virtual void load() const;
00114                 /** Unload all images, for the case they being delayed-load images stored in external files (othewise, has no effect).
00115                   * \sa load
00116                   */
00117                 virtual void unload();
00118                 /** @} */
00119 
00120                 /** Compute the 3D points coordinates from the depth image (\a rangeImage) and the depth camera camera parameters (\a cameraParams).
00121                   *  The formulas for the i'th point, with rangeImage pixel coordinates (r,c) are:
00122                   * \code
00123                   *   x(i) = rangeImage(r,c)
00124                   *   y(i) = (r_cx - c) * x(i) / r_fy
00125                   *   z(i) = (r_cy - r) * x(i) / r_fx
00126                   * \endcode
00127                   */
00128                 void project3DPointsFromDepthImage();
00129 
00130                 bool hasPoints3D;                                                               //!< true means the field points3D contains valid data.
00131                 std::vector<float> points3D_x;   //!< If hasPoints3D=true, the X coordinates of the 3D point cloud detected by the camera.
00132                 std::vector<float> points3D_y;   //!< If hasPoints3D=true, the Y coordinates of the 3D point cloud detected by the camera.
00133                 std::vector<float> points3D_z;   //!< If hasPoints3D=true, the Z coordinates of the 3D point cloud detected by the camera.
00134 
00135                 // 3D points external storage functions ---------
00136                 inline bool points3D_isExternallyStored() const { return m_points3D_external_stored; }
00137                 inline std::string points3D_getExternalStorageFile() const { return m_points3D_external_file; }
00138                 void points3D_getExternalStorageFileAbsolutePath(std::string &out_path) const;
00139                 inline std::string points3D_getExternalStorageFileAbsolutePath() const {
00140                                 std::string tmp;
00141                                 points3D_getExternalStorageFileAbsolutePath(tmp);
00142                                 return tmp;
00143                 }
00144                 void points3D_convertToExternalStorage( const std::string &fileName, const std::string &use_this_base_dir ); //!< Users won't normally want to call this, it's only used from internal MRPT programs.
00145                 // ---------
00146 
00147                 bool hasRangeImage;                             //!< true means the field rangeImage contains valid data
00148                 mrpt::math::CMatrix rangeImage;         //!< If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters).
00149 
00150                 // Range Matrix external storage functions ---------
00151                 inline bool rangeImage_isExternallyStored() const { return m_rangeImage_external_stored; }
00152                 inline std::string rangeImage_getExternalStorageFile() const { return m_rangeImage_external_file; }
00153                 void rangeImage_getExternalStorageFileAbsolutePath(std::string &out_path) const;
00154                 inline std::string rangeImage_getExternalStorageFileAbsolutePath() const {
00155                                 std::string tmp;
00156                                 rangeImage_getExternalStorageFileAbsolutePath(tmp);
00157                                 return tmp;
00158                 }
00159                 void rangeImage_convertToExternalStorage( const std::string &fileName, const std::string &use_this_base_dir ); //!< Users won't normally want to call this, it's only used from internal MRPT programs.
00160                 // ---------
00161 
00162                 bool hasIntensityImage;                         //!< true means the field intensityImage contains valid data
00163                 mrpt::utils::CImage intensityImage;     //!< If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"
00164 
00165                 bool hasConfidenceImage;                        //!< true means the field confidenceImage contains valid data
00166                 mrpt::utils::CImage confidenceImage;  //!< If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the capture drivers.
00167 
00168                 mrpt::utils::TCamera    cameraParams;   //!< Projection parameters of the depth camera.
00169                 mrpt::utils::TCamera    cameraParamsIntensity;  //!< Projection parameters of the intensity (graylevel or RGB) camera.
00170 
00171                 /** Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this observation).
00172                   *  In a SwissRanger camera, this will be (0,0,0,0,0,0) since both cameras coincide.
00173                   *  In a Kinect, this will include a small lateral displacement and a rotation, according to the drawing on the top of this page.
00174                   */
00175                 mrpt::poses::CPose3D    relativePoseIntensityWRTDepth;
00176 
00177 
00178                 float   maxRange;       //!< The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
00179                 CPose3D sensorPose;     //!< The 6D pose of the sensor on the robot.
00180                 float   stdError;       //!< The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
00181 
00182 
00183                 /** A general method to retrieve the sensor pose on the robot.
00184                   *  Note that most sensors will return a full (6D) CPose3D, but see the derived classes for more details or special cases.
00185                   * \sa setSensorPose
00186                   */
00187                 void getSensorPose( CPose3D &out_sensorPose ) const { out_sensorPose = sensorPose; }
00188 
00189                 /** A general method to change the sensor pose on the robot.
00190                   *  Note that most sensors will use the full (6D) CPose3D, but see the derived classes for more details or special cases.
00191                   * \sa getSensorPose
00192                   */
00193                 void setSensorPose( const CPose3D &newSensorPose ) { sensorPose = newSensorPose; }
00194 
00195                 void swap(CObservation3DRangeScan &o);  //!< Very efficient method to swap the contents of two observations.
00196 
00197                 void getZoneAsObs( CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2 );
00198 
00199                 /** A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a range (depth) image and the corresponding 3D point cloud.
00200                   * \param camera_offset The offset (in meters) in the +X direction of the point cloud. It's 1cm for SwissRanger SR4000.
00201                   * \return The final average reprojection error per pixel (typ <0.05 px)
00202                   */
00203                 static double recoverCameraCalibrationParameters(
00204                         const CObservation3DRangeScan   &in_obs,
00205                         mrpt::utils::TCamera                    &out_camParams,
00206                         const double camera_offset = 0.01 );
00207 
00208 
00209         }; // End of class def.
00210 
00211 
00212         } // End of namespace
00213 
00214         namespace utils
00215         {
00216                 using namespace ::mrpt::slam;
00217                 // Specialization must occur in the same namespace
00218                 MRPT_DECLARE_TTYPENAME_PTR(CObservation3DRangeScan)
00219         }
00220 
00221 } // End of namespace
00222 
00223 #endif



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