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/utils/TMatchingPair.h> 00032 #include <mrpt/poses/CPose3DQuat.h> 00033 00034 #include <mrpt/base/link_pragmas.h> 00035 00036 namespace mrpt 00037 { 00038 namespace poses 00039 { 00040 class CPosePDFParticles; 00041 class CPosePDFGaussian; 00042 class CPosePDFSOG; 00043 } 00044 00045 /** A set of scan matching-related static functions. 00046 * \sa mrpt::slam::CICP 00047 */ 00048 namespace scan_matching 00049 { 00050 using namespace mrpt::poses; 00051 using namespace mrpt::slam; 00052 using namespace mrpt::math; 00053 using namespace mrpt::utils; 00054 00055 /** This struct contains the options considered for the Horn method. 00056 * \param scales A vector which will contain the computed output scale of the Horn method. 00057 * \param forceScaleToUnity Whether or not force the scale employed to rotate the coordinate systems to one (rigid transformation). 00058 */ 00059 struct BASE_IMPEXP THornMethodOpts 00060 { 00061 vector_double scales; // The vector of the computed scales 00062 bool forceScaleToUnity; // Whether or not force the scale employed to rotate the coordinate systems to one (rigid transformation) 00063 00064 THornMethodOpts() : forceScaleToUnity( false ) {} 00065 00066 }; // end struct THornMethodOpts 00067 00068 /** This function implements the Horn method for computing the change in pose between two coordinate systems 00069 * \param inVector A vector containing the coordinates of the input points in the format: 00070 [x11 y11 z11, x12 y12 z12, x21 y21 z21, x22 y22 z22, x31 y31 z31, x32 y32 z32, ... ] 00071 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" 00072 * \param outVector A 7D vector containing the traslation and rotation (in a quaternion form) which indicates the change in pose of system "2" wrt "1". 00073 * \param opts The options for the method. 00074 * \sa THornMethodOpts 00075 */ 00076 void BASE_IMPEXP HornMethod( 00077 const vector_double &inVector, 00078 vector_double &outVector, // The output vector 00079 THornMethodOpts &opts ); 00080 00081 /** 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. 00082 * 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. 00083 * 00084 * \param in_correspondences The set of correspondences in TMatchingPairList form ("this" and "other"). 00085 * \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. 00086 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of three correspondences. 00087 * \return True if there are at least three correspondences, or false otherwise, thus we cannot establish any correspondence. 00088 * Implemented by FAMD, 2007. Revised in 2010. 00089 * \sa robustRigidTransformation 00090 */ 00091 bool BASE_IMPEXP leastSquareErrorRigidTransformation6D( 00092 const TMatchingPairList &in_correspondences, 00093 CPose3DQuat &out_transformation, 00094 double &out_scale, 00095 const bool forceScaleToUnity = false ); 00096 00097 /** 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. 00098 * 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. 00099 * 00100 * \param in_correspondences The set of correspondences. 00101 * \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. 00102 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00103 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00104 * Implemented by FAMD, 2007. Revised in 2010 00105 * \sa robustRigidTransformation 00106 */ 00107 inline bool BASE_IMPEXP leastSquareErrorRigidTransformation6D( 00108 const TMatchingPairList &in_correspondences, 00109 CPose3D &out_transformation, 00110 double &out_scale, 00111 const bool forceScaleToUnity = false ) 00112 { 00113 MRPT_START; 00114 00115 CPose3DQuat qAux(UNINITIALIZED_QUATERNION); // Convert the CPose3D to CPose3DQuat 00116 00117 if( !scan_matching::leastSquareErrorRigidTransformation6D( in_correspondences, qAux, out_scale, forceScaleToUnity ) ) 00118 return false; 00119 out_transformation = CPose3D( qAux ); // Convert back the CPose3DQuat to CPose3D 00120 00121 return true; 00122 00123 MRPT_END; 00124 } 00125 00126 /** 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. 00127 * 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. 00128 * If supplied, the output covariance matrix is computed using... TODO 00129 * \todo Explain covariance!! 00130 * 00131 * \param in_correspondences The set of correspondences. 00132 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00133 * \param out_scale The estimated scale of the rigid transformation (should be very close to 1.0) 00134 * \param out_inliers_idx Indexes within the "in_correspondences" list which corresponds with inliers 00135 * \param ransac_minSetSize The minimum amount of points in the set 00136 * \param ransac_nmaxSimulations The maximum number of iterations of the RANSAC algorithm 00137 * \param ransac_maxSetSizePct The (minimum) assumed percent (0.0 - 1.0) of the input set to be considered as inliers 00138 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00139 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00140 * Implemented by FAMD, 2008. 00141 * \sa robustRigidTransformation 00142 */ 00143 bool BASE_IMPEXP leastSquareErrorRigidTransformation6DRANSAC( 00144 const TMatchingPairList &in_correspondences, 00145 CPose3D &out_transformation, 00146 double &out_scale, 00147 vector_int &out_inliers_idx, 00148 const unsigned int ransac_minSetSize = 5, 00149 const unsigned int ransac_nmaxSimulations = 50, 00150 const double ransac_maxSetSizePct = 0.7, 00151 const bool forceScaleToUnity = false ); 00152 00153 00154 /** 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. 00155 * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$ 00156 * \param in_correspondences The set of correspondences. 00157 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00158 * \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) 00159 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00160 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00161 * \sa robustRigidTransformation 00162 */ 00163 bool BASE_IMPEXP leastSquareErrorRigidTransformation( 00164 TMatchingPairList &in_correspondences, 00165 CPose2D &out_transformation, 00166 CMatrixDouble33 *out_estimateCovariance = NULL ); 00167 00168 /** 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. 00169 * The optimal transformation q fulfills: \f$ point_this = q \oplus point_other \f$ 00170 * \param in_correspondences The set of correspondences. 00171 * \param out_transformation The pose that minimizes the mean-square-error between all the correspondences. 00172 * \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) 00173 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00174 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. 00175 * \sa robustRigidTransformation 00176 */ 00177 bool BASE_IMPEXP leastSquareErrorRigidTransformation( 00178 TMatchingPairList &in_correspondences, 00179 CPosePDFGaussian &out_transformation ); 00180 00181 /** 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. 00182 * This works are follows: 00183 - Repeat "ransac_nSimulations" times: 00184 - Randomly pick TWO correspondences from the set "in_correspondences". 00185 - Compute the associated rigid transformation. 00186 - For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group: 00187 - If if is compatible (ransac_mahalanobisDistanceThreshold), grow the "consensus set" 00188 - If not, do not add it. 00189 * 00190 * For more details refer to the tutorial on <a href="http://www.mrpt.org/Scan_Matching_Algorithms">scan matching methods</a>. 00191 * NOTE: 00192 * - If a pointer is supplied to "out_largestSubSet", the largest consensus sub-set 00193 * of correspondences will be returned there. 00194 * - The parameter "normalizationStd" is the <b>standard deviation</b> (not variance) of landmarks 00195 * being matched in X,Y. Used to normalize covariances returned as the SoG. 00196 * - If ransac_nSimulations=0 then an adaptive algorithm is used to determine the number of iterations, such as 00197 * a good model is found with a probability p=0.999, or that passed as the parameter probability_find_good_model 00198 * - When using "probability_find_good_model", the minimum number of iterations can be set with "ransac_min_nSimulations". 00199 * 00200 * If ransac_fuseByCorrsMatch=true (the default), the weight of Gaussian modes will be increased when an exact match in the 00201 * subset of correspondences for the modes is found. Otherwise, an approximate method is used as test by just looking at the 00202 * resulting X,Y,PHI means (Threshold in this case are: ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi). 00203 * 00204 * \exception Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences. 00205 * \sa leastSquareErrorRigidTransformation 00206 */ 00207 void BASE_IMPEXP robustRigidTransformation( 00208 TMatchingPairList &in_correspondences, 00209 poses::CPosePDFSOG &out_transformation, 00210 float normalizationStd, 00211 unsigned int ransac_minSetSize = 3, 00212 unsigned int ransac_maxSetSize = 20, 00213 float ransac_mahalanobisDistanceThreshold = 3.0f, 00214 unsigned int ransac_nSimulations = 0, 00215 TMatchingPairList *out_largestSubSet = NULL, 00216 bool ransac_fuseByCorrsMatch = true, 00217 float ransac_fuseMaxDiffXY = 0.01f, 00218 float ransac_fuseMaxDiffPhi = DEG2RAD(0.1f), 00219 bool ransac_algorithmForLandmarks = true, 00220 double probability_find_good_model = 0.999, 00221 unsigned int ransac_min_nSimulations = 1500 00222 ); 00223 00224 } 00225 00226 } // End of namespace 00227 00228 #endif
| Page generated by Doxygen 1.6.1 for MRPT 0.9.0 SVN: at Mon Jun 7 06:47:58 UTC 2010 |
