Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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 
14 #include <mrpt/utils/TEnumType.h>
15 
16 namespace mrpt
17 {
18  namespace slam
19  {
20  /** The ICP algorithm selection, used in mrpt::slam::CICP::options.
21  * For details on the algorithms refer to http://www.mrpt.org/Scan_Matching_Algorithms
22  * \ingroup mrpt_slam_grp
23  */
25  {
29  };
30 
31  /** Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a point map wrt a grid map.
32  *
33  * CICP::AlignPDF() or CICP::Align() are the two main entry points of the algorithm.
34  *
35  * To choose among existing ICP algorithms or customizing their parameters, see CICP::TConfigParams and the member \a options.
36  *
37  * There exists an extension of the original ICP algorithm that provides multihypotheses-support for the correspondences, and which generates a Sum-of-Gaussians (SOG)
38  * PDF as output. See scanmatching::robustRigidTransformation.
39  *
40  * For further details on the implemented methods, check the web:
41  * http://www.mrpt.org/Iterative_Closest_Point_(ICP)_and_other_matching_algorithms
42  *
43  * For a paper explaining some of the basic equations, see for example:
44  * J. Martinez, J. Gonzalez, J. Morales, A. Mandow, A. Garcia-Cerezo,
45  * "Mobile robot motion estimation by 2D scan matching with genetic and iterative closest point algorithms",
46  * Journal of Field Robotics, vol. 23, no. 1, pp. 21-34, 2006. ( http://babel.isa.uma.es/~jlblanco/papers/martinez2006gil.pdf )
47  *
48  * \sa CMetricMapsAlignmentAlgorithm
49  * \ingroup mrpt_slam_grp
50  */
52  {
53  public:
54  /** The ICP algorithm configuration data
55  */
57  {
58  public:
59  TConfigParams(); //!< Initializer for default values:
60 
61  void loadFromConfigFile(
62  const mrpt::utils::CConfigFileBase &source,
63  const std::string &section); //!< See utils::CLoadableOptions
64 
65  void dumpToTextStream(mrpt::utils::CStream &out) const; //!<See utils::CLoadableOptions
66 
67 
68  /** The algorithm to use (default: icpClassic)
69  * See http://www.mrpt.org/Scan_Matching_Algorithms for details.
70  */
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  /** @name Termination criteria
77  @{ */
78  unsigned int maxIterations; //!< Maximum number of iterations to run.
79  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)
80  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)
81  /** @} */
82 
83  float thresholdDist,thresholdAng; //!< Initial threshold distance for two points to become a correspondence.
84  float ALFA; //!< The scale factor for threshold everytime convergence is achieved.
85  float smallestThresholdDist; //!< The size for threshold such that iterations will stop, since it is considered precise enough.
86 
87  /** This is the normalization constant \f$ \sigma^2_p \f$ that is used to scale the whole 3x3 covariance.
88  * This has a default value of \f$ (0.02)^2 \f$, that is, a 2cm sigma.
89  * See the paper: ....
90  */
92 
93  bool doRANSAC; //!< Perform a RANSAC step after the ICP convergence, to obtain a better estimation of the pose PDF.
94 
95  /** RANSAC-step options:
96  * \sa CICP::robustRigidTransformation
97  */
98  unsigned int ransac_minSetSize,ransac_maxSetSize,ransac_nSimulations;
99 
100  /** RANSAC-step options:
101  * \sa CICP::robustRigidTransformation
102  */
104 
105  /** RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used to compute covariances in the SoG)
106  * \sa CICP::robustRigidTransformation
107  */
109 
110  /** RANSAC-step options:
111  * \sa CICP::robustRigidTransformation
112  */
114 
115  /** RANSAC-step options:
116  * \sa CICP::robustRigidTransformation
117  */
118  float ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi;
119 
120  /** Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0.07m). */
121  float kernel_rho;
122 
123  /** Whether to use kernel_rho to smooth distances, or use distances directly (default=true) */
125 
126  /** The size of the perturbance in x & y used to estimate the Jacobians of the square error (in LM & IKF methods, default=0.05).*/
128 
129  /** The initial value of the lambda parameter in the LM method (default=1e-4). */
131 
132  /** Skip the computation of the covariance (saves some time) (default=false) */
134 
135  /** Skip the (sometimes) expensive evaluation of the term 'quality' at ICP output (Default=true) */
137 
138  /** 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
139  * 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,
140  * the most expensive step in ICP.
141  */
143 
144  };
145 
146  TConfigParams options; //!< The options employed by the ICP align.
147 
148 
149  /** Constructor with the default options */
150  CICP() : options() { }
151  /** Constructor that directly set the ICP params from a given struct \sa options */
152  CICP(const TConfigParams &icpParams) : options(icpParams) { }
153 
154  virtual ~CICP() { } //!< Destructor
155 
156 
157  /** The ICP algorithm return information.
158  */
160  {
162  cbSize(sizeof(TReturnInfo)),
163  nIterations(0),
164  goodness(0),
165  quality(0)
166  {
167  }
168 
169  /** Size in bytes of this struct: Must be set correctly before using it */
170  unsigned int cbSize;
171 
172  /** The number of executed iterations until convergence.
173  */
174  unsigned short nIterations;
175 
176  /** A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
177  */
178  float goodness;
179 
180  /** A measure of the 'quality' of the local minimum of the sqr. error found by the method.
181  * Higher values are better. Low values will be found in ill-conditioned situations (e.g. a corridor).
182  */
183  float quality;
184  };
185 
186  /** An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/point map.
187  *
188  * This method computes the PDF of the displacement (relative pose) between
189  * two maps: <b>the relative pose of m2 with respect to m1</b>. This pose
190  * is returned as a PDF rather than a single value.
191  *
192  * \note This method can be configurated with "CICP::options"
193  * \note The output PDF is a CPosePDFGaussian if "doRANSAC=false", or a CPosePDFSOG otherwise.
194  *
195  * \param m1 [IN] The first map (CAN BE A mrpt::poses::CPointsMap derived class or a mrpt::slam::COccupancyGrid2D class)
196  * \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.
197  * \param initialEstimationPDF [IN] An initial gross estimation for the displacement.
198  * \param runningTime [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
199  * \param info [OUT] A pointer to a CICP::TReturnInfo, or NULL if it isn't needed.
200  *
201  * \return A smart pointer to the output estimated pose PDF.
202  *
203  * \sa CMetricMapsAlignmentAlgorithm, CICP::options, CICP::TReturnInfo
204  */
205  mrpt::poses::CPosePDFPtr AlignPDF(
206  const mrpt::maps::CMetricMap *m1,
207  const mrpt::maps::CMetricMap *m2,
208  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
209  float *runningTime = NULL,
210  void *info = NULL );
211 
212  /** Align a pair of metric maps, aligning the full 6D pose.
213  * The meaning of some parameters are implementation dependant,
214  * so look at the derived classes for more details.
215  * The goal is to find a PDF for the pose displacement between
216  * maps, that is, <b>the pose of m2 relative to m1</b>. This pose
217  * is returned as a PDF rather than a single value.
218  *
219  * \note This method can be configurated with a "options" structure in the implementation classes.
220  *
221  * \param m1 [IN] The first map (MUST BE A COccupancyGridMap2D derived class)
222  * \param m2 [IN] The second map. (MUST BE A CPointsMap derived class) The pose of this map respect to m1 is to be estimated.
223  * \param initialEstimationPDF [IN] An initial gross estimation for the displacement.
224  * \param runningTime [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
225  * \param info [OUT] See derived classes for details, or NULL if it isn't needed.
226  *
227  * \return A smart pointer to the output estimated pose PDF.
228  * \sa CICP
229  */
230  mrpt::poses::CPose3DPDFPtr Align3DPDF(
231  const mrpt::maps::CMetricMap *m1,
232  const mrpt::maps::CMetricMap *m2,
233  const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF,
234  float *runningTime = NULL,
235  void *info = NULL );
236 
237 
238  protected:
239  /** Computes:
240  * \f[ K(x^2) = \frac{x^2}{x^2+\rho^2} \f]
241  * or just return the input if options.useKernel = false.
242  */
243  float kernel(const float &x2, const float &rho2);
244 
245  /** The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpClassic.
246  */
247  mrpt::poses::CPosePDFPtr ICP_Method_Classic(
248  const mrpt::maps::CMetricMap *m1,
249  const mrpt::maps::CMetricMap *m2,
250  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
251  TReturnInfo &outInfo );
252 
253  /** The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpLevenbergMarquardt.
254  */
255  mrpt::poses::CPosePDFPtr ICP_Method_LM(
256  const mrpt::maps::CMetricMap *m1,
257  const mrpt::maps::CMetricMap *m2,
258  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
259  TReturnInfo &outInfo );
260 
261  /** The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpIKF.
262  */
263  mrpt::poses::CPosePDFPtr ICP_Method_IKF(
264  const mrpt::maps::CMetricMap *m1,
265  const mrpt::maps::CMetricMap *m2,
266  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
267  TReturnInfo &outInfo );
268 
269  /** The internal method implementing CICP::Align3DPDF when options.ICP_algorithm is icpClassic.
270  */
271  mrpt::poses::CPose3DPDFPtr ICP3D_Method_Classic(
272  const mrpt::maps::CMetricMap *m1,
273  const mrpt::maps::CMetricMap *m2,
274  const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF,
275  TReturnInfo &outInfo );
276 
277 
278  };
279 
280  } // End of namespace
281 
282  // Specializations MUST occur at the same namespace:
283  namespace utils
284  {
285  template <>
287  {
289  static void fill(bimap<enum_t,std::string> &m_map)
290  {
291  m_map.insert(slam::icpClassic, "icpClassic");
292  m_map.insert(slam::icpLevenbergMarquardt, "icpLevenbergMarquardt");
293  m_map.insert(slam::icpIKF, "icpIKF");
294  }
295  };
296  } // End of namespace
297 
298 } // End of namespace
299 
300 #endif
unsigned int cbSize
Size in bytes of this struct: Must be set correctly before using it.
Definition: CICP.h:170
float quality
A measure of the &#39;quality&#39; of the local minimum of the sqr.
Definition: CICP.h:183
The ICP algorithm configuration data.
Definition: CICP.h:56
float ransac_mahalanobisDistanceThreshold
RANSAC-step options:
Definition: CICP.h:103
TICPAlgorithm ICP_algorithm
The algorithm to use (default: icpClassic) See http://www.mrpt.org/Scan_Matching_Algorithms for detai...
Definition: CICP.h:71
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:51
static void fill(bimap< enum_t, std::string > &m_map)
Definition: CICP.h:289
bool skip_quality_calculation
Skip the (sometimes) expensive evaluation of the term &#39;quality&#39; at ICP output (Default=true) ...
Definition: CICP.h:136
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
unsigned int ransac_nSimulations
Definition: CICP.h:98
float ALFA
The scale factor for threshold everytime convergence is achieved.
Definition: CICP.h:84
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:146
float kernel_rho
Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0...
Definition: CICP.h:121
float minAbsStep_rot
If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians)...
Definition: CICP.h:80
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:142
This class allows loading and storing values and vectors of different types from a configuration text...
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:178
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
bool ransac_fuseByCorrsMatch
RANSAC-step options:
Definition: CICP.h:113
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:78
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:28
float covariance_varPoints
This is the normalization constant that is used to scale the whole 3x3 covariance.
Definition: CICP.h:91
bool doRANSAC
Perform a RANSAC step after the ICP convergence, to obtain a better estimation of the pose PDF...
Definition: CICP.h:93
CICP(const TConfigParams &icpParams)
Constructor that directly set the ICP params from a given struct.
Definition: CICP.h:152
TICPAlgorithm
The ICP algorithm selection, used in mrpt::slam::CICP::options.
Definition: CICP.h:24
bool onlyClosestCorrespondences
The usual approach: to consider only the closest correspondence for each local point (Default to true...
Definition: CICP.h:73
unsigned short nIterations
The number of executed iterations until convergence.
Definition: CICP.h:174
bool use_kernel
Whether to use kernel_rho to smooth distances, or use distances directly (default=true) ...
Definition: CICP.h:124
CICP()
Constructor with the default options.
Definition: CICP.h:150
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
bool skip_cov_calculation
Skip the computation of the covariance (saves some time) (default=false)
Definition: CICP.h:133
Declares a virtual base class for all metric maps storage classes.
A base class for any algorithm able of maps alignment.
float smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
Definition: CICP.h:85
The ICP algorithm return information.
Definition: CICP.h:159
float ransac_fuseMaxDiffXY
RANSAC-step options:
Definition: CICP.h:118
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
virtual ~CICP()
Destructor.
Definition: CICP.h:154
float LM_initial_lambda
The initial value of the lambda parameter in the LM method (default=1e-4).
Definition: CICP.h:130
float normalizationStd
RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used t...
Definition: CICP.h:108
float minAbsStep_trans
If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters)...
Definition: CICP.h:79
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
float Axy_aprox_derivatives
The size of the perturbance in x & y used to estimate the Jacobians of the square error (in LM & IKF ...
Definition: CICP.h:127
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...



Page generated by Doxygen 1.8.11 for MRPT 1.3.2 SVN: at Wed May 25 02:34:21 UTC 2016