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