Main MRPT website > C++ reference
MRPT logo
scanmatching/scan_matching.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef ScanMatching_H
29 #define ScanMatching_H
30 
31 #include <mrpt/math.h> // These 2 headers, in this order, are needed to avoid
32 #include <mrpt/poses.h> // undefined classes errors in inline constructors of mrpt::poses classes.
33 
34 #include <mrpt/utils/TMatchingPair.h>
35 
36 #include <mrpt/scanmatching/link_pragmas.h>
37 
38 namespace mrpt
39 {
40  namespace poses
41  {
42  class CPosePDFParticles;
43  class CPosePDFGaussian;
44  class CPosePDFSOG;
45  }
46 
47  /** A set of scan matching-related static functions.
48  * \sa mrpt::slam::CICP
49  * \ingroup mrpt_scanmatching_grp
50  */
51  namespace scanmatching
52  {
53  using namespace mrpt::poses;
54  using namespace mrpt::math;
55  using namespace mrpt::utils;
56 
57  /** \addtogroup mrpt_scanmatching_grp
58  * @{ */
59 
60  /** This function implements the Horn method for computing the change in pose between two coordinate systems
61  * \param[in] inPoints A vector containing the coordinates of the input points in the format:
62  * [x11 y11 z11, x12 y12 z12, x21 y21 z21, x22 y22 z22, x31 y31 z31, x32 y32 z32, ... ]
63  * where [xi1 yi1 zi1] and [xi2 yi2 zi2] represent the i-th pair of corresponding 3D points in the two coordinate systems "1" and "2"
64  * \param[out] outQuat A 7D vector containing the traslation and rotation (in a quaternion form) which indicates the change in pose of system "2" wrt "1".
65  * \param[in] forceScaleToUnity Whether or not force the scale employed to rotate the coordinate systems to one (rigid transformation)
66  *
67  * \return The computed scale of the optimal transformation (will be 1.0 for a perfectly rigid translation + rotation).
68  * \sa THornMethodOpts
69  */
70  double SCANMATCHING_IMPEXP HornMethod(
71  const vector_double &inPoints,
72  vector_double &outQuat,
73  bool forceScaleToUnity = false );
74 
75  //! \overload
76  double SCANMATCHING_IMPEXP HornMethod(
77  const vector_double &inPoints,
78  mrpt::poses::CPose3DQuat &outQuat,
79  bool forceScaleToUnity = false);
80 
81  /** This method provides the closed-form solution of absolute orientation using unit quaternions to a set of over-constrained correspondences for finding the 6D rigid transformation between two cloud of 3D points.
82  * The output 3D pose is computed using the method described in "Closed-form solution of absolute orientation using unit quaternions", BKP Horn, Journal of the Optical Society of America, 1987.
83  *
84  * \param in_correspondences The set of correspondences in TMatchingPairList form ("this" and "other").
85  * \param out_transformation The change in pose (CPose3DQuat) of the "other" reference system wrt "this" reference system which minimizes the mean-square-error between all the correspondences.
86  * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of three correspondences.
87  * \return True if there are at least three correspondences, or false otherwise, thus we cannot establish any correspondence.
88  * Implemented by FAMD, 2007. Revised in 2010.
89  * \sa robustRigidTransformation
90  */
91  bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation6D(
92  const TMatchingPairList &in_correspondences,
93  CPose3DQuat &out_transformation,
94  double &out_scale,
95  const bool forceScaleToUnity = false );
96 
97  /** This method provides the closed-form solution of absolute orientation using unit quaternions to a set of over-constrained correspondences for finding the 6D rigid transformation between two cloud of 3D points.
98  * The output 3D pose is computed using the method described in "Closed-form solution of absolute orientation using unit quaternions", BKP Horn, Journal of the Optical Society of America, 1987.
99  *
100  * \param in_correspondences The set of correspondences.
101  * \param out_transformation The change in pose (CPose3DQuat) of the "other" reference system wrt "this" reference system which minimizes the mean-square-error between all the correspondences.
102  * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
103  * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence.
104  * Implemented by FAMD, 2007. Revised in 2010
105  * \sa robustRigidTransformation
106  */
107  bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation6D(
108  const TMatchingPairList &in_correspondences,
109  CPose3D &out_transformation,
110  double &out_scale,
111  const bool forceScaleToUnity = false );
112 
113  /** This method provides the closed-form solution of absolute orientation using unit quaternions to a set of over-constrained correspondences for finding the 6D rigid transformation between two cloud of 3D points using RANSAC.
114  * The output 3D pose is computed using the method described in "Closed-form solution of absolute orientation using unit quaternions", BKP Horn, Journal of the Optical Society of America, 1987.
115  * If supplied, the output covariance matrix is computed using... TODO
116  * \todo Explain covariance!!
117  *
118  * \param in_correspondences The set of correspondences.
119  * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences.
120  * \param out_scale The estimated scale of the rigid transformation (should be very close to 1.0)
121  * \param out_inliers_idx Indexes within the "in_correspondences" list which corresponds with inliers
122  * \param ransac_minSetSize The minimum amount of points in the set
123  * \param ransac_nmaxSimulations The maximum number of iterations of the RANSAC algorithm
124  * \param ransac_maxSetSizePct The (minimum) assumed percent (0.0 - 1.0) of the input set to be considered as inliers
125  * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
126  * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence.
127  * Implemented by FAMD, 2008.
128  * \sa robustRigidTransformation
129  */
130  bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation6DRANSAC(
131  const TMatchingPairList &in_correspondences,
132  CPose3D &out_transformation,
133  double &out_scale,
134  vector_int &out_inliers_idx,
135  const unsigned int ransac_minSetSize = 5,
136  const unsigned int ransac_nmaxSimulations = 50,
137  const double ransac_maxSetSizePct = 0.7,
138  const bool forceScaleToUnity = false );
139 
140 
141  /** This method provides the basic least-square-error solution to a set of over-constrained correspondences for finding the (x,y,phi) rigid transformation between two planes.
142  * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$
143  * \param in_correspondences The set of correspondences.
144  * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences.
145  * \param out_estimateCovariance If provided (!=NULL) this will contain on return a 3x3 covariance matrix with the NORMALIZED optimal estimate uncertainty. This matrix must be multiplied by \f$\sigma^2_p\f$, the variance of matched points in \f$x\f$ and \f$y\f$ (see paper http://www.mrpt.org/Paper:Occupancy_Grid_Matching)
146  * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
147  * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence.
148  * \sa robustRigidTransformation
149  */
150  bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation(
151  TMatchingPairList &in_correspondences,
152  CPose2D &out_transformation,
153  CMatrixDouble33 *out_estimateCovariance = NULL );
154 
155  /** This method provides the basic least-square-error solution to a set of over-constrained correspondences for finding the (x,y,phi) rigid transformation between two planes.
156  * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$
157  * \param in_correspondences The set of correspondences.
158  * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences.
159  * \param out_estimateCovariance If provided (!=NULL) this will contain on return a 3x3 covariance matrix with the NORMALIZED optimal estimate uncertainty. This matrix must be multiplied by \f$\sigma^2_p\f$, the variance of matched points in \f$x\f$ and \f$y\f$ (see paper http://www.mrpt.org/Paper:Occupancy_Grid_Matching)
160  * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
161  * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence.
162  * \sa robustRigidTransformation
163  */
164  bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation(
165  TMatchingPairList &in_correspondences,
166  CPosePDFGaussian &out_transformation );
167 
168  /** This method implements a RANSAC-based robust estimation of the rigid transformation between two planes, returning a probability distribution over all the posibilities as a Sum of Gaussians.
169  * This works are follows:
170  - Repeat "ransac_nSimulations" times:
171  - Randomly pick TWO correspondences from the set "in_correspondences".
172  - Compute the associated rigid transformation.
173  - For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group:
174  - If if is compatible (ransac_mahalanobisDistanceThreshold), grow the "consensus set"
175  - If not, do not add it.
176  *
177  * For more details refer to the tutorial on <a href="http://www.mrpt.org/Scan_Matching_Algorithms">scan matching methods</a>.
178  * NOTE:
179  * - If a pointer is supplied to "out_largestSubSet", the largest consensus sub-set
180  * of correspondences will be returned there.
181  * - The parameter "normalizationStd" is the <b>standard deviation</b> (not variance) of landmarks
182  * being matched in X,Y. Used to normalize covariances returned as the SoG.
183  * - If ransac_nSimulations=0 then an adaptive algorithm is used to determine the number of iterations, such as
184  * a good model is found with a probability p=0.999, or that passed as the parameter probability_find_good_model
185  * - When using "probability_find_good_model", the minimum number of iterations can be set with "ransac_min_nSimulations".
186  *
187  * If ransac_fuseByCorrsMatch=true (the default), the weight of Gaussian modes will be increased when an exact match in the
188  * subset of correspondences for the modes is found. Otherwise, an approximate method is used as test by just looking at the
189  * resulting X,Y,PHI means (Threshold in this case are: ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi).
190  *
191  * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
192  * \sa leastSquareErrorRigidTransformation
193  */
194  void SCANMATCHING_IMPEXP robustRigidTransformation(
195  TMatchingPairList &in_correspondences,
196  poses::CPosePDFSOG &out_transformation,
197  float normalizationStd,
198  unsigned int ransac_minSetSize = 3,
199  unsigned int ransac_maxSetSize = 20,
200  float ransac_mahalanobisDistanceThreshold = 3.0f,
201  unsigned int ransac_nSimulations = 0,
202  TMatchingPairList *out_largestSubSet = NULL,
203  bool ransac_fuseByCorrsMatch = true,
204  float ransac_fuseMaxDiffXY = 0.01f,
205  float ransac_fuseMaxDiffPhi = DEG2RAD(0.1f),
206  bool ransac_algorithmForLandmarks = true,
207  double probability_find_good_model = 0.999,
208  unsigned int ransac_min_nSimulations = 1500
209  );
210 
211 
212  /** @} */ // end of grouping
213 
214  }
215 
216 } // End of namespace
217 
218 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013