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