MRPT  2.0.0
CICP.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 
14 
15 namespace mrpt::slam
16 {
17 /** The ICP algorithm selection, used in mrpt::slam::CICP::options \ingroup
18  * mrpt_slam_grp */
20 {
23 };
24 
25 /** ICP covariance estimation methods, used in mrpt::slam::CICP::options
26  * \ingroup mrpt_slam_grp */
28 {
29  /** Use the covariance of the optimal registration, disregarding uncertainty
30  in data association */
32  /** Covariance of a least-squares optimizer (includes data association
33  uncertainty) */
35 };
36 
37 /** Several implementations of ICP (Iterative closest point) algorithms for
38  * aligning two point maps or a point map wrt a grid map.
39  *
40  * CICP::AlignPDF() or CICP::Align() are the two main entry points of the
41  * algorithm.
42  *
43  * To choose among existing ICP algorithms or customizing their parameters, see
44  * CICP::TConfigParams and the member \a options.
45  *
46  * There exists an extension of the original ICP algorithm that provides
47  * multihypotheses-support for the correspondences, and which generates a
48  * Sum-of-Gaussians (SOG)
49  * PDF as output. See mrpt::tfest::se2_l2_robust()
50  *
51  * For further details on the implemented methods, check the web:
52  * https://www.mrpt.org/Iterative_Closest_Point_(ICP)_and_other_matching_algorithms
53  *
54  * For a paper explaining some of the basic equations, see for example:
55  * J. Martinez, J. Gonzalez, J. Morales, A. Mandow, A. Garcia-Cerezo,
56  * "Mobile robot motion estimation by 2D scan matching with genetic and
57  * iterative closest point algorithms",
58  * Journal of Field Robotics, vol. 23, no. 1, pp. 21-34, 2006. (
59  * http://babel.isa.uma.es/~jlblanco/papers/martinez2006gil.pdf )
60  *
61  * \sa CMetricMapsAlignmentAlgorithm
62  * \ingroup mrpt_slam_grp
63  */
65 {
66  public:
67  /** The ICP algorithm configuration data
68  */
70  {
71  public:
72  void loadFromConfigFile(
73  const mrpt::config::CConfigFileBase& source,
74  const std::string& section) override; // See base docs
75  void saveToConfigFile(
77  const std::string& s) const override;
78 
79  /** @name Algorithms selection
80  @{ */
81  /** The algorithm to use (default: icpClassic). See
82  * https://www.mrpt.org/tutorials/programming/scan-matching-and-icp/ for
83  * details */
85  /** The method to use for covariance estimation (Default:
86  * icpCovFiniteDifferences) */
88  /** @} */
89 
90  /** @name Correspondence-finding criteria
91  @{ */
92  bool onlyUniqueRobust{false};
93  //! if this option is enabled only the closest
94  //! correspondence for each reference point will
95  //! be kept (default=false).
96  /** @} */
97 
98  /** @name Termination criteria
99  @{ */
100  /** Maximum number of iterations to run. */
101  unsigned int maxIterations{40};
102  /** If the correction in all translation coordinates (X,Y,Z) is below
103  * this threshold (in meters), iterations are terminated (Default:1e-6)
104  */
105  double minAbsStep_trans{1e-6};
106  /** If the correction in all rotation coordinates (yaw,pitch,roll) is
107  * below this threshold (in radians), iterations are terminated
108  * (Default:1e-6) */
109  double minAbsStep_rot{1e-6};
110  /** @} */
111 
112  /** Initial threshold distance for two points to become a
113  * correspondence. */
114  double thresholdDist{0.75}, thresholdAng{0.15 * M_PI / 180.0};
115  /** The scale factor for thresholds every time convergence is achieved.
116  */
117  double ALFA{0.5};
118  /** The size for threshold such that iterations will stop, since it is
119  * considered precise enough. */
121 
122  /** This is the normalization constant \f$ \sigma^2_p \f$ that is used
123  * to scale the whole 3x3 covariance.
124  * This has a default value of \f$ (0.02)^2 \f$, that is, a 2cm sigma.
125  * See paper: J.L. Blanco, J. Gonzalez-Jimenez, J.A.
126  * Fernandez-Madrigal, "A Robust, Multi-Hypothesis Approach to Matching
127  * Occupancy Grid Maps", Robotica, vol. 31, no. 5, pp. 687-701, 2013.
128  */
129  double covariance_varPoints{0.02 * 0.02};
130 
131  /** Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP
132  * convergence, to obtain a better estimation of the pose PDF. */
133  bool doRANSAC{false};
134 
135  /** @name RANSAC-step options for mrpt::tfest::se2_l2_robust() if \a
136  * doRANSAC=true
137  * @{ */
138  unsigned int ransac_minSetSize{3}, ransac_maxSetSize{20},
141  /** RANSAC-step option: The standard deviation in X,Y of
142  * landmarks/points which are being matched (used to compute covariances
143  * in the SoG) */
144  double normalizationStd{0.02};
146  double ransac_fuseMaxDiffXY{0.01},
147  ransac_fuseMaxDiffPhi{0.1 * M_PI / 180.0};
148  /** @} */
149 
150  /** Cauchy kernel rho, for estimating the optimal transformation
151  * covariance, in meters (default = 0.07m) */
152  double kernel_rho{0.07};
153  /** Whether to use kernel_rho to smooth distances, or use distances
154  * directly (default=true) */
155  bool use_kernel{true};
156  /** [LM method only] The size of the perturbance in x & y used to
157  * estimate the Jacobians of the square error (default=0.05) */
158  double Axy_aprox_derivatives{0.05};
159  /** [LM method only] The initial value of the lambda parameter in the LM
160  * method (default=1e-4) */
161  double LM_initial_lambda{1e-4};
162 
163  /** Skip the computation of the covariance (saves some time)
164  * (default=false) */
165  bool skip_cov_calculation{false};
166  /** Skip the (sometimes) expensive evaluation of the term 'quality' at
167  * ICP output (Default=true) */
169 
170  /** Decimation of the point cloud being registered against the reference
171  * one (default=5) - set to 1 to have the older (MRPT <0.9.5) behavior
172  * of not approximating ICP by ignoring the correspondence of some
173  * points. The speed-up comes from a decimation of the number of KD-tree
174  * queries,
175  * the most expensive step in ICP */
177  };
178 
179  /** The options employed by the ICP align. */
181 
182  /** Constructor with the default options */
183  CICP() : options() {}
184  /** Constructor that directly set the ICP params from a given struct \sa
185  * options */
186  CICP(const TConfigParams& icpParams) : options(icpParams) {}
187  /** Destructor */
188  ~CICP() override = default;
189  /** The ICP algorithm return information*/
191  {
192  TReturnInfo() = default;
193  virtual ~TReturnInfo() override = default;
194 
195  /** The number of executed iterations until convergence */
196  unsigned int nIterations = 0;
197 
198  /** A goodness measure for the alignment, it is a [0,1] range indicator
199  * of percentage of correspondences. */
200  double goodness = 0;
201 
202  /** A measure of the 'quality' of the local minimum of the sqr. error
203  * found by the method. Higher values are better. Low values will be
204  * found in ill-conditioned situations (e.g. a corridor) */
205  double quality = 0;
206  };
207 
208  /** See base method docs.
209  * This class offers an implementation for the case of "m1" being a point
210  * map and "m2" either an occupancy grid or a point map.
211  *
212  * \note This method can be configurated with "CICP::options"
213  * \note The output PDF is a CPosePDFGaussian if "doRANSAC=false", or a
214  * CPosePDFSOG otherwise.
215  *
216  * \sa CMetricMapsAlignmentAlgorithm, CICP::options, CICP::TReturnInfo
217  */
219  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
220  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
222  std::nullopt) override;
223 
224  // See base class for docs
226  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
227  const mrpt::poses::CPose3DPDFGaussian& initialEstimationPDF,
229  std::nullopt) override;
230 
231  protected:
232  /** Computes:
233  * \f[ K(x^2) = \frac{x^2}{x^2+\rho^2} \f]
234  * or just return the input if options.useKernel = false.
235  */
236  float kernel(float x2, float rho2);
237 
239  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
240  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
241  TReturnInfo& outInfo);
243  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
244  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
245  TReturnInfo& outInfo);
247  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
248  const mrpt::poses::CPose3DPDFGaussian& initialEstimationPDF,
249  TReturnInfo& outInfo);
250 };
251 } // namespace mrpt::slam
253 using namespace mrpt::slam;
257 
259 using namespace mrpt::slam;
Use the covariance of the optimal registration, disregarding uncertainty in data association.
Definition: CICP.h:31
The ICP algorithm configuration data.
Definition: CICP.h:69
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
mrpt::poses::CPosePDF::Ptr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt) override
See base method docs.
Definition: CICP.cpp:36
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic).
Definition: CICP.h:84
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
double LM_initial_lambda
[LM method only] The initial value of the lambda parameter in the LM method (default=1e-4) ...
Definition: CICP.h:161
unsigned int nIterations
The number of executed iterations until convergence.
Definition: CICP.h:196
bool skip_quality_calculation
Skip the (sometimes) expensive evaluation of the term &#39;quality&#39; at ICP output (Default=true) ...
Definition: CICP.h:168
unsigned int ransac_nSimulations
Definition: CICP.h:139
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
Definition: CICP.h:120
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:180
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: CICP.cpp:80
mrpt::poses::CPose3DPDF::Ptr ICP3D_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:957
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:176
Used as base class for other result structures of each particular algorithm in CMetricMapsAlignmentAl...
virtual ~TReturnInfo() override=default
This class allows loading and storing values and vectors of different types from a configuration text...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:101
double normalizationStd
RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used t...
Definition: CICP.h:144
TICPCovarianceMethod ICP_covariance_method
The method to use for covariance estimation (Default: icpCovFiniteDifferences)
Definition: CICP.h:87
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:200
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
Definition: CICP.h:133
CICP(const TConfigParams &icpParams)
Constructor that directly set the ICP params from a given struct.
Definition: CICP.h:186
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
mrpt::poses::CPosePDF::Ptr ICP_Method_LM(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:556
bool use_kernel
Whether to use kernel_rho to smooth distances, or use distances directly (default=true) ...
Definition: CICP.h:155
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
Definition: CICP.cpp:126
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options mrpt_slam_grp.
Definition: CICP.h:19
double ransac_mahalanobisDistanceThreshold
Definition: CICP.h:140
CICP()
Constructor with the default options.
Definition: CICP.h:183
double kernel_rho
Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0...
Definition: CICP.h:152
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double minAbsStep_trans
If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters)...
Definition: CICP.h:105
bool skip_cov_calculation
Skip the computation of the covariance (saves some time) (default=false)
Definition: CICP.h:165
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A base class for any algorithm able of maps alignment.
double ALFA
The scale factor for thresholds every time convergence is achieved.
Definition: CICP.h:117
The ICP algorithm return information.
Definition: CICP.h:190
unsigned int ransac_minSetSize
Definition: CICP.h:138
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
unsigned int ransac_maxSetSize
Definition: CICP.h:138
double covariance_varPoints
This is the normalization constant that is used to scale the whole 3x3 covariance.
Definition: CICP.h:129
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
TICPCovarianceMethod
ICP covariance estimation methods, used in mrpt::slam::CICP::options.
Definition: CICP.h:27
double thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:114
mrpt::poses::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt) override
Definition: CICP.cpp:917
MRPT_FILL_ENUM(icpClassic)
double 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:158
~CICP() override=default
Destructor.
mrpt::poses::CPosePDF::Ptr ICP_Method_Classic(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
Definition: CICP.cpp:184
Covariance of a least-squares optimizer (includes data association uncertainty)
Definition: CICP.h:34
double minAbsStep_rot
If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians)...
Definition: CICP.h:109
double quality
A measure of the &#39;quality&#39; of the local minimum of the sqr.
Definition: CICP.h:205
float kernel(float x2, float rho2)
Computes: or just return the input if options.useKernel = false.
Definition: CICP.cpp:174



Page generated by Doxygen 1.8.14 for MRPT 2.0.0 Git: b38439d21 Tue Mar 31 19:58:06 2020 +0200 at mar mar 31 20:00:11 CEST 2020