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 CGridMapAligner_H 00029 #define CGridMapAligner_H 00030 00031 #include <mrpt/slam/CMetricMapsAlignmentAlgorithm.h> 00032 #include <mrpt/slam/CLandmarksMap.h> 00033 #include <mrpt/utils/CLoadableOptions.h> 00034 #include <mrpt/poses/CPosePDFSOG.h> 00035 #include <mrpt/vision/CFeatureExtraction.h> 00036 #include <mrpt/slam/COccupancyGridMapFeatureExtractor.h> 00037 00038 namespace mrpt 00039 { 00040 namespace slam 00041 { 00042 using namespace poses; 00043 using namespace utils; 00044 00045 //class CLandmark; 00046 00047 /** A class for aligning two multi-metric maps (with an occupancy grid maps and a points map, at least) based on features extraction and matching. 00048 * The matching pose is returned as a Sum of Gaussians (poses::CPosePDFSOG). 00049 * 00050 * This method was reported in the paper: <br> 00051 * 00052 * See CGridMapAligner::Align for more instructions. 00053 * 00054 * \sa CMetricMapsAlignmentAlgorithm 00055 */ 00056 class SLAM_IMPEXP CGridMapAligner : public CMetricMapsAlignmentAlgorithm 00057 { 00058 private: 00059 /** Private member, implements one the algorithms. 00060 */ 00061 CPosePDFPtr AlignPDF_correlation( 00062 const CMetricMap *m1, 00063 const CMetricMap *m2, 00064 const CPosePDFGaussian &initialEstimationPDF, 00065 float *runningTime = NULL, 00066 void *info = NULL ); 00067 00068 /** Private member, implements both, the "robustMatch" and the newer "modifiedRANSAC" algorithms. 00069 */ 00070 CPosePDFPtr AlignPDF_robustMatch( 00071 const CMetricMap *m1, 00072 const CMetricMap *m2, 00073 const CPosePDFGaussian &initialEstimationPDF, 00074 float *runningTime = NULL, 00075 void *info = NULL ); 00076 00077 COccupancyGridMapFeatureExtractor m_grid_feat_extr; //!< Grid map features extractor 00078 public: 00079 00080 CGridMapAligner() : 00081 options() 00082 {} 00083 00084 /** The type for selecting the grid-map alignment algorithm. 00085 */ 00086 enum TAlignerMethod 00087 { 00088 amRobustMatch = 0, 00089 amCorrelation, 00090 amModifiedRANSAC 00091 }; 00092 00093 /** The ICP algorithm configuration data 00094 */ 00095 class SLAM_IMPEXP TConfigParams : public utils::CLoadableOptions 00096 { 00097 public: 00098 /** Initializer for default values: 00099 */ 00100 TConfigParams(); 00101 00102 /** See utils::CLoadableOptions 00103 */ 00104 void loadFromConfigFile( 00105 const mrpt::utils::CConfigFileBase &source, 00106 const std::string §ion); 00107 00108 /** See utils::CLoadableOptions 00109 */ 00110 void dumpToTextStream(CStream &out) const; 00111 00112 00113 TAlignerMethod methodSelection; //!< The aligner method: 00114 00115 /** The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images */ 00116 mrpt::vision::TDescriptorType feature_descriptor; 00117 00118 /** All the parameters for the feature detector. */ 00119 mrpt::vision::CFeatureExtraction::TOptions feature_detector_options; 00120 00121 /** RANSAC-step options: 00122 * \sa CICP::robustRigidTransformation 00123 */ 00124 float ransac_minSetSizeRatio; //!< The ratio of landmarks that must be inliers to accepto an hypotheses (typ: 0.20) 00125 float ransac_SOG_sigma_m; //!< The square root of the uncertainty normalization variance var_m (see papers...) 00126 00127 /** [amRobustMatch method only] RANSAC-step options: 00128 * \sa CICP::robustRigidTransformation 00129 */ 00130 float ransac_mahalanobisDistanceThreshold; 00131 00132 double ransac_chi2_quantile; //!< [amModifiedRANSAC method only] The quantile used for chi-square thresholding (default=0.99) 00133 00134 double ransac_prob_good_inliers; //!< Probability of having a good inliers (def:0,9999), used for automatic number of iterations 00135 float featsPerSquareMeter; //!< Features extraction from grid map: How many features to extract 00136 float threshold_max; //!< Correspondences are considered if their distances are below this threshold (in the range [0,1]) (default=0.15). 00137 float threshold_delta; //!< Correspondences are considered if their distances to the best match are below this threshold (in the range [0,1]) (default=0.15). 00138 00139 float min_ICP_goodness; //!< The minimum goodness (0-1) of the post-matching ICP to accept a hypothesis as good (default=0.25) 00140 double max_ICP_mahadist; //!< The maximum Mahalanobis distance between the initial and final poses in the ICP not to discard the hypothesis (default=10) 00141 double maxKLd_for_merge; //!< Maximum KL-divergence for merging modes of the SOG (default=0.9) 00142 00143 bool save_feat_coors; //!< DEBUG - Dump all feature correspondences in a directory "grid_feats" 00144 bool debug_show_corrs; //!< DEBUG - Show graphs with the details of each feature correspondences 00145 bool debug_save_map_pairs; //!< DEBUG - Save the pair of maps with all the pairings. 00146 00147 } options; 00148 00149 /** The ICP algorithm return information. 00150 */ 00151 struct SLAM_IMPEXP TReturnInfo 00152 { 00153 /** Initialization 00154 */ 00155 TReturnInfo() : 00156 cbSize(sizeof(TReturnInfo)), 00157 goodness(0), 00158 noRobustEstimation() 00159 { 00160 } 00161 00162 size_t cbSize; //!< Size of the structure, do not change, it's set automatically 00163 00164 /** A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences. 00165 */ 00166 float goodness; 00167 00168 /** The "brute" estimation from using all the available correspondences, provided just for comparison purposes (it is not the robust estimation, available as the result of the Align method). 00169 */ 00170 CPose2D noRobustEstimation; 00171 00172 /** The different SOG densities at different steps of the algorithm: 00173 * - sog1 : Directly from the matching of features 00174 * - sog2 : Merged of sog1 00175 * - sog3 : sog2 refined with ICP 00176 * 00177 * - The final sog is the merge of sog3. 00178 * 00179 */ 00180 CPosePDFSOGPtr sog1,sog2,sog3; 00181 00182 /** The landmarks of each map (the indices of these landmarks correspond to those in "correspondences") */ 00183 CLandmarksMapPtr landmarks_map1, landmarks_map2; 00184 00185 /** All the found correspondences (not consistent) */ 00186 mrpt::utils::TMatchingPairList correspondences; 00187 00188 struct TPairPlusDistance 00189 { 00190 TPairPlusDistance(size_t i1, size_t i2, float d) : 00191 idx_this(i1), idx_other(i2), dist(d) 00192 { } 00193 size_t idx_this,idx_other; 00194 float dist; 00195 }; 00196 00197 std::vector<TPairPlusDistance> correspondences_dists_maha; //!< Mahalanobis distance for each potential correspondence 00198 00199 std::vector<double> icp_goodness_all_sog_modes; //!< The ICP goodness of all potential SOG modes at the stage "sog2", thus before the removing of "bad" ICP matches. 00200 }; 00201 00202 /** The method for aligning a pair of 2D points map. 00203 * The meaning of some parameters are implementation dependant, 00204 * so look for derived classes for instructions. 00205 * The target is to find a PDF for the pose displacement between 00206 * maps, thus <b>the pose of m2 relative 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 "options" 00210 * 00211 * \param m1 [IN] The first map (Must be a mrpt::slam::CMultiMetricMap class) 00212 * \param m2 [IN] The second map (Must be a mrpt::slam::CMultiMetricMap class) 00213 * \param initialEstimationPDF [IN] (IGNORED IN THIS ALGORITHM!) 00214 * \param runningTime [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it. 00215 * \param info [OUT] A pointer to a CAlignerFromMotionDraws::TReturnInfo struct, or NULL if result information are not required. 00216 * 00217 * \note The returned PDF depends on the selected alignment method: 00218 * - "amRobustMatch" --> A "poses::CPosePDFSOG" object. 00219 * - "amCorrelation" --> A "poses::CPosePDFGrid" object. 00220 * 00221 * \return A smart pointer to the output estimated pose PDF. 00222 * \sa CPointsMapAlignmentAlgorithm, options 00223 */ 00224 CPosePDFPtr AlignPDF( 00225 const CMetricMap *m1, 00226 const CMetricMap *m2, 00227 const CPosePDFGaussian &initialEstimationPDF, 00228 float *runningTime = NULL, 00229 00230 void *info = NULL ); 00231 00232 00233 /** The virtual method for aligning 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 THROW_EXCEPTION("Align3D method not applicable to gridmap-aligner"); 00259 } 00260 00261 }; 00262 00263 } // End of namespace 00264 } // End of namespace 00265 00266 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011 |