Main MRPT website > C++ reference
MRPT logo

CICP.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 CICP_H
00029 #define CICP_H
00030 
00031 #include <mrpt/slam/CMetricMapsAlignmentAlgorithm.h>
00032 #include <mrpt/utils/CLoadableOptions.h>
00033 
00034 namespace mrpt
00035 {
00036         namespace slam
00037         {
00038                 using namespace poses;
00039 
00040                 /** The ICP algorithm selection, used in mrpt::slam::CICP::options.
00041                   *  For details on the algorithms refer to http://www.mrpt.org/Scan_Matching_Algorithms
00042                   */
00043                 enum TICPAlgorithm
00044                 {
00045                         icpClassic = 0,
00046                         icpLevenbergMarquardt,
00047                         icpIKF
00048                 };
00049 
00050                 /** Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps.
00051                  *
00052                  *   See CICP::AlignPDF for the entry point of the algorithm, and CICP::TConfigParams for all the parameters to the method.
00053                  *  The algorithm has been exteneded with multihypotheses-support for the correspondences, which generates a Sum-of-Gaussians (SOG)
00054                  *    PDF as output. See scanmatching::robustRigidTransformation.
00055                  *
00056                  * For further details on the method, check the wiki:
00057                  *   http://www.mrpt.org/Scan_Matching_Algorithms
00058                  *
00059                  *  For a paper explaining the used equations, see for example:
00060                  *   J. Martinez, J. Gonzalez, J. Morales, A. Mandow, A. Garcia-Cerezo,
00061                  *   "Genetic and ICP Laser Point Matching for 2D Mobile Robot Motion Estimation",
00062                  *    Journal of Field Robotics, vol. 23, no. 1, 2006. ( http://babel.isa.uma.es/~jlblanco/papers/martinez2006gil.pdf )
00063                  *
00064                  * \sa CMetricMapsAlignmentAlgorithm
00065                  */
00066                 class SLAM_IMPEXP  CICP : public CMetricMapsAlignmentAlgorithm
00067                 {
00068                 public:
00069                         /** The ICP algorithm configuration data
00070                          */
00071                         class SLAM_IMPEXP TConfigParams : public utils::CLoadableOptions
00072                         {
00073                         public:
00074                                 /** Initializer for default values:
00075                                   */
00076                                 TConfigParams();
00077 
00078                                 /** See utils::CLoadableOptions
00079                                   */
00080                                 void  loadFromConfigFile(
00081                                         const mrpt::utils::CConfigFileBase  &source,
00082                                         const std::string &section);
00083 
00084                                 /** See utils::CLoadableOptions
00085                                   */
00086                                 void  dumpToTextStream(CStream  &out) const;
00087 
00088 
00089                                 /** The algorithm to use (default: icpClassic)
00090                                   *  See http://www.mrpt.org/Scan_Matching_Algorithms for details.
00091                                   */
00092                                 TICPAlgorithm   ICP_algorithm;
00093 
00094                                 /** The usual approach: to consider only the closest correspondence for each local point (Default to true)
00095                                   */
00096                                 bool    onlyClosestCorrespondences;
00097 
00098                                 /** Apart of "onlyClosestCorrespondences=true", if this option is enabled only the closest correspondence for each reference point will be kept (default=false).
00099                                   */
00100                                 bool    onlyUniqueRobust;
00101 
00102                                 /** Maximum number of iterations to run.
00103                                   */
00104                                 unsigned int    maxIterations;
00105 
00106                                 /** Initial threshold distance for two points to become a correspondence.
00107                                   */
00108                                 float   thresholdDist,thresholdAng;
00109 
00110                                 /**     The scale factor for threshold everytime convergence is achieved.
00111                                  */
00112                                 float   ALFA;
00113 
00114                                 /** The size for threshold such that iterations will stop, since it is considered precise enough.
00115                                  */
00116                                 float   smallestThresholdDist;
00117 
00118                                 /** This is the normalization constant \f$ \sigma^2_p \f$ that is used to scale the whole 3x3 covariance.
00119                                   *  This has a default value of \f$ (0.02)^2 \f$, that is, a 2cm sigma.
00120                                   *  See the paper: ....
00121                                   */
00122                                 float   covariance_varPoints;
00123 
00124                                 /** Perform a RANSAC step after the ICP convergence, to obtain a better estimation of the pose PDF.
00125                                   */
00126                                 bool    doRANSAC;
00127 
00128                                 /** RANSAC-step options:
00129                                   * \sa CICP::robustRigidTransformation
00130                                   */
00131                                 unsigned int    ransac_minSetSize,ransac_maxSetSize,ransac_nSimulations;
00132 
00133                                 /** RANSAC-step options:
00134                                   * \sa CICP::robustRigidTransformation
00135                                   */
00136                                 float                   ransac_mahalanobisDistanceThreshold;
00137 
00138                                 /** RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used to compute covariances in the SoG)
00139                                   * \sa CICP::robustRigidTransformation
00140                                   */
00141                                 float                   normalizationStd;
00142 
00143                                 /** RANSAC-step options:
00144                                   * \sa CICP::robustRigidTransformation
00145                                   */
00146                                 bool                    ransac_fuseByCorrsMatch;
00147 
00148                                 /** RANSAC-step options:
00149                                   * \sa CICP::robustRigidTransformation
00150                                   */
00151                                 float                   ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi;
00152 
00153                                 /** Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0.07m).
00154                                   */
00155                                 float                   kernel_rho;
00156 
00157                                 /** Whether to use kernel_rho to smooth distances, or use distances directly (default=true)
00158                                   */
00159                                 bool                    use_kernel;
00160 
00161                                 /** The size of the perturbance in x & y used to estimate the Jacobians of the square error (in LM & IKF methods, default=0.05).
00162                                   */
00163                                 float                   Axy_aprox_derivatives;
00164 
00165                                 /** The initial value of the lambda parameter in the LM method (default=1e-4).
00166                                   */
00167                                 float                   LM_initial_lambda;
00168 
00169                                 /** Skip the computation of the covariance (saves some time) (default=false) */
00170                                 bool                    skip_cov_calculation;
00171 
00172                         };
00173 
00174                         TConfigParams  options; //!< The options employed by the ICP align.
00175 
00176 
00177                         /** Constructor with the default options */
00178                         CICP() : options() { }
00179                         /** Constructor that directly set the ICP params from a given struct \sa options */
00180                         CICP(const TConfigParams &icpParams) : options(icpParams) { }
00181 
00182                         virtual ~CICP() { }     //!< Destructor
00183 
00184 
00185                         /** The ICP algorithm return information.
00186                          */
00187                         struct SLAM_IMPEXP TReturnInfo
00188                         {
00189                                 TReturnInfo() :
00190                                         cbSize(sizeof(TReturnInfo)),
00191                                         nIterations(0),
00192                                         goodness(0),
00193                                         quality(0)
00194                                 {
00195                                 }
00196 
00197                                 /** Size in bytes of this struct: Must be set correctly before using it */
00198                                 unsigned int    cbSize;
00199 
00200                                 /** The number of executed iterations until convergence.
00201                                  */
00202                                 unsigned short  nIterations;
00203 
00204                                 /** A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
00205                                  */
00206                                 float                   goodness;
00207 
00208                                 /** A measure of the 'quality' of the local minimum of the sqr. error found by the method.
00209                                   * Higher values are better. Low values will be found in ill-conditioned situations (e.g. a corridor).
00210                                   */
00211                                 float                   quality;
00212                         };
00213 
00214                         /** An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/point map.
00215                          *
00216                          *  This method computes the PDF of the displacement (relative pose) between
00217                          *   two maps: <b>the relative pose of m2 with respect to m1</b>. This pose
00218                          *   is returned as a PDF rather than a single value.
00219                          *
00220                          *  \note This method can be configurated with "CICP::options"
00221                          *  \note The output PDF is a CPosePDFGaussian if "doRANSAC=false", or a CPosePDFSOG otherwise.
00222                          *
00223                          * \param m1                    [IN] The first map (CAN BE A mrpt::poses::CPointsMap derived class or a mrpt::slam::COccupancyGrid2D class)
00224                          * \param m2                    [IN] The second map. (MUST BE A mrpt::poses::CPointsMap derived class)The pose of this map respect to m1 is to be estimated.
00225                          * \param initialEstimationPDF  [IN] An initial gross estimation for the displacement.
00226                          * \param runningTime   [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
00227                          * \param info                  [OUT] A pointer to a CICP::TReturnInfo, or NULL if it isn't needed.
00228                          *
00229                          * \return A smart pointer to the output estimated pose PDF.
00230                          *
00231                          * \sa CMetricMapsAlignmentAlgorithm, CICP::options, CICP::TReturnInfo
00232                          */
00233                         CPosePDFPtr AlignPDF(
00234                                         const CMetricMap                *m1,
00235                                         const CMetricMap                *m2,
00236                                         const CPosePDFGaussian  &initialEstimationPDF,
00237                                         float                                   *runningTime = NULL,
00238                                         void                                    *info = NULL );
00239 
00240                         /** Align a pair of metric maps, aligning the full 6D pose.
00241                          *   The meaning of some parameters are implementation dependant,
00242                          *    so look at the derived classes for more details.
00243                          *  The goal is to find a PDF for the pose displacement between
00244                          *   maps, that is, <b>the pose of m2 relative to m1</b>. This pose
00245                          *   is returned as a PDF rather than a single value.
00246                          *
00247                          *  \note This method can be configurated with a "options" structure in the implementation classes.
00248                          *
00249                          * \param m1                    [IN] The first map (MUST BE A COccupancyGridMap2D  derived class)
00250                          * \param m2                    [IN] The second map. (MUST BE A CPointsMap derived class) The pose of this map respect to m1 is to be estimated.
00251                          * \param initialEstimationPDF  [IN] An initial gross estimation for the displacement.
00252                          * \param runningTime   [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
00253                          * \param info                  [OUT] See derived classes for details, or NULL if it isn't needed.
00254                          *
00255                          * \return A smart pointer to the output estimated pose PDF.
00256                          * \sa CICP
00257                          */
00258                         CPose3DPDFPtr Align3DPDF(
00259                                         const CMetricMap                *m1,
00260                                         const CMetricMap                *m2,
00261                                         const CPose3DPDFGaussian        &initialEstimationPDF,
00262                                         float                                   *runningTime = NULL,
00263                                         void                                    *info = NULL );
00264 
00265 
00266                 protected:
00267                         /** Computes:
00268                           *  \f[ K(x^2) = \frac{x^2}{x^2+\rho^2}  \f]
00269                           *  or just return the input if options.useKernel = false.
00270                           */
00271                         float kernel(const float &x2, const float &rho2);
00272 
00273                         /** The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpClassic.
00274                           */
00275                         CPosePDFPtr ICP_Method_Classic(
00276                                         const CMetricMap                *m1,
00277                                         const CMetricMap                *m2,
00278                                         const CPosePDFGaussian  &initialEstimationPDF,
00279                                         TReturnInfo                             &outInfo );
00280 
00281                         /** The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpLevenbergMarquardt.
00282                           */
00283                         CPosePDFPtr ICP_Method_LM(
00284                                         const CMetricMap                *m1,
00285                                         const CMetricMap                *m2,
00286                                         const CPosePDFGaussian  &initialEstimationPDF,
00287                                         TReturnInfo                             &outInfo );
00288 
00289                         /** The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpIKF.
00290                           */
00291                         CPosePDFPtr ICP_Method_IKF(
00292                                         const CMetricMap                *m1,
00293                                         const CMetricMap                *m2,
00294                                         const CPosePDFGaussian  &initialEstimationPDF,
00295                                         TReturnInfo                             &outInfo );
00296 
00297                         /** The internal method implementing CICP::Align3DPDF when options.ICP_algorithm is icpClassic.
00298                           */
00299                         CPose3DPDFPtr ICP3D_Method_Classic(
00300                                         const CMetricMap                *m1,
00301                                         const CMetricMap                *m2,
00302                                         const CPose3DPDFGaussian &initialEstimationPDF,
00303                                         TReturnInfo                             &outInfo );
00304 
00305 
00306                 };
00307 
00308         } // End of namespace
00309 } // End of namespace
00310 
00311 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011