00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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 const bool forceScaleToUnity = false ); 00070 00071 /** 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. 00072 * 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. 00073 * If supplied, the output covariance matrix is computed using... TODO 00074 * \todo Explain covariance!! 00075 * 00076 * \param in_correspondences The set of correspondences. 00077 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00078 * \param out_scale The estimated scale of the rigid transformation (should be very close to 1.0) 00079 * \param out_inliers_idx Indexes within the "in_correspondences" list which corresponds with inliers 00080 * \param ransac_minSetSize The minimum amount of points in the set 00081 * \param ransac_nmaxSimulations The maximum number of iterations of the RANSAC algorithm 00082 * \param ransac_maxSetSizePct The (minimum) assumed percent (0.0 - 1.0) of the input set to be considered as inliers 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 const unsigned int ransac_minSetSize = 5, 00094 const unsigned int ransac_nmaxSimulations = 50, 00095 const double ransac_maxSetSizePct = 0.7, 00096 const bool forceScaleToUnity = false ); 00097 00098 00099 /** 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. 00100 * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$ 00101 * \param in_correspondences The set of correspondences. 00102 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00103 * \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) 00104 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00105 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00106 * \sa robustRigidTransformation 00107 */ 00108 bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation( 00109 CMetricMap::TMatchingPairList &in_correspondences, 00110 CPose2D &out_transformation, 00111 CMatrixDouble33 *out_estimateCovariance = NULL ); 00112 00113 /** 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. 00114 * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$ 00115 * \param in_correspondences The set of correspondences. 00116 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00117 * \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) 00118 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00119 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00120 * \sa robustRigidTransformation 00121 */ 00122 bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation( 00123 CMetricMap::TMatchingPairList &in_correspondences, 00124 CPosePDFGaussian &out_transformation ); 00125 00126 /** 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. 00127 * This works are follows: 00128 - Repeat "ransac_nSimulations" times: 00129 - Randomly pick TWO correspondences from the set "in_correspondences". 00130 - Compute the associated rigid transformation. 00131 - For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group: 00132 - If if is compatible (ransac_mahalanobisDistanceThreshold), grow the "consensus set" 00133 - If not, do not add it. 00134 * 00135 * 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>. 00136 * NOTE: 00137 * - If a pointer is supplied to "out_largestSubSet", the largest consensus sub-set 00138 * of correspondences will be returned there. 00139 * - The parameter "normalizationStd" is the <b>standard deviation</b> (not variance) of landmarks 00140 * being matched in X,Y. Used to normalize covariances returned as the SoG. 00141 * - If ransac_nSimulations=0 then an adaptive algorithm is used to determine the number of iterations, such as 00142 * a good model is found with a probability p=0.999, or that passed as the parameter probability_find_good_model 00143 * - When using "probability_find_good_model", the minimum number of iterations can be set with "ransac_min_nSimulations". 00144 * 00145 * If ransac_fuseByCorrsMatch=true (the default), the weight of Gaussian modes will be increased when an exact match in the 00146 * subset of correspondences for the modes is found. Otherwise, an approximate method is used as test by just looking at the 00147 * resulting X,Y,PHI means (Threshold in this case are: ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi). 00148 * 00149 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00150 * \sa leastSquareErrorRigidTransformation 00151 */ 00152 void MRPTDLLIMPEXP robustRigidTransformation( 00153 mrpt::slam::CMetricMap::TMatchingPairList &in_correspondences, 00154 poses::CPosePDFSOG &out_transformation, 00155 float normalizationStd, 00156 unsigned int ransac_minSetSize = 3, 00157 unsigned int ransac_maxSetSize = 20, 00158 float ransac_mahalanobisDistanceThreshold = 3.0f, 00159 unsigned int ransac_nSimulations = 0, 00160 mrpt::slam::CMetricMap::TMatchingPairList *out_largestSubSet = NULL, 00161 bool ransac_fuseByCorrsMatch = true, 00162 float ransac_fuseMaxDiffXY = 0.01f, 00163 float ransac_fuseMaxDiffPhi = DEG2RAD(0.1f), 00164 bool ransac_algorithmForLandmarks = true, 00165 double probability_find_good_model = 0.999, 00166 unsigned int ransac_min_nSimulations = 1500 00167 ); 00168 00169 } 00170 00171 } // End of namespace 00172 00173 #endif
| Page generated by Doxygen 1.6.2 for MRPT 0.8.1 SVN:exported at Mon Feb 15 22:01:07 UTC 2010 |