Main MRPT website > C++ reference
MRPT logo
CGridMapAligner.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CGridMapAligner_H
29 #define CGridMapAligner_H
30 
31 #include <mrpt/slam/CMetricMapsAlignmentAlgorithm.h>
32 #include <mrpt/slam/CLandmarksMap.h>
33 #include <mrpt/utils/CLoadableOptions.h>
34 #include <mrpt/poses/CPosePDFSOG.h>
35 #include <mrpt/vision/CFeatureExtraction.h>
36 #include <mrpt/slam/COccupancyGridMapFeatureExtractor.h>
37 
38 namespace mrpt
39 {
40  namespace slam
41  {
42  using namespace poses;
43  using namespace utils;
44 
45  /** 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.
46  * The matching pose is returned as a Sum of Gaussians (poses::CPosePDFSOG).
47  *
48  * This method was reported in the paper: <br>
49  *
50  * See CGridMapAligner::Align for more instructions.
51  *
52  * \sa CMetricMapsAlignmentAlgorithm
53  * \ingroup mrpt_slam_grp
54  */
55  class SLAM_IMPEXP CGridMapAligner : public CMetricMapsAlignmentAlgorithm
56  {
57  private:
58  /** Private member, implements one the algorithms.
59  */
60  CPosePDFPtr AlignPDF_correlation(
61  const CMetricMap *m1,
62  const CMetricMap *m2,
63  const CPosePDFGaussian &initialEstimationPDF,
64  float *runningTime = NULL,
65  void *info = NULL );
66 
67  /** Private member, implements both, the "robustMatch" and the newer "modifiedRANSAC" algorithms.
68  */
69  CPosePDFPtr AlignPDF_robustMatch(
70  const CMetricMap *m1,
71  const CMetricMap *m2,
72  const CPosePDFGaussian &initialEstimationPDF,
73  float *runningTime = NULL,
74  void *info = NULL );
75 
76  COccupancyGridMapFeatureExtractor m_grid_feat_extr; //!< Grid map features extractor
77  public:
78 
79  CGridMapAligner() :
80  options()
81  {}
82 
83  /** The type for selecting the grid-map alignment algorithm.
84  */
85  enum TAlignerMethod
86  {
87  amRobustMatch = 0,
88  amCorrelation,
89  amModifiedRANSAC
90  };
91 
92  /** The ICP algorithm configuration data
93  */
94  class SLAM_IMPEXP TConfigParams : public utils::CLoadableOptions
95  {
96  public:
97  /** Initializer for default values:
98  */
99  TConfigParams();
100 
101  /** See utils::CLoadableOptions
102  */
103  void loadFromConfigFile(
104  const mrpt::utils::CConfigFileBase &source,
105  const std::string &section);
106 
107  /** See utils::CLoadableOptions
108  */
109  void dumpToTextStream(CStream &out) const;
110 
111 
112  TAlignerMethod methodSelection; //!< The aligner method:
113 
114  /** The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images */
115  mrpt::vision::TDescriptorType feature_descriptor;
116 
117  /** All the parameters for the feature detector. */
118  mrpt::vision::CFeatureExtraction::TOptions feature_detector_options;
119 
120  /** RANSAC-step options:
121  * \sa CICP::robustRigidTransformation
122  */
123  float ransac_minSetSizeRatio; //!< The ratio of landmarks that must be inliers to accepto an hypotheses (typ: 0.20)
124  float ransac_SOG_sigma_m; //!< The square root of the uncertainty normalization variance var_m (see papers...)
125 
126  /** [amRobustMatch method only] RANSAC-step options:
127  * \sa CICP::robustRigidTransformation
128  */
129  float ransac_mahalanobisDistanceThreshold;
130 
131  double ransac_chi2_quantile; //!< [amModifiedRANSAC method only] The quantile used for chi-square thresholding (default=0.99)
132 
133  double ransac_prob_good_inliers; //!< Probability of having a good inliers (def:0,9999), used for automatic number of iterations
134  float featsPerSquareMeter; //!< Features extraction from grid map: How many features to extract
135  float threshold_max; //!< Correspondences are considered if their distances are below this threshold (in the range [0,1]) (default=0.15).
136  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).
137 
138  float min_ICP_goodness; //!< The minimum goodness (0-1) of the post-matching ICP to accept a hypothesis as good (default=0.25)
139  double max_ICP_mahadist; //!< The maximum Mahalanobis distance between the initial and final poses in the ICP not to discard the hypothesis (default=10)
140  double maxKLd_for_merge; //!< Maximum KL-divergence for merging modes of the SOG (default=0.9)
141 
142  bool save_feat_coors; //!< DEBUG - Dump all feature correspondences in a directory "grid_feats"
143  bool debug_show_corrs; //!< DEBUG - Show graphs with the details of each feature correspondences
144  bool debug_save_map_pairs; //!< DEBUG - Save the pair of maps with all the pairings.
145 
146  } options;
147 
148  /** The ICP algorithm return information.
149  */
150  struct SLAM_IMPEXP TReturnInfo
151  {
152  /** Initialization
153  */
154  TReturnInfo() :
155  cbSize(sizeof(TReturnInfo)),
156  goodness(0),
157  noRobustEstimation()
158  {
159  }
160 
161  size_t cbSize; //!< Size of the structure, do not change, it's set automatically
162 
163  /** A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
164  */
165  float goodness;
166 
167  /** 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).
168  */
169  CPose2D noRobustEstimation;
170 
171  /** The different SOG densities at different steps of the algorithm:
172  * - sog1 : Directly from the matching of features
173  * - sog2 : Merged of sog1
174  * - sog3 : sog2 refined with ICP
175  *
176  * - The final sog is the merge of sog3.
177  *
178  */
179  CPosePDFSOGPtr sog1,sog2,sog3;
180 
181  /** The landmarks of each map (the indices of these landmarks correspond to those in "correspondences") */
182  CLandmarksMapPtr landmarks_map1, landmarks_map2;
183 
184  /** All the found correspondences (not consistent) */
185  mrpt::utils::TMatchingPairList correspondences;
186 
187  struct TPairPlusDistance
188  {
189  TPairPlusDistance(size_t i1, size_t i2, float d) :
190  idx_this(i1), idx_other(i2), dist(d)
191  { }
192  size_t idx_this,idx_other;
193  float dist;
194  };
195 
196  std::vector<TPairPlusDistance> correspondences_dists_maha; //!< Mahalanobis distance for each potential correspondence
197 
198  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.
199  };
200 
201  /** The method for aligning a pair of 2D points map.
202  * The meaning of some parameters are implementation dependant,
203  * so look for derived classes for instructions.
204  * The target is to find a PDF for the pose displacement between
205  * maps, thus <b>the pose of m2 relative to m1</b>. This pose
206  * is returned as a PDF rather than a single value.
207  *
208  * NOTE: This method can be configurated with "options"
209  *
210  * \param m1 [IN] The first map (Must be a mrpt::slam::CMultiMetricMap class)
211  * \param m2 [IN] The second map (Must be a mrpt::slam::CMultiMetricMap class)
212  * \param initialEstimationPDF [IN] (IGNORED IN THIS ALGORITHM!)
213  * \param runningTime [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
214  * \param info [OUT] A pointer to a CAlignerFromMotionDraws::TReturnInfo struct, or NULL if result information are not required.
215  *
216  * \note The returned PDF depends on the selected alignment method:
217  * - "amRobustMatch" --> A "poses::CPosePDFSOG" object.
218  * - "amCorrelation" --> A "poses::CPosePDFGrid" object.
219  *
220  * \return A smart pointer to the output estimated pose PDF.
221  * \sa CPointsMapAlignmentAlgorithm, options
222  */
223  CPosePDFPtr AlignPDF(
224  const CMetricMap *m1,
225  const CMetricMap *m2,
226  const CPosePDFGaussian &initialEstimationPDF,
227  float *runningTime = NULL,
228 
229  void *info = NULL );
230 
231 
232  /** The virtual method for aligning a pair of metric maps, aligning the full 6D pose.
233  * The meaning of some parameters are implementation dependant,
234  * so look at the derived classes for more details.
235  * The goal is to find a PDF for the pose displacement between
236  * maps, that is, <b>the pose of m2 relative to m1</b>. This pose
237  * is returned as a PDF rather than a single value.
238  *
239  * \note This method can be configurated with a "options" structure in the implementation classes.
240  *
241  * \param m1 [IN] The first map (MUST BE A COccupancyGridMap2D derived class)
242  * \param m2 [IN] The second map. (MUST BE A CPointsMap derived class) The pose of this map respect to m1 is to be estimated.
243  * \param initialEstimationPDF [IN] An initial gross estimation for the displacement.
244  * \param runningTime [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
245  * \param info [OUT] See derived classes for details, or NULL if it isn't needed.
246  *
247  * \return A smart pointer to the output estimated pose PDF.
248  * \sa CICP
249  */
250  CPose3DPDFPtr Align3DPDF(
251  const CMetricMap *m1,
252  const CMetricMap *m2,
253  const CPose3DPDFGaussian &initialEstimationPDF,
254  float *runningTime = NULL,
255  void *info = NULL )
256  {
257  THROW_EXCEPTION("Align3D method not applicable to gridmap-aligner");
258  }
259 
260  };
261 
262  } // End of namespace
263 } // End of namespace
264 
265 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013