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



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST