00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2011 University of Malaga | 00007 | | 00008 | This software was written by the Machine Perception and Intelligent | 00009 | Robotics Lab, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 00026 | | 00027 +---------------------------------------------------------------------------+ */ 00028 #ifndef ScanMatching_H 00029 #define ScanMatching_H 00030 00031 #include <mrpt/math.h> // These 2 headers, in this order, are needed to avoid 00032 #include <mrpt/poses.h> // undefined classes errors in inline constructors of mrpt::poses classes. 00033 00034 #include <mrpt/utils/TMatchingPair.h> 00035 00036 #include <mrpt/scanmatching/link_pragmas.h> 00037 00038 namespace mrpt 00039 { 00040 namespace poses 00041 { 00042 class CPosePDFParticles; 00043 class CPosePDFGaussian; 00044 class CPosePDFSOG; 00045 } 00046 00047 /** A set of scan matching-related static functions. 00048 * \sa mrpt::slam::CICP 00049 */ 00050 namespace scanmatching 00051 { 00052 using namespace mrpt::poses; 00053 using namespace mrpt::math; 00054 using namespace mrpt::utils; 00055 00056 /** This function implements the Horn method for computing the change in pose between two coordinate systems 00057 * \param[in] inPoints A vector containing the coordinates of the input points in the format: 00058 * [x11 y11 z11, x12 y12 z12, x21 y21 z21, x22 y22 z22, x31 y31 z31, x32 y32 z32, ... ] 00059 * 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" 00060 * \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". 00061 * \param[in] forceScaleToUnity Whether or not force the scale employed to rotate the coordinate systems to one (rigid transformation) 00062 * 00063 * \return The computed scale of the optimal transformation (will be 1.0 for a perfectly rigid translation + rotation). 00064 * \sa THornMethodOpts 00065 */ 00066 double SCANMATCHING_IMPEXP HornMethod( 00067 const vector_double &inPoints, 00068 vector_double &outQuat, 00069 bool forceScaleToUnity = false ); 00070 00071 //! \overload 00072 double SCANMATCHING_IMPEXP HornMethod( 00073 const vector_double &inPoints, 00074 mrpt::poses::CPose3DQuat &outQuat, 00075 bool forceScaleToUnity = false); 00076 00077 /** 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. 00078 * 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. 00079 * 00080 * \param in_correspondences The set of correspondences in TMatchingPairList form ("this" and "other"). 00081 * \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. 00082 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of three correspondences. 00083 * \return True if there are at least three correspondences, or false otherwise, thus we cannot establish any correspondence. 00084 * Implemented by FAMD, 2007. Revised in 2010. 00085 * \sa robustRigidTransformation 00086 */ 00087 bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation6D( 00088 const TMatchingPairList &in_correspondences, 00089 CPose3DQuat &out_transformation, 00090 double &out_scale, 00091 const bool forceScaleToUnity = false ); 00092 00093 /** 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. 00094 * 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. 00095 * 00096 * \param in_correspondences The set of correspondences. 00097 * \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. 00098 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00099 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00100 * Implemented by FAMD, 2007. Revised in 2010 00101 * \sa robustRigidTransformation 00102 */ 00103 inline bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation6D( 00104 const TMatchingPairList &in_correspondences, 00105 CPose3D &out_transformation, 00106 double &out_scale, 00107 const bool forceScaleToUnity = false ) 00108 { 00109 MRPT_START; 00110 00111 CPose3DQuat qAux(UNINITIALIZED_QUATERNION); // Convert the CPose3D to CPose3DQuat 00112 00113 if( !scanmatching::leastSquareErrorRigidTransformation6D( in_correspondences, qAux, out_scale, forceScaleToUnity ) ) 00114 return false; 00115 out_transformation = CPose3D( qAux ); // Convert back the CPose3DQuat to CPose3D 00116 00117 return true; 00118 00119 MRPT_END; 00120 } 00121 00122 /** 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. 00123 * 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. 00124 * If supplied, the output covariance matrix is computed using... TODO 00125 * \todo Explain covariance!! 00126 * 00127 * \param in_correspondences The set of correspondences. 00128 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00129 * \param out_scale The estimated scale of the rigid transformation (should be very close to 1.0) 00130 * \param out_inliers_idx Indexes within the "in_correspondences" list which corresponds with inliers 00131 * \param ransac_minSetSize The minimum amount of points in the set 00132 * \param ransac_nmaxSimulations The maximum number of iterations of the RANSAC algorithm 00133 * \param ransac_maxSetSizePct The (minimum) assumed percent (0.0 - 1.0) of the input set to be considered as inliers 00134 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00135 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00136 * Implemented by FAMD, 2008. 00137 * \sa robustRigidTransformation 00138 */ 00139 bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation6DRANSAC( 00140 const TMatchingPairList &in_correspondences, 00141 CPose3D &out_transformation, 00142 double &out_scale, 00143 vector_int &out_inliers_idx, 00144 const unsigned int ransac_minSetSize = 5, 00145 const unsigned int ransac_nmaxSimulations = 50, 00146 const double ransac_maxSetSizePct = 0.7, 00147 const bool forceScaleToUnity = false ); 00148 00149 00150 /** 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. 00151 * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$ 00152 * \param in_correspondences The set of correspondences. 00153 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00154 * \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) 00155 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00156 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00157 * \sa robustRigidTransformation 00158 */ 00159 bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation( 00160 TMatchingPairList &in_correspondences, 00161 CPose2D &out_transformation, 00162 CMatrixDouble33 *out_estimateCovariance = NULL ); 00163 00164 /** 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. 00165 * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$ 00166 * \param in_correspondences The set of correspondences. 00167 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00168 * \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) 00169 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00170 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00171 * \sa robustRigidTransformation 00172 */ 00173 bool SCANMATCHING_IMPEXP leastSquareErrorRigidTransformation( 00174 TMatchingPairList &in_correspondences, 00175 CPosePDFGaussian &out_transformation ); 00176 00177 /** 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. 00178 * This works are follows: 00179 - Repeat "ransac_nSimulations" times: 00180 - Randomly pick TWO correspondences from the set "in_correspondences". 00181 - Compute the associated rigid transformation. 00182 - For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group: 00183 - If if is compatible (ransac_mahalanobisDistanceThreshold), grow the "consensus set" 00184 - If not, do not add it. 00185 * 00186 * For more details refer to the tutorial on <a href="http://www.mrpt.org/Scan_Matching_Algorithms">scan matching methods</a>. 00187 * NOTE: 00188 * - If a pointer is supplied to "out_largestSubSet", the largest consensus sub-set 00189 * of correspondences will be returned there. 00190 * - The parameter "normalizationStd" is the <b>standard deviation</b> (not variance) of landmarks 00191 * being matched in X,Y. Used to normalize covariances returned as the SoG. 00192 * - If ransac_nSimulations=0 then an adaptive algorithm is used to determine the number of iterations, such as 00193 * a good model is found with a probability p=0.999, or that passed as the parameter probability_find_good_model 00194 * - When using "probability_find_good_model", the minimum number of iterations can be set with "ransac_min_nSimulations". 00195 * 00196 * If ransac_fuseByCorrsMatch=true (the default), the weight of Gaussian modes will be increased when an exact match in the 00197 * subset of correspondences for the modes is found. Otherwise, an approximate method is used as test by just looking at the 00198 * resulting X,Y,PHI means (Threshold in this case are: ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi). 00199 * 00200 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00201 * \sa leastSquareErrorRigidTransformation 00202 */ 00203 void SCANMATCHING_IMPEXP robustRigidTransformation( 00204 TMatchingPairList &in_correspondences, 00205 poses::CPosePDFSOG &out_transformation, 00206 float normalizationStd, 00207 unsigned int ransac_minSetSize = 3, 00208 unsigned int ransac_maxSetSize = 20, 00209 float ransac_mahalanobisDistanceThreshold = 3.0f, 00210 unsigned int ransac_nSimulations = 0, 00211 TMatchingPairList *out_largestSubSet = NULL, 00212 bool ransac_fuseByCorrsMatch = true, 00213 float ransac_fuseMaxDiffXY = 0.01f, 00214 float ransac_fuseMaxDiffPhi = DEG2RAD(0.1f), 00215 bool ransac_algorithmForLandmarks = true, 00216 double probability_find_good_model = 0.999, 00217 unsigned int ransac_min_nSimulations = 1500 00218 ); 00219 00220 } 00221 00222 00223 #ifdef MRPT_BACKCOMPATIB_08X // For backward compatibility 00224 namespace scan_matching = scanmatching; 00225 #endif 00226 00227 } // End of namespace 00228 00229 #endif
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:46:17 UTC 2011 |