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