00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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/slam/CMetricMap.h> 00032 00033 namespace mrpt 00034 { 00035 namespace poses 00036 { 00037 class CPosePDFParticles; 00038 class CPosePDFGaussian; 00039 class CPosePDFSOG; 00040 } 00041 00042 /** A set of scan matching-related static functions. 00043 * \sa mrpt::slam::CICP 00044 */ 00045 namespace scan_matching 00046 { 00047 using namespace mrpt::poses; 00048 using namespace mrpt::slam; 00049 using namespace mrpt::math; 00050 using namespace mrpt::utils; 00051 00052 /** 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. 00053 * 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. 00054 * If supplied, the output covariance matrix is computed using... TODO 00055 * \todo Explain covariance!! 00056 * 00057 * \param in_correspondences The set of correspondences. 00058 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00059 * \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....) 00060 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00061 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00062 * Implemented by FAMD, 2007. 00063 * \sa robustRigidTransformation 00064 */ 00065 bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation6D( 00066 const CMetricMap::TMatchingPairList &in_correspondences, 00067 CPose3D &out_transformation, 00068 double &out_scale, 00069 CMatrixDouble *in_rotationMatrix = NULL, 00070 CMatrixDouble *out_estimateCovariance = NULL, 00071 bool forceScaleToUnity = false ); 00072 00073 /** 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. 00074 * 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. 00075 * If supplied, the output covariance matrix is computed using... TODO 00076 * \todo Explain covariance!! 00077 * 00078 * \param in_correspondences The set of correspondences. 00079 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00080 * \param out_scale The estimated scale of the rigid transformation (should be very close to 1.0) 00081 * \param out_inliers_idx Indexes within the "in_correspondences" list which corresponds with inliers 00082 * \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....) 00083 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00084 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00085 * Implemented by FAMD, 2008. 00086 * \sa robustRigidTransformation 00087 */ 00088 bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation6DRANSAC( 00089 const CMetricMap::TMatchingPairList &in_correspondences, 00090 CPose3D &out_transformation, 00091 double &out_scale, 00092 vector_int &out_inliers_idx, 00093 CMatrixD *in_rotationMatrix = NULL, 00094 CMatrixD *out_estimateCovariance = NULL, 00095 bool forceScaleToUnity = false ); 00096 00097 00098 /** 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. 00099 * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$ 00100 * \param in_correspondences The set of correspondences. 00101 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00102 * \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://babel.isa.uma.es/mrpt/index.php/Paper:Occupancy_Grid_Matching) 00103 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00104 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00105 * \sa robustRigidTransformation 00106 */ 00107 bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation( 00108 CMetricMap::TMatchingPairList &in_correspondences, 00109 CPose2D &out_transformation, 00110 CMatrixDouble33 *out_estimateCovariance = NULL ); 00111 00112 /** 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. 00113 * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$ 00114 * \param in_correspondences The set of correspondences. 00115 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00116 * \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://babel.isa.uma.es/mrpt/index.php/Paper:Occupancy_Grid_Matching) 00117 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00118 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00119 * \sa robustRigidTransformation 00120 */ 00121 bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation( 00122 CMetricMap::TMatchingPairList &in_correspondences, 00123 CPosePDFGaussian &out_transformation ); 00124 00125 /** 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. 00126 * This works are follows: 00127 - Repeat "ransac_nSimulations" times: 00128 - Randomly pick TWO correspondences from the set "in_correspondences". 00129 - Compute the associated rigid transformation. 00130 - For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group: 00131 - If if is compatible (ransac_mahalanobisDistanceThreshold), grow the "consensus set" 00132 - If not, do not add it. 00133 * 00134 * For more details refer to the tutorial on <a href="http://babel.isa.uma.es/mrpt/index.php/Scan_Matching_Algorithms">scan matching methods</a>. 00135 * NOTE: 00136 * - If a pointer is supplied to "out_largestSubSet", the largest consensus sub-set 00137 * of correspondences will be returned there. 00138 * - The parameter "normalizationStd" is the <b>standard deviation</b> (not variance) of landmarks 00139 * being matched in X,Y. Used to normalize covariances returned as the SoG. 00140 * - If ransac_nSimulations=0 then an adaptive algorithm is used to determine the number of iterations, such as 00141 * a good model is found with a probability p=0.999, or that passed as the parameter probability_find_good_model 00142 * - When using "probability_find_good_model", the minimum number of iterations can be set with "ransac_min_nSimulations". 00143 * 00144 * If ransac_fuseByCorrsMatch=true (the default), the weight of Gaussian modes will be increased when an exact match in the 00145 * subset of correspondences for the modes is found. Otherwise, an approximate method is used as test by just looking at the 00146 * resulting X,Y,PHI means (Threshold in this case are: ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi). 00147 * 00148 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00149 * \sa leastSquareErrorRigidTransformation 00150 */ 00151 void MRPTDLLIMPEXP robustRigidTransformation( 00152 mrpt::slam::CMetricMap::TMatchingPairList &in_correspondences, 00153 poses::CPosePDFSOG &out_transformation, 00154 float normalizationStd, 00155 unsigned int ransac_minSetSize = 3, 00156 unsigned int ransac_maxSetSize = 20, 00157 float ransac_mahalanobisDistanceThreshold = 3.0f, 00158 unsigned int ransac_nSimulations = 0, 00159 mrpt::slam::CMetricMap::TMatchingPairList *out_largestSubSet = NULL, 00160 bool ransac_fuseByCorrsMatch = true, 00161 float ransac_fuseMaxDiffXY = 0.01f, 00162 float ransac_fuseMaxDiffPhi = DEG2RAD(0.1f), 00163 bool ransac_algorithmForLandmarks = true, 00164 double probability_find_good_model = 0.999, 00165 unsigned int ransac_min_nSimulations = 1500 00166 ); 00167 00168 } 00169 00170 } // End of namespace 00171 00172 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:27:43 EDT 2009 |