Main MRPT website > C++ reference for MRPT 1.3.2
CGridMapAligner.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CGridMapAligner_H
10 #define CGridMapAligner_H
11 
15 #include <mrpt/utils/TEnumType.h>
16 #include <mrpt/poses/CPosePDFSOG.h>
17 #include <mrpt/poses/poses_frwds.h>
20 
21 namespace mrpt
22 {
23  namespace slam
24  {
25  /** 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.
26  * The matching pose is returned as a Sum of Gaussians (poses::CPosePDFSOG).
27  *
28  * This class can use three methods (see options.methodSelection):
29  * - amCorrelation: "Brute-force" correlation of the two maps over a 2D+orientation grid of possible 2D poses.
30  * - amRobustMatch: Detection of features + RANSAC matching
31  * - amModifiedRANSAC: Detection of features + modified multi-hypothesis RANSAC matching as described in was reported in the paper http://www.mrpt.org/Paper%3AOccupancy_Grid_Matching
32  *
33  * See CGridMapAligner::Align for more instructions.
34  *
35  * \sa CMetricMapsAlignmentAlgorithm
36  * \ingroup mrpt_slam_grp
37  */
39  {
40  private:
41  /** Private member, implements one the algorithms.
42  */
43  mrpt::poses::CPosePDFPtr AlignPDF_correlation(
44  const mrpt::maps::CMetricMap *m1,
45  const mrpt::maps::CMetricMap *m2,
46  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
47  float *runningTime = NULL,
48  void *info = NULL );
49 
50  /** Private member, implements both, the "robustMatch" and the newer "modifiedRANSAC" algorithms.
51  */
52  mrpt::poses::CPosePDFPtr AlignPDF_robustMatch(
53  const mrpt::maps::CMetricMap *m1,
54  const mrpt::maps::CMetricMap *m2,
55  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
56  float *runningTime = NULL,
57  void *info = NULL );
58 
59  COccupancyGridMapFeatureExtractor m_grid_feat_extr; //!< Grid map features extractor
60  public:
61 
63  options()
64  {}
65 
66  /** The type for selecting the grid-map alignment algorithm.
67  */
69  {
70  amRobustMatch = 0,
72  amModifiedRANSAC
73  };
74 
75  /** The ICP algorithm configuration data
76  */
78  {
79  public:
80  /** Initializer for default values:
81  */
82  TConfigParams();
83 
84  /** See utils::CLoadableOptions
85  */
86  void loadFromConfigFile(
87  const mrpt::utils::CConfigFileBase &source,
88  const std::string &section);
89 
90  /** See utils::CLoadableOptions
91  */
92  void dumpToTextStream(mrpt::utils::CStream &out) const;
93 
94 
95  TAlignerMethod methodSelection; //!< The aligner method:
96 
97  /** The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images */
99 
100  /** All the parameters for the feature detector. */
102 
103  /** RANSAC-step options:
104  * \sa CICP::robustRigidTransformation
105  */
106  float ransac_minSetSizeRatio; //!< The ratio of landmarks that must be inliers to accepto an hypotheses (typ: 0.20)
107  float ransac_SOG_sigma_m; //!< The square root of the uncertainty normalization variance var_m (see papers...)
108 
109  /** [amRobustMatch method only] RANSAC-step options:
110  * \sa CICP::robustRigidTransformation
111  */
113 
114  double ransac_chi2_quantile; //!< [amModifiedRANSAC method only] The quantile used for chi-square thresholding (default=0.99)
115 
116  double ransac_prob_good_inliers; //!< Probability of having a good inliers (def:0,9999), used for automatic number of iterations
117  float featsPerSquareMeter; //!< Features extraction from grid map: How many features to extract
118  float threshold_max; //!< Correspondences are considered if their distances are below this threshold (in the range [0,1]) (default=0.15).
119  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).
120 
121  float min_ICP_goodness; //!< The minimum goodness (0-1) of the post-matching ICP to accept a hypothesis as good (default=0.25)
122  double max_ICP_mahadist; //!< The maximum Mahalanobis distance between the initial and final poses in the ICP not to discard the hypothesis (default=10)
123  double maxKLd_for_merge; //!< Maximum KL-divergence for merging modes of the SOG (default=0.9)
124 
125  bool save_feat_coors; //!< DEBUG - Dump all feature correspondences in a directory "grid_feats"
126  bool debug_show_corrs; //!< DEBUG - Show graphs with the details of each feature correspondences
127  bool debug_save_map_pairs; //!< DEBUG - Save the pair of maps with all the pairings.
128 
129  } options;
130 
131  /** The ICP algorithm return information.
132  */
134  {
135  /** Initialization
136  */
138  cbSize(sizeof(TReturnInfo)),
139  goodness(0),
140  noRobustEstimation()
141  {
142  }
143 
144  size_t cbSize; //!< Size of the structure, do not change, it's set automatically
145 
146  /** A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
147  */
148  float goodness;
149 
150  /** 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).
151  */
153 
154  /** The different SOG densities at different steps of the algorithm:
155  * - sog1 : Directly from the matching of features
156  * - sog2 : Merged of sog1
157  * - sog3 : sog2 refined with ICP
158  *
159  * - The final sog is the merge of sog3.
160  *
161  */
162  mrpt::poses::CPosePDFSOGPtr sog1,sog2,sog3;
163 
164  /** The landmarks of each map (the indices of these landmarks correspond to those in "correspondences") */
166 
167  /** All the found correspondences (not consistent) */
169 
171  {
172  TPairPlusDistance(size_t i1, size_t i2, float d) :
173  idx_this(i1), idx_other(i2), dist(d)
174  { }
175  size_t idx_this,idx_other;
176  float dist;
177  };
178 
179  std::vector<TPairPlusDistance> correspondences_dists_maha; //!< Mahalanobis distance for each potential correspondence
180 
181  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.
182  };
183 
184  /** The method for aligning a pair of 2D points map.
185  * The meaning of some parameters are implementation dependant,
186  * so look for derived classes for instructions.
187  * The target is to find a PDF for the pose displacement between
188  * maps, thus <b>the pose of m2 relative to m1</b>. This pose
189  * is returned as a PDF rather than a single value.
190  *
191  * NOTE: This method can be configurated with "options"
192  *
193  * \param m1 [IN] The first map (Must be a mrpt::maps::CMultiMetricMap class)
194  * \param m2 [IN] The second map (Must be a mrpt::maps::CMultiMetricMap class)
195  * \param initialEstimationPDF [IN] (IGNORED IN THIS ALGORITHM!)
196  * \param runningTime [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
197  * \param info [OUT] A pointer to a CAlignerFromMotionDraws::TReturnInfo struct, or NULL if result information are not required.
198  *
199  * \note The returned PDF depends on the selected alignment method:
200  * - "amRobustMatch" --> A "poses::CPosePDFSOG" object.
201  * - "amCorrelation" --> A "poses::CPosePDFGrid" object.
202  *
203  * \return A smart pointer to the output estimated pose PDF.
204  * \sa CPointsMapAlignmentAlgorithm, options
205  */
206  mrpt::poses::CPosePDFPtr AlignPDF(
207  const mrpt::maps::CMetricMap *m1,
208  const mrpt::maps::CMetricMap *m2,
209  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
210  float *runningTime = NULL,
211 
212  void *info = NULL );
213 
214 
215  /** Not applicable in this class, will launch an exception. */
216  mrpt::poses::CPose3DPDFPtr Align3DPDF(
217  const mrpt::maps::CMetricMap *m1,
218  const mrpt::maps::CMetricMap *m2,
219  const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF,
220  float *runningTime = NULL,
221  void *info = NULL );
222 
223  };
224 
225  } // End of namespace
226 
227  // Specializations MUST occur at the same namespace:
228  namespace utils
229  {
230  template <>
232  {
234  static void fill(bimap<enum_t,std::string> &m_map)
235  {
236  m_map.insert(slam::CGridMapAligner::amRobustMatch, "amRobustMatch");
237  m_map.insert(slam::CGridMapAligner::amCorrelation, "amCorrelation");
238  m_map.insert(slam::CGridMapAligner::amModifiedRANSAC, "amModifiedRANSAC");
239  }
240  };
241  } // End of namespace
242 
243 } // End of namespace
244 
245 #endif
double ransac_chi2_quantile
[amModifiedRANSAC method only] The quantile used for chi-square thresholding (default=0.99)
std::vector< TPairPlusDistance > correspondences_dists_maha
Mahalanobis distance for each potential correspondence.
double maxKLd_for_merge
Maximum KL-divergence for merging modes of the SOG (default=0.9)
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map...
bool debug_show_corrs
DEBUG - Show graphs with the details of each feature correspondences.
TAlignerMethod
The type for selecting the grid-map alignment algorithm.
The ICP algorithm configuration data.
The set of parameters for all the detectors & descriptor algorithms.
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
The ICP algorithm return information.
This class allows loading and storing values and vectors of different types from a configuration text...
size_t cbSize
Size of the structure, do not change, it&#39;s set automatically.
mrpt::maps::CLandmarksMapPtr landmarks_map2
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
mrpt::vision::TDescriptorType feature_descriptor
The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
bool debug_save_map_pairs
DEBUG - Save the pair of maps with all the pairings.
mrpt::maps::CLandmarksMapPtr CLandmarksMapPtr
Backward compatible typedef.
bool save_feat_coors
DEBUG - Dump all feature correspondences in a directory "grid_feats".
A list of TMatchingPair.
Definition: TMatchingPair.h:66
mrpt::vision::CFeatureExtraction::TOptions feature_detector_options
All the parameters for the feature detector.
COccupancyGridMapFeatureExtractor m_grid_feat_extr
Grid map features extractor.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:28
mrpt::utils::TMatchingPairList correspondences
All the found correspondences (not consistent)
float ransac_minSetSizeRatio
RANSAC-step options:
float ransac_SOG_sigma_m
The square root of the uncertainty normalization variance var_m (see papers...)
float threshold_delta
Correspondences are considered if their distances to the best match are below this threshold (in the ...
TDescriptorType
The bitwise OR combination of values of TDescriptorType are used in CFeatureExtraction::computeDescri...
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
float ransac_mahalanobisDistanceThreshold
[amRobustMatch method only] RANSAC-step options:
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" IC...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double ransac_prob_good_inliers
Probability of having a good inliers (def:0,9999), used for automatic number of iterations.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose.
Definition: CPose2D.h:36
A base class for any algorithm able of maps alignment.
static void fill(bimap< enum_t, std::string > &m_map)
double max_ICP_mahadist
The maximum Mahalanobis distance between the initial and final poses in the ICP not to discard the hy...
float featsPerSquareMeter
Features extraction from grid map: How many features to extract.
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
TAlignerMethod methodSelection
The aligner method:
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
A class for detecting features from occupancy grid maps.
float threshold_max
Correspondences are considered if their distances are below this threshold (in the range [0...
mrpt::poses::CPose2D noRobustEstimation
The "brute" estimation from using all the available correspondences, provided just for comparison pur...
float min_ICP_goodness
The minimum goodness (0-1) of the post-matching ICP to accept a hypothesis as good (default=0...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...



Page generated by Doxygen 1.8.12 for MRPT 1.3.2 SVN: at Thu Nov 10 13:22:34 UTC 2016