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 §ion); 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.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |