Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
17
18
19namespace mrpt
20{
21namespace slam
22{
23 /** A class for very simple 2D SLAM based on ICP. This is a non-probabilistic pose tracking algorithm.
24 * Map are stored as in files as binary dumps of "mrpt::maps::CSimpleMap" objects. The methods are
25 * thread-safe.
26 * \ingroup metric_slam_grp
27 */
29 {
30 public:
31 /** Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize".
32 */
34
35 /** Destructor:
36 */
38
39 /** Algorithm configuration params
40 */
42 {
43 /** Initializer */
45 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
46 void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
47
48 /** (default:false) Match against the occupancy grid or the points map? The former is quicker but less precise. */
50
51 double insertionLinDistance; //!< Minimum robot linear (m) displacement for a new observation to be inserted in the map.
52 double insertionAngDistance; //!< Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be inserted in the map.
53 double localizationLinDistance; //!< Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (otherwise, dead-reckon with odometry).
54 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).
55
56 double minICPgoodnessToAccept; //!< Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0.40)
57
58 /** What maps to create (at least one points map and/or a grid map are needed).
59 * For the expected format in the .ini file when loaded with loadFromConfigFile(), see documentation of TSetOfMetricMapInitializers.
60 */
62
63 };
64
65 TConfigParams ICP_options; //!< Options for the ICP-SLAM application \sa ICP_params
66 CICP::TConfigParams ICP_params; //!< Options for the ICP algorithm itself \sa ICP_options
67
68 /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map.
69 * This method MUST be called if using the default constructor, after loading the configuration into ICP_options. In particular, TConfigParams::mapInitializers
70 */
73 mrpt::poses::CPosePDF *x0 = NULL
74 );
75
76 /** Returns a copy of the current best pose estimation as a pose PDF.
77 */
79
80 /** 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.
81 */
82 void setCurrentMapFile( const char *mapFile );
83
84 /** 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.
85 * \param action The estimation of the incremental pose change in the robot pose.
86 * \param in_SF The set of observations that robot senses at the new pose.
87 * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
88 * \sa processObservation
89 */
93
94 /** The main method of this class: Process one odometry or sensor observation.
95 The new entry point of the algorithm (the old one was processActionObservation, which now is a wrapper to
96 this method).
97 * See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options
98 */
100
101
102 /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
103 */
105
106
107 /** Returns the 2D points of current local map
108 */
109 void getCurrentMapPoints( std::vector<float> &x, std::vector<float> &y);
110
111 /** 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()".
112 */
114
115 /** Returns just how many sensory-frames are stored in the currently build map.
116 */
118
119 /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
120 * \param file The output file name
121 * \param formatEMF_BMP Output format = true:EMF, false:BMP
122 */
123 void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true);
124
125 private:
126 /** The set of observations that leads to current map: */
128
129 /** The metric map representation as a points map: */
131
132 /** Current map file. */
133 std::string currentMapFile;
134
135 /** The pose estimation by the alignment algorithm (ICP). */
136 mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst; //!< Last pose estimation (Mean)
137 mrpt::math::CMatrixDouble33 m_lastPoseEst_cov; //!< Last pose estimation (covariance)
138
139 /** The estimated robot path:
140 */
141 std::deque<mrpt::math::TPose2D> m_estRobotPath;
143
144 /** Traveled distances from last map update / ICP-based localization. */
146 {
147 TDist() : lin(0),ang(0) { }
148 double lin; // meters
149 double ang; // degrees
151
154 };
158
161
162 };
163
164 } // End of namespace
165} // End of namespace
166
167#endif
This class stores any customizable set of metric maps.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
A numeric matrix of compile-time fixed size.
Declares a class for storing a collection of robot actions.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
A class used to store a 2D pose.
Definition CPose2D.h:37
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
Definition CPosePDF.h:40
A simple filter to estimate and extrapolate the robot 2D (x,y,phi) pose from asynchronous odometry an...
The ICP algorithm configuration data.
Definition CICP.h:58
This virtual class is the base for SLAM implementations.
A class for very simple 2D SLAM based on ICP.
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true)
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
TConfigParams ICP_options
Options for the ICP-SLAM application.
void resetRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), mrpt::poses::CPosePDF *x0=NULL)
Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmod...
mrpt::math::CMatrixDouble33 m_lastPoseEst_cov
Last pose estimation (covariance)
virtual ~CMetricMapBuilderICP()
Destructor:
mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation() const
Returns a copy of the current best pose estimation as a pose PDF.
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...
CMetricMapBuilderICP()
Default constructor - Upon construction, you can set the parameters in ICP_options,...
std::deque< mrpt::math::TPose2D > m_estRobotPath
The estimated robot path:
mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap()
Returns the map built so far.
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF)
Appends a new action and observations to update this map: See the description of the class at the top...
CICP::TConfigParams ICP_params
Options for the ICP algorithm itself.
std::string currentMapFile
Current map file.
mrpt::maps::CSimpleMap SF_Poses_seq
The set of observations that leads to current map:
unsigned int getCurrentlyBuiltMapSize()
Returns just how many sensory-frames are stored in the currently build map.
mrpt::poses::CRobot2DPoseEstimator m_lastPoseEst
The pose estimation by the alignment algorithm (ICP).
void getCurrentMapPoints(std::vector< float > &x, std::vector< float > &y)
Returns the 2D points of current local map.
mrpt::maps::CMultiMetricMap metricMap
The metric map representation as a points map:
void processObservation(const mrpt::obs::CObservationPtr &obs)
The main method of this class: Process one odometry or sensor observation.
void accumulateRobotDisplacementCounters(const mrpt::poses::CPose2D &new_pose)
mrpt::aligned_containers< std::string, TDist >::map_t m_distSinceLastInsertion
Indexed by sensor label.
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition CStream.h:39
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition mrpt_macros.h:28
struct OBS_IMPEXP CObservationPtr
struct BASE_IMPEXP CPose3DPDFPtr
Definition CPose3DPDF.h:23
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
mrpt::maps::TSetOfMetricMapInitializers mapInitializers
What maps to create (at least one points map and/or a grid map are needed).
double minICPgoodnessToAccept
Minimum ICP goodness (0,1) to accept the resulting corrected position (default: 0....
double insertionLinDistance
Minimum robot linear (m) displacement for a new observation to be inserted in the map.
double localizationLinDistance
Minimum robot linear (m) displacement for a new observation to be used to do ICP-based localization (...
double insertionAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be i...
double localizationAngDistance
Minimum robot angular (rad, deg when loaded from the .ini) displacement for a new observation to be u...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
bool matchAgainstTheGrid
(default:false) Match against the occupancy grid or the points map? The former is quicker but less pr...
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
Traveled distances from last map update / ICP-based localization.
void updateDistances(const mrpt::poses::CPose2D &p)
void updatePose(const mrpt::poses::CPose2D &p)



Page generated by Doxygen 1.9.8 for MRPT 1.4.0 SVN: at Fri Dec 15 05:36:48 UTC 2023