Main MRPT website > C++ reference
MRPT logo

CGridMapAligner.h

Go to the documentation of this file.
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 &section);
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:exported at Tue Jan 25 21:56:31 UTC 2011