Main MRPT website > C++ reference
MRPT logo
CMetricMapBuilderICP.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 CMetricMapBuilderICP_H
29 #define CMetricMapBuilderICP_H
30 
31 #include <mrpt/slam/CMetricMapBuilder.h>
32 #include <mrpt/slam/CICP.h>
33 #include <mrpt/poses/CRobot2DPoseEstimator.h>
34 
35 #include <mrpt/slam/link_pragmas.h>
36 
37 
38 namespace mrpt
39 {
40 namespace slam
41 {
42  /** A class for very simple 2D SLAM based on ICP. This is a non-probabilistic pose tracking algorithm.
43  * Map are stored as in files as binary dumps of "mrpt::slam::CSimpleMap" objects. The methods are
44  * thread-safe.
45  * \ingroup metric_slam_grp
46  */
47  class SLAM_IMPEXP CMetricMapBuilderICP : public CMetricMapBuilder
48  {
49  public:
50  /** Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize".
51  */
52  CMetricMapBuilderICP();
53 
54  /** Destructor:
55  */
56  virtual ~CMetricMapBuilderICP();
57 
58  /** Algorithm configuration params
59  */
60  struct SLAM_IMPEXP TConfigParams : public mrpt::utils::CLoadableOptions
61  {
62  /** Initializer */
63  TConfigParams ();
64  virtual void loadFromConfigFile(
65  const mrpt::utils::CConfigFileBase &source,
66  const std::string &section);
67  virtual void dumpToTextStream( CStream &out) const;
68 
69  /** (default:false) Match against the occupancy grid or the points map? The former is quicker but less precise. */
70  bool matchAgainstTheGrid;
71 
72  double insertionLinDistance; //!< Minimum robot linear (m) displacement for a new observation to be inserted in the map.
73  double insertionAngDistance; //!< Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be inserted in the map.
74  double localizationLinDistance; //!< Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (otherwise, dead-reckon with odometry).
75  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).
76 
77  double minICPgoodnessToAccept; //!< Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0.40)
78 
79  /** What maps to create (at least one points map and/or a grid map are needed).
80  * For the expected format in the .ini file when loaded with loadFromConfigFile(), see documentation of TSetOfMetricMapInitializers.
81  */
82  TSetOfMetricMapInitializers mapInitializers;
83 
84  };
85 
86  TConfigParams ICP_options; //!< Options for the ICP-SLAM application \sa ICP_params
87  CICP::TConfigParams ICP_params; //!< Options for the ICP algorithm itself \sa ICP_options
88 
89  /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map.
90  * This method MUST be called if using the default constructor, after loading the configuration into ICP_options. In particular, TConfigParams::mapInitializers
91  */
92  void initialize(
93  const CSimpleMap &initialMap = CSimpleMap(),
94  CPosePDF *x0 = NULL
95  );
96 
97  /** Returns a copy of the current best pose estimation as a pose PDF.
98  */
99  CPose3DPDFPtr getCurrentPoseEstimation() const;
100 
101  /** 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.
102  */
103  void setCurrentMapFile( const char *mapFile );
104 
105  /** 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.
106  * \param action The estimation of the incremental pose change in the robot pose.
107  * \param in_SF The set of observations that robot senses at the new pose.
108  * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
109  * \sa processObservation
110  */
111  void processActionObservation(
112  CActionCollection &action,
113  CSensoryFrame &in_SF );
114 
115  /** The main method of this class: Process one odometry or sensor observation.
116  The new entry point of the algorithm (the old one was processActionObservation, which now is a wrapper to
117  this method).
118  * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
119  */
120  void processObservation(const CObservationPtr &obs);
121 
122 
123  /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
124  */
125  void getCurrentlyBuiltMap(CSimpleMap &out_map) const;
126 
127 
128  /** Returns the 2D points of current local map
129  */
130  void getCurrentMapPoints( std::vector<float> &x, std::vector<float> &y);
131 
132  /** 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()".
133  */
134  CMultiMetricMap* getCurrentlyBuiltMetricMap();
135 
136  /** Returns just how many sensory-frames are stored in the currently build map.
137  */
138  unsigned int getCurrentlyBuiltMapSize();
139 
140  /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
141  * \param file The output file name
142  * \param formatEMF_BMP Output format = true:EMF, false:BMP
143  */
144  void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true);
145 
146  private:
147  /** The set of observations that leads to current map:
148  */
149  CSimpleMap SF_Poses_seq;
150 
151  /** The metric map representation as a points map:
152  */
153  CMultiMetricMap metricMap;
154 
155  /** Current map file.
156  */
157  std::string currentMapFile;
158 
159  /** The pose estimation by the alignment algorithm (ICP). */
160  mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst; //!< Last pose estimation (Mean)
161  mrpt::math::CMatrixDouble33 m_lastPoseEst_cov; //!< Last pose estimation (covariance)
162 
163  /** The estimated robot path:
164  */
165  std::deque<mrpt::math::TPose2D> m_estRobotPath;
166  mrpt::poses::CPose2D m_auxAccumOdometry;
167 
168  /** Traveled distances from last map update / ICP-based localization. */
169  struct SLAM_IMPEXP TDist
170  {
171  TDist() : lin(0),ang(0) { }
172  double lin; // meters
173  double ang; // degrees
174  };
175  TDist m_distSinceLastICP;
176  std::map<std::string,TDist> m_distSinceLastInsertion; //!< Indexed by sensor label.
177  bool m_there_has_been_an_odometry;
178  void accumulateRobotDisplacementCounters(const CPose2D &Apose);
179 
180  };
181 
182  } // End of namespace
183 } // End of namespace
184 
185 #endif


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