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