MRPT  2.0.1
CMetricMapBuilderICP.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
12 #include <mrpt/slam/CICP.h>
14 #include <map>
15 
16 namespace mrpt::slam
17 {
18 /** A class for very simple 2D SLAM based on ICP. This is a non-probabilistic
19  *pose tracking algorithm.
20  * Map are stored as in files as binary dumps of "mrpt::maps::CSimpleMap"
21  *objects. The methods are
22  * thread-safe.
23  * \ingroup metric_slam_grp
24  */
26 {
27  public:
28  /** Default constructor - Upon construction, you can set the parameters in
29  * ICP_options, then call "initialize".
30  */
32 
33  /** Destructor:
34  */
35  ~CMetricMapBuilderICP() override;
36 
37  /** Algorithm configuration params
38  */
40  {
41  /** Initializer */
42  TConfigParams(mrpt::system::VerbosityLevel& parent_verbosity_level);
43  TConfigParams& operator=(const TConfigParams& other); // Copy
44  // assignment
45 
46  void loadFromConfigFile(
47  const mrpt::config::CConfigFileBase& source,
48  const std::string& section) override; // See base docs
49  void dumpToTextStream(
50  std::ostream& out) const override; // See base docs
51  /** (default:false) Match against the occupancy grid or the points map?
52  * The former is quicker but less precise. */
54 
55  /** Minimum robot linear (m) displacement for a new observation to be
56  * inserted in the map. */
58  /** Minimum robot angular (rad, deg when loaded from the .ini)
59  * displacement for a new observation to be inserted in the map. */
61  /** Minimum robot linear (m) displacement for a new observation to be
62  * used to do ICP-based localization (otherwise, dead-reckon with
63  * odometry). */
65  /** Minimum robot angular (rad, deg when loaded from the .ini)
66  * displacement for a new observation to be used to do ICP-based
67  * localization (otherwise, dead-reckon with odometry). */
69 
70  /** Minimum ICP goodness (0,1) to accept the resulting corrected
71  * position (default: 0.40) */
73 
75 
76  /** What maps to create (at least one points map and/or a grid map are
77  * needed).
78  * For the expected format in the .ini file when loaded with
79  * loadFromConfigFile(), see documentation of
80  * TSetOfMetricMapInitializers.
81  */
83  };
84 
85  /** Options for the ICP-SLAM application \sa ICP_params */
87  /** Options for the ICP algorithm itself \sa ICP_options */
89 
90  /** Initialize the method, starting with a known location PDF "x0"(if
91  * supplied, set to nullptr to left unmodified) and a given fixed, past map.
92  * This method MUST be called if using the default constructor, after
93  * loading the configuration into ICP_options. In particular,
94  * TConfigParams::mapInitializers
95  */
96  void initialize(
97  const mrpt::maps::CSimpleMap& initialMap = mrpt::maps::CSimpleMap(),
98  const mrpt::poses::CPosePDF* x0 = nullptr) override;
99 
100  /** Returns a copy of the current best pose estimation as a pose PDF.
101  */
103 
104  /** Sets the "current map file", thus that map will be loaded if it exists
105  * or a new one will be created if it does not, and the updated map will be
106  * save to that file when destroying the object.
107  */
108  void setCurrentMapFile(const char* mapFile);
109 
110  /** Appends a new action and observations to update this map: See the
111  *description of the class at the top of this page to see a more complete
112  *description.
113  * \param action The estimation of the incremental pose change in the robot
114  *pose.
115  * \param in_SF The set of observations that robot senses at the new pose.
116  * See params in CMetricMapBuilder::options and
117  *CMetricMapBuilderICP::ICP_options
118  * \sa processObservation
119  */
122  mrpt::obs::CSensoryFrame& in_SF) override;
123 
124  /** The main method of this class: Process one odometry or sensor
125  observation.
126  The new entry point of the algorithm (the old one was
127  processActionObservation, which now is a wrapper to
128  this method).
129  * See params in CMetricMapBuilder::options and
130  CMetricMapBuilderICP::ICP_options
131  */
133 
134  /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so
135  * far built map */
136  void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap& out_map) const override;
137 
138  /** Returns the 2D points of current local map */
139  void getCurrentMapPoints(std::vector<float>& x, std::vector<float>& y);
140 
142  const override;
143 
144  /** Returns just how many sensory-frames are stored in the currently build
145  * map */
146  unsigned int getCurrentlyBuiltMapSize() override;
147 
148  /** A useful method for debugging: the current map (and/or poses) estimation
149  * is dumped to an image file.
150  * \param file The output file name
151  * \param formatEMF_BMP Output format = true:EMF, false:BMP
152  */
154  const std::string& file, bool formatEMF_BMP = true) override;
155 
156  private:
157  /** The set of observations that leads to current map: */
159 
160  /** The metric map representation as a points map: */
162 
163  /** Current map file. */
164  std::string currentMapFile;
165 
166  /** The pose estimation by the alignment algorithm (ICP). */
167  /** Last pose estimation (Mean) */
169  /** Last pose estimation (covariance) */
171 
172  /** The estimated robot path:
173  */
174  std::deque<mrpt::math::TPose2D> m_estRobotPath;
176 
177  /** Traveled distances from last map update / ICP-based localization. */
178  struct TDist
179  {
180  TDist() = default;
181  double lin{0}; // meters
182  double ang{0}; // degrees
184 
185  void updateDistances(const mrpt::poses::CPose2D& p);
186  void updatePose(const mrpt::poses::CPose2D& p);
187  };
189  /** Indexed by sensor label. */
190  std::map<std::string, TDist> m_distSinceLastInsertion;
192 
194  const mrpt::poses::CPose2D& new_pose);
196 };
197 
198 } // namespace mrpt::slam
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
double localizationAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be u...
void updatePose(const mrpt::poses::CPose2D &p)
mrpt::maps::TSetOfMetricMapInitializers mapInitializers
What maps to create (at least one points map and/or a grid map are needed).
std::map< std::string, TDist > m_distSinceLastInsertion
Indexed by sensor label.
The ICP algorithm configuration data.
Definition: CICP.h:69
VerbosityLevel
Enumeration of available verbosity levels.
mrpt::maps::CMultiMetricMap metricMap
The metric map representation as a points map:
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
CMetricMapBuilderICP()
Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize".
double minICPgoodnessToAccept
Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0.40)
void getCurrentMapPoints(std::vector< float > &x, std::vector< float > &y)
Returns the 2D points of current local map.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
std::deque< mrpt::math::TPose2D > m_estRobotPath
The estimated robot path:
A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry an...
mrpt::maps::CSimpleMap SF_Poses_seq
The set of observations that leads to current map:
TConfigParams ICP_options
Options for the ICP-SLAM application.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
double insertionAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be i...
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr) override
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left un...
Declares a class for storing a collection of robot actions.
bool matchAgainstTheGrid
(default:false) Match against the occupancy grid or the points map? The former is quicker but less pr...
void updateDistances(const mrpt::poses::CPose2D &p)
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
This class allows loading and storing values and vectors of different types from a configuration text...
TConfigParams(mrpt::system::VerbosityLevel &parent_verbosity_level)
Initializer.
TConfigParams & operator=(const TConfigParams &other)
std::string currentMapFile
Current map file.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
void resetRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
This virtual class is the base for SLAM implementations.
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
void setCurrentMapFile(const char *mapFile)
Sets the "current map file", thus that map will be loaded if it exists or a new one will be created i...
mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst
The pose estimation by the alignment algorithm (ICP).
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
mrpt::vision::TStereoCalibResults out
~CMetricMapBuilderICP() override
Destructor:
Lightweight 2D pose.
Definition: TPose2D.h:22
void processObservation(const mrpt::obs::CObservation::Ptr &obs)
The main method of this class: Process one odometry or sensor observation.
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const override
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
mrpt::math::CMatrixDouble33 m_lastPoseEst_cov
Last pose estimation (covariance)
double localizationLinDistance
Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (...
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
A class for very simple 2D SLAM based on ICP.
This class stores any customizable set of metric maps.
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF) override
Appends a new action and observations to update this map: See the description of the class at the top...
double insertionLinDistance
Minimum robot linear (m) displacement for a new observation to be inserted in the map...
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true) override
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file...
void accumulateRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
Traveled distances from last map update / ICP-based localization.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020