Main MRPT website > C++ reference
MRPT logo

scan_matching.h

Go to the documentation of this file.
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.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011