00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 * \ingroup mrpt_scanmatching_grp 00050 */ 00051 namespace scanmatching 00052 { 00053 using namespace mrpt::poses; 00054 using namespace mrpt::math; 00055 using namespace mrpt::utils; 00056 00057 /** \addtogroup mrpt_scanmatching_grp 00058 * @{ */ 00059 00060 /** This function implements the Horn method for computing the change in pose between two coordinate systems 00061 * \param[in] inPoints A vector containing the coordinates of the input points in the format: 00062 * [x11 y11 z11, x12 y12 z12, x21 y21 z21, x22 y22 z22, x31 y31 z31, x32 y32 z32, ... ] 00063 * 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" 00064 * \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". 00065 * \param[in] forceScaleToUnity Whether or not force the scale employed to rotate the coordinate systems to one (rigid transformation) 00066 * 00067 * \return The computed scale of the optimal transformation (will be 1.0 for a perfectly rigid translation + rotation). 00068 * \sa THornMethodOpts 00069 */ 00070 double SCANMATCHING_IMPEXP HornMethod( 00071 const vector_double &inPoints, 00072 vector_double &outQuat, 00073 bool forceScaleToUnity = false ); 00074 00075 //! \overload 00076 double SCANMATCHING_IMPEXP HornMethod( 00077 const vector_double &inPoints, 00078 mrpt::poses::CPose3DQuat &outQuat, 00079 bool forceScaleToUnity = false); 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 SCANMATCHING_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 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( !scanmatching::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 SCANMATCHING_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 SCANMATCHING_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 SCANMATCHING_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 SCANMATCHING_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 /** @} */ // end of grouping 00226 00227 } 00228 00229 } // End of namespace 00230 00231 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |