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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019