MRPT  1.9.9
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-2019, 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(
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 everytime 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*/
190  struct TReturnInfo
191  {
192  TReturnInfo() = default;
193  /** The number of executed iterations until convergence */
194  unsigned short nIterations{0};
195  /** A goodness measure for the alignment, it is a [0,1] range indicator
196  * of percentage of correspondences. */
197  float goodness{0};
198  /** A measure of the 'quality' of the local minimum of the sqr. error
199  * found by the method. Higher values are better. Low values will be
200  * found in ill-conditioned situations (e.g. a corridor) */
201  float quality{0};
202  };
203 
204  /** An implementation of CMetricMapsAlignmentAlgorithm for the case of a
205  * point maps and a occupancy grid/point map.
206  *
207  * This method computes the PDF of the displacement (relative pose) between
208  * two maps: <b>the relative pose of m2 with respect to m1</b>. This pose
209  * is returned as a PDF rather than a single value.
210  *
211  * \note This method can be configurated with "CICP::options"
212  * \note The output PDF is a CPosePDFGaussian if "doRANSAC=false", or a
213  * CPosePDFSOG otherwise.
214  *
215  * \param m1 [IN] The first map (CAN BE A mrpt::poses::CPointsMap
216  * derived
217  * class or a mrpt::slam::COccupancyGrid2D class)
218  * \param m2 [IN] The second map. (MUST BE A
219  * mrpt::poses::CPointsMap
220  * derived class)The pose of this map respect to m1 is to be estimated.
221  * \param initialEstimationPDF [IN] An initial gross estimation for the
222  * displacement.
223  * \param runningTime [OUT] A pointer to a container for obtaining the
224  * algorithm running time in seconds, or nullptr if you don't need it.
225  * \param info [OUT] A pointer to a CICP::TReturnInfo, or nullptr
226  * if
227  * it
228  * isn't needed.
229  *
230  * \return A smart pointer to the output estimated pose PDF.
231  *
232  * \sa CMetricMapsAlignmentAlgorithm, CICP::options, CICP::TReturnInfo
233  */
235  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
236  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
237  float* runningTime = nullptr, void* info = nullptr) override;
238 
239  // See base class for docs
241  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
242  const mrpt::poses::CPose3DPDFGaussian& initialEstimationPDF,
243  float* runningTime = nullptr, void* info = nullptr) override;
244 
245  protected:
246  /** Computes:
247  * \f[ K(x^2) = \frac{x^2}{x^2+\rho^2} \f]
248  * or just return the input if options.useKernel = false.
249  */
250  float kernel(float x2, float rho2);
251 
253  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
254  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
255  TReturnInfo& outInfo);
257  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
258  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
259  TReturnInfo& outInfo);
261  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
262  const mrpt::poses::CPose3DPDFGaussian& initialEstimationPDF,
263  TReturnInfo& outInfo);
264 };
265 } // namespace mrpt::slam
267 using namespace mrpt::slam;
271 
273 using namespace mrpt::slam;
Use the covariance of the optimal registration, disregarding uncertainty in data association.
Definition: CICP.h:31
float quality
A measure of the &#39;quality&#39; of the local minimum of the sqr.
Definition: CICP.h:201
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...
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
mrpt::poses::CPosePDF::Ptr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr) override
An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/...
Definition: CICP.cpp:36
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
GLdouble s
Definition: glext.h:3682
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:77
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:958
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
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:197
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 ...
const GLubyte * c
Definition: glext.h:6406
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
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:553
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:194
bool use_kernel
Whether to use kernel_rho to smooth distances, or use distances directly (default=true) ...
Definition: CICP.h:155
GLsizei const GLchar ** string
Definition: glext.h:4116
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:123
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 everytime convergence is achieved.
Definition: CICP.h:117
The ICP algorithm return information.
Definition: CICP.h:190
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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_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:181
Covariance of a least-squares optimizer (includes data association uncertainty)
Definition: CICP.h:34
mrpt::poses::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr) override
The virtual method for aligning a pair of metric maps, aligning the full 6D pose. ...
Definition: CICP.cpp:914
double minAbsStep_rot
If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians)...
Definition: CICP.h:109
float kernel(float x2, float rho2)
Computes: or just return the input if options.useKernel = false.
Definition: CICP.cpp:171



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 45d659fbb Tue Dec 10 18:21:14 2019 +0100 at mar dic 10 18:30:09 CET 2019