Main MRPT website > C++ reference
MRPT logo

CMetricMapBuilderICP.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 CMetricMapBuilderICP_H
00029 #define CMetricMapBuilderICP_H
00030 
00031 #include <mrpt/slam/CMetricMapBuilder.h>
00032 #include <mrpt/slam/CICP.h>
00033 #include <mrpt/poses/CRobot2DPoseEstimator.h>
00034 
00035 #include <mrpt/slam/link_pragmas.h>
00036 
00037 
00038 namespace mrpt
00039 {
00040 namespace slam
00041 {
00042         /** A class for very simple 2D SLAM based on ICP. This is a non-probabilistic pose tracking algorithm.
00043          *   Map are stored as in files as binary dumps of "mrpt::slam::CSimpleMap" objects. The methods are
00044          *       thread-safe.
00045          */
00046         class SLAM_IMPEXP  CMetricMapBuilderICP : public CMetricMapBuilder
00047         {
00048          public:
00049                  /** Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize".
00050                    */
00051                  CMetricMapBuilderICP();
00052 
00053                  /** *DEPRECATED* Constructor with arguments - use the default constructor, then set the parameters in the "ICP_options" member.
00054                    * \param mapInitializers What maps to create (at least one points map and/or a grid map are needed).
00055                    * \param insertionLinDistance Minimum robot linear displacement for a new observation to be inserted in the map.
00056                    * \param insertionAngDistance Minimum robot angular displacement for a new observation to be inserted in the map.
00057                    */
00058                  CMetricMapBuilderICP(
00059                         TSetOfMetricMapInitializers     *mapInitializers,
00060                         float                                           insertionLinDistance = 1.0f,
00061                         float                                           insertionAngDistance = DEG2RAD(30),
00062                         CICP::TConfigParams                     *icpParams = NULL
00063                         );
00064 
00065                  /** Destructor:
00066                    */
00067                  virtual ~CMetricMapBuilderICP();
00068 
00069                  /** Algorithm configuration params
00070                    */
00071                  struct SLAM_IMPEXP TConfigParams : public mrpt::utils::CLoadableOptions
00072                  {
00073                          /** Initializer */
00074                          TConfigParams ();
00075                          virtual void  loadFromConfigFile(
00076                                 const mrpt::utils::CConfigFileBase      &source,
00077                                 const std::string               &section);
00078                          virtual void  dumpToTextStream( CStream                &out) const;
00079 
00080                         /** (default:false) Match against the occupancy grid or the points map? The former is quicker but less precise. */
00081                         bool    matchAgainstTheGrid;
00082 
00083                         double insertionLinDistance;    //!< Minimum robot linear (m) displacement for a new observation to be inserted in the map.
00084                         double insertionAngDistance;    //!< Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be inserted in the map.
00085                         double localizationLinDistance; //!< Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (otherwise, dead-reckon with odometry).
00086                         double localizationAngDistance;//!< Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be used to do ICP-based localization (otherwise, dead-reckon with odometry).
00087 
00088                         double minICPgoodnessToAccept;  //!< Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0.40)
00089 
00090                         /** What maps to create (at least one points map and/or a grid map are needed).
00091                           *  For the expected format in the .ini file when loaded with loadFromConfigFile(), see documentation of TSetOfMetricMapInitializers.
00092                           */
00093                         TSetOfMetricMapInitializers     mapInitializers;
00094 
00095                  };
00096 
00097                  TConfigParams                  ICP_options; //!< Options for the ICP-SLAM application \sa ICP_params
00098                  CICP::TConfigParams    ICP_params;  //!< Options for the ICP algorithm itself \sa ICP_options
00099 
00100                 /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map.
00101                   *  This method MUST be called if using the default constructor, after loading the configuration into ICP_options. In particular, TConfigParams::mapInitializers
00102                   */
00103                 void  initialize(
00104                         const CSimpleMap &initialMap  = CSimpleMap(),
00105                         CPosePDF                                        *x0 = NULL
00106                         );
00107 
00108                 /** Returns a copy of the current best pose estimation as a pose PDF.
00109                   */
00110                 CPose3DPDFPtr  getCurrentPoseEstimation() const;
00111 
00112                  /** Sets the "current map file", thus that map will be loaded if it exists or a new one will be created if it does not, and the updated map will be save to that file when destroying the object.
00113                    */
00114                  void  setCurrentMapFile( const char *mapFile );
00115 
00116                 /** Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description.
00117                  *  \param action The estimation of the incremental pose change in the robot pose.
00118                  *      \param in_SF The set of observations that robot senses at the new pose.
00119                  * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
00120                  * \sa processObservation
00121                  */
00122                 void  processActionObservation(
00123                                         CActionCollection       &action,
00124                                         CSensoryFrame           &in_SF );
00125 
00126                 /**  The main method of this class: Process one odometry or sensor observation.
00127                     The new entry point of the algorithm (the old one  was processActionObservation, which now is a wrapper to
00128                   this method).
00129                  * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
00130                   */
00131                 void  processObservation(const CObservationPtr &obs);
00132 
00133 
00134                 /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
00135                   */
00136                 void  getCurrentlyBuiltMap(CSimpleMap &out_map) const;
00137 
00138 
00139                  /** Returns the 2D points of current local map
00140                    */
00141                 void  getCurrentMapPoints( std::vector<float> &x, std::vector<float> &y);
00142 
00143                 /** Returns the map built so far. NOTE that for efficiency a pointer to the internal object is passed, DO NOT delete nor modify the object in any way, if desired, make a copy of ir with "duplicate()".
00144                   */
00145                 CMultiMetricMap*   getCurrentlyBuiltMetricMap();
00146 
00147                 /** Returns just how many sensory-frames are stored in the currently build map.
00148                   */
00149                 unsigned int  getCurrentlyBuiltMapSize();
00150 
00151                 /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
00152                   * \param file The output file name
00153                   * \param formatEMF_BMP Output format = true:EMF, false:BMP
00154                   */
00155                 void  saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true);
00156 
00157          private:
00158                  /** The set of observations that leads to current map:
00159                    */
00160                  CSimpleMap             SF_Poses_seq;
00161 
00162                  /** The metric map representation as a points map:
00163                    */
00164                  CMultiMetricMap                        metricMap;
00165 
00166                  /** Current map file.
00167                    */
00168                  std::string                            currentMapFile;
00169 
00170                  /** The pose estimation by the alignment algorithm (ICP). */
00171                  mrpt::poses::CRobot2DPoseEstimator             m_lastPoseEst;  //!< Last pose estimation (Mean)
00172                  mrpt::math::CMatrixDouble33                    m_lastPoseEst_cov; //!< Last pose estimation (covariance)
00173 
00174                  /** The estimated robot path:
00175                    */
00176                  std::deque<mrpt::math::TPose2D>                m_estRobotPath;
00177                  mrpt::poses::CPose2D                                   m_auxAccumOdometry;
00178 
00179                 /** Traveled distances from last map update / ICP-based localization. */
00180                 struct SLAM_IMPEXP TDist
00181                 {
00182                         TDist() : lin(0),ang(0) { }
00183                         double lin; // meters
00184                         double ang; // degrees
00185                 };
00186                 TDist                        m_distSinceLastICP;
00187                 std::map<std::string,TDist>  m_distSinceLastInsertion; //!< Indexed by sensor label.
00188                 bool                         m_there_has_been_an_odometry;
00189                 void accumulateRobotDisplacementCounters(const CPose2D &Apose);
00190 
00191         };
00192 
00193         } // End of namespace
00194 } // End of namespace
00195 
00196 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011