Main MRPT website > C++ reference for MRPT 1.4.0
CICP.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 CICP_H
10#define CICP_H
11
15
16namespace mrpt
17{
18 namespace slam
19 {
20 /** The ICP algorithm selection, used in mrpt::slam::CICP::options \ingroup mrpt_slam_grp */
24 };
25
26 /** ICP covariance estimation methods, used in mrpt::slam::CICP::options \ingroup mrpt_slam_grp */
28 icpCovLinealMSE = 0, //!< Use the covariance of the optimal registration, disregarding uncertainty in data association
29 icpCovFiniteDifferences //!< Covariance of a least-squares optimizer (includes data association uncertainty)
30 };
31
32 /** Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a point map wrt a grid map.
33 *
34 * CICP::AlignPDF() or CICP::Align() are the two main entry points of the algorithm.
35 *
36 * To choose among existing ICP algorithms or customizing their parameters, see CICP::TConfigParams and the member \a options.
37 *
38 * There exists an extension of the original ICP algorithm that provides multihypotheses-support for the correspondences, and which generates a Sum-of-Gaussians (SOG)
39 * PDF as output. See mrpt::tfest::se2_l2_robust()
40 *
41 * For further details on the implemented methods, check the web:
42 * http://www.mrpt.org/Iterative_Closest_Point_(ICP)_and_other_matching_algorithms
43 *
44 * For a paper explaining some of the basic equations, see for example:
45 * J. Martinez, J. Gonzalez, J. Morales, A. Mandow, A. Garcia-Cerezo,
46 * "Mobile robot motion estimation by 2D scan matching with genetic and iterative closest point algorithms",
47 * Journal of Field Robotics, vol. 23, no. 1, pp. 21-34, 2006. ( http://babel.isa.uma.es/~jlblanco/papers/martinez2006gil.pdf )
48 *
49 * \sa CMetricMapsAlignmentAlgorithm
50 * \ingroup mrpt_slam_grp
51 */
53 {
54 public:
55 /** The ICP algorithm configuration data
56 */
58 {
59 public:
60 TConfigParams(); //!< Initializer for default values:
61
62 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
63 void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
64
65 /** @name Algorithms selection
66 @{ */
67 TICPAlgorithm ICP_algorithm; //!< The algorithm to use (default: icpClassic). See http://www.mrpt.org/tutorials/programming/scan-matching-and-icp/ for details
68 TICPCovarianceMethod ICP_covariance_method; //!< The method to use for covariance estimation (Default: icpCovFiniteDifferences)
69 /** @} */
70
71 /** @name Correspondence-finding criteria
72 @{ */
73 bool onlyClosestCorrespondences; //!< The usual approach: to consider only the closest correspondence for each local point (Default to true)
74 bool onlyUniqueRobust; //! Apart of "onlyClosestCorrespondences=true", if this option is enabled only the closest correspondence for each reference point will be kept (default=false).
75 /** @} */
76
77 /** @name Termination criteria
78 @{ */
79 unsigned int maxIterations; //!< Maximum number of iterations to run.
80 float minAbsStep_trans; //!< If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated (Default:1e-6)
81 float minAbsStep_rot; //!< If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated (Default:1e-6)
82 /** @} */
83
84 float thresholdDist,thresholdAng; //!< Initial threshold distance for two points to become a correspondence.
85 float ALFA; //!< The scale factor for threshold everytime convergence is achieved.
86 float smallestThresholdDist; //!< The size for threshold such that iterations will stop, since it is considered precise enough.
87
88 /** This is the normalization constant \f$ \sigma^2_p \f$ that is used to scale the whole 3x3 covariance.
89 * This has a default value of \f$ (0.02)^2 \f$, that is, a 2cm sigma.
90 * See paper: J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "A Robust, Multi-Hypothesis Approach to Matching Occupancy Grid Maps", Robotica, vol. 31, no. 5, pp. 687-701, 2013.
91 */
93
94 bool doRANSAC; //!< Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better estimation of the pose PDF.
95
96 /** @name RANSAC-step options for mrpt::tfest::se2_l2_robust() if \a doRANSAC=true
97 * @{ */
98 unsigned int ransac_minSetSize,ransac_maxSetSize,ransac_nSimulations;
100 float normalizationStd; //!< RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used to compute covariances in the SoG)
102 float ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi;
103 /** @} */
104
105 float kernel_rho; //!< Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0.07m)
106 bool use_kernel; //!< Whether to use kernel_rho to smooth distances, or use distances directly (default=true)
107 float Axy_aprox_derivatives; //!< [LM method only] The size of the perturbance in x & y used to estimate the Jacobians of the square error (default=0.05)
108 float LM_initial_lambda; //!< [LM method only] The initial value of the lambda parameter in the LM method (default=1e-4)
109
110 bool skip_cov_calculation; //!< Skip the computation of the covariance (saves some time) (default=false)
111 bool skip_quality_calculation; //!< Skip the (sometimes) expensive evaluation of the term 'quality' at ICP output (Default=true)
112
113 /** Decimation of the point cloud being registered against the reference one (default=5) - set to 1 to have the older (MRPT <0.9.5) behavior
114 * of not approximating ICP by ignoring the correspondence of some points. The speed-up comes from a decimation of the number of KD-tree queries,
115 * the most expensive step in ICP */
117 };
118
119 TConfigParams options; //!< The options employed by the ICP align.
120
121 /** Constructor with the default options */
122 CICP() : options() { }
123 /** Constructor that directly set the ICP params from a given struct \sa options */
124 CICP(const TConfigParams &icpParams) : options(icpParams) { }
125 virtual ~CICP() { } //!< Destructor
126
127 /** The ICP algorithm return information*/
129 {
131 nIterations(0),
132 goodness(0),
133 quality(0),
134 cbSize(sizeof(TReturnInfo))
135 {
136 }
137 unsigned short nIterations; //!< The number of executed iterations until convergence
138 float goodness; //!< A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
139 float quality; //!< A measure of the 'quality' of the local minimum of the sqr. error found by the method. Higher values are better. Low values will be found in ill-conditioned situations (e.g. a corridor)
140
141 unsigned int cbSize;
142 };
143
144 /** An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/point map.
145 *
146 * This method computes the PDF of the displacement (relative pose) between
147 * two maps: <b>the relative pose of m2 with respect to m1</b>. This pose
148 * is returned as a PDF rather than a single value.
149 *
150 * \note This method can be configurated with "CICP::options"
151 * \note The output PDF is a CPosePDFGaussian if "doRANSAC=false", or a CPosePDFSOG otherwise.
152 *
153 * \param m1 [IN] The first map (CAN BE A mrpt::poses::CPointsMap derived class or a mrpt::slam::COccupancyGrid2D class)
154 * \param m2 [IN] The second map. (MUST BE A mrpt::poses::CPointsMap derived class)The pose of this map respect to m1 is to be estimated.
155 * \param initialEstimationPDF [IN] An initial gross estimation for the displacement.
156 * \param runningTime [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
157 * \param info [OUT] A pointer to a CICP::TReturnInfo, or NULL if it isn't needed.
158 *
159 * \return A smart pointer to the output estimated pose PDF.
160 *
161 * \sa CMetricMapsAlignmentAlgorithm, CICP::options, CICP::TReturnInfo
162 */
164 const mrpt::maps::CMetricMap *m1,
165 const mrpt::maps::CMetricMap *m2,
166 const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
167 float *runningTime = NULL,
168 void *info = NULL );
169
170 // See base class for docs
172 const mrpt::maps::CMetricMap *m1,
173 const mrpt::maps::CMetricMap *m2,
174 const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF,
175 float *runningTime = NULL,
176 void *info = NULL );
177
178
179 protected:
180 /** Computes:
181 * \f[ K(x^2) = \frac{x^2}{x^2+\rho^2} \f]
182 * or just return the input if options.useKernel = false.
183 */
184 float kernel(const float &x2, const float &rho2);
185
187 const mrpt::maps::CMetricMap *m1,
188 const mrpt::maps::CMetricMap *m2,
189 const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
190 TReturnInfo &outInfo );
192 const mrpt::maps::CMetricMap *m1,
193 const mrpt::maps::CMetricMap *m2,
194 const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
195 TReturnInfo &outInfo );
197 const mrpt::maps::CMetricMap *m1,
198 const mrpt::maps::CMetricMap *m2,
199 const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF,
200 TReturnInfo &outInfo );
201 };
202 } // End of namespace
203
204 // Specializations MUST occur at the same namespace:
205 namespace utils
206 {
207 template <>
208 struct TEnumTypeFiller<slam::TICPAlgorithm>
209 {
211 static void fill(bimap<enum_t,std::string> &m_map)
212 {
213 m_map.insert(slam::icpClassic, "icpClassic");
214 m_map.insert(slam::icpLevenbergMarquardt, "icpLevenbergMarquardt");
215 }
216 };
217 template <>
218 struct TEnumTypeFiller<slam::TICPCovarianceMethod>
219 {
221 static void fill(bimap<enum_t,std::string> &m_map)
222 {
223 m_map.insert(slam::icpCovLinealMSE, "icpCovLinealMSE");
224 m_map.insert(slam::icpCovFiniteDifferences, "icpCovFiniteDifferences");
225 }
226 };
227 } // End of namespace
228} // End of namespace
229
230#endif
Declares a virtual base class for all metric maps storage classes.
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
The ICP algorithm configuration data.
Definition: CICP.h:58
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:79
bool skip_quality_calculation
Skip the (sometimes) expensive evaluation of the term 'quality' at ICP output (Default=true)
Definition: CICP.h:111
float Axy_aprox_derivatives
[LM method only] The size of the perturbance in x & y used to estimate the Jacobians of the square er...
Definition: CICP.h:107
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic). See http://www.mrpt.org/tutorials/programming/scan-matchi...
Definition: CICP.h:67
uint32_t corresponding_points_decimation
Decimation of the point cloud being registered against the reference one (default=5) - set to 1 to ha...
Definition: CICP.h:116
float smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough.
Definition: CICP.h:86
float normalizationStd
RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used t...
Definition: CICP.h:100
bool onlyClosestCorrespondences
The usual approach: to consider only the closest correspondence for each local point (Default to true...
Definition: CICP.h:73
float thresholdAng
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:84
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
bool skip_cov_calculation
Skip the computation of the covariance (saves some time) (default=false)
Definition: CICP.h:110
float minAbsStep_trans
If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters),...
Definition: CICP.h:80
TICPCovarianceMethod ICP_covariance_method
The method to use for covariance estimation (Default: icpCovFiniteDifferences)
Definition: CICP.h:68
float ALFA
The scale factor for threshold everytime convergence is achieved.
Definition: CICP.h:85
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.
TConfigParams()
Initializer for default values:
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
Definition: CICP.h:94
float kernel_rho
Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0....
Definition: CICP.h:105
float covariance_varPoints
This is the normalization constant that is used to scale the whole 3x3 covariance.
Definition: CICP.h:92
bool use_kernel
Whether to use kernel_rho to smooth distances, or use distances directly (default=true)
Definition: CICP.h:106
float LM_initial_lambda
[LM method only] The initial value of the lambda parameter in the LM method (default=1e-4)
Definition: CICP.h:108
float ransac_mahalanobisDistanceThreshold
Definition: CICP.h:99
unsigned int ransac_maxSetSize
Definition: CICP.h:98
float minAbsStep_rot
If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians),...
Definition: CICP.h:81
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:53
CICP()
Constructor with the default options.
Definition: CICP.h:122
mrpt::poses::CPosePDFPtr ICP_Method_LM(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
mrpt::poses::CPose3DPDFPtr ICP3D_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
float kernel(const float &x2, const float &rho2)
Computes:
mrpt::poses::CPosePDFPtr ICP_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
mrpt::poses::CPosePDFPtr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=NULL, void *info=NULL)
An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/...
CICP(const TConfigParams &icpParams)
Constructor that directly set the ICP params from a given struct.
Definition: CICP.h:124
virtual ~CICP()
Destructor.
Definition: CICP.h:125
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:119
mrpt::poses::CPose3DPDFPtr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, float *runningTime=NULL, void *info=NULL)
The virtual method for aligning a pair of metric maps, aligning the full 6D pose.
A base class for any algorithm able of maps alignment.
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
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:29
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
TICPCovarianceMethod
ICP covariance estimation methods, used in mrpt::slam::CICP::options <>
Definition: CICP.h:27
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options <>
Definition: CICP.h:21
@ icpCovLinealMSE
Use the covariance of the optimal registration, disregarding uncertainty in data association.
Definition: CICP.h:28
@ icpCovFiniteDifferences
Covariance of a least-squares optimizer (includes data association uncertainty)
Definition: CICP.h:29
@ icpLevenbergMarquardt
Definition: CICP.h:23
@ icpClassic
Definition: CICP.h:22
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
struct BASE_IMPEXP CPose3DPDFPtr
Definition: CPose3DPDF.h:23
struct BASE_IMPEXP CPosePDFPtr
Definition: CPosePDF.h:25
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned long uint32_t
Definition: pstdint.h:216
The ICP algorithm return information.
Definition: CICP.h:129
float quality
A measure of the 'quality' of the local minimum of the sqr. error found by the method....
Definition: CICP.h:139
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:137
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
Definition: CICP.h:138
static void fill(bimap< enum_t, std::string > &m_map)
Definition: CICP.h:211
static void fill(bimap< enum_t, std::string > &m_map)
Definition: CICP.h:221
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:24



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 06:31:24 UTC 2023