Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
85  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
86 
87  TAlignerMethod methodSelection; //!< The aligner method:
88 
89  /** The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images */
91 
92  /** All the parameters for the feature detector. */
94 
95  /** RANSAC-step options:
96  * \sa CICP::robustRigidTransformation
97  */
98  float ransac_minSetSizeRatio; //!< The ratio of landmarks that must be inliers to accepto an hypotheses (typ: 0.20)
99  float ransac_SOG_sigma_m; //!< The square root of the uncertainty normalization variance var_m (see papers...)
100 
101  /** [amRobustMatch method only] RANSAC-step options:
102  * \sa CICP::robustRigidTransformation
103  */
105 
106  double ransac_chi2_quantile; //!< [amModifiedRANSAC method only] The quantile used for chi-square thresholding (default=0.99)
107 
108  double ransac_prob_good_inliers; //!< Probability of having a good inliers (def:0,9999), used for automatic number of iterations
109  float featsPerSquareMeter; //!< Features extraction from grid map: How many features to extract
110  float threshold_max; //!< Correspondences are considered if their distances are below this threshold (in the range [0,1]) (default=0.15).
111  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).
112 
113  float min_ICP_goodness; //!< The minimum goodness (0-1) of the post-matching ICP to accept a hypothesis as good (default=0.25)
114  double max_ICP_mahadist; //!< The maximum Mahalanobis distance between the initial and final poses in the ICP not to discard the hypothesis (default=10)
115  double maxKLd_for_merge; //!< Maximum KL-divergence for merging modes of the SOG (default=0.9)
116 
117  bool save_feat_coors; //!< DEBUG - Dump all feature correspondences in a directory "grid_feats"
118  bool debug_show_corrs; //!< DEBUG - Show graphs with the details of each feature correspondences
119  bool debug_save_map_pairs; //!< DEBUG - Save the pair of maps with all the pairings.
120 
121  } options;
122 
123  /** The ICP algorithm return information.
124  */
126  {
127  /** Initialization
128  */
130  cbSize(sizeof(TReturnInfo)),
131  goodness(0),
132  noRobustEstimation()
133  {
134  }
135 
136  size_t cbSize; //!< Size of the structure, do not change, it's set automatically
137 
138  /** A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
139  */
140  float goodness;
141 
142  /** 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).
143  */
145 
146  /** The different SOG densities at different steps of the algorithm:
147  * - sog1 : Directly from the matching of features
148  * - sog2 : Merged of sog1
149  * - sog3 : sog2 refined with ICP
150  *
151  * - The final sog is the merge of sog3.
152  *
153  */
154  mrpt::poses::CPosePDFSOGPtr sog1,sog2,sog3;
155 
156  /** The landmarks of each map (the indices of these landmarks correspond to those in "correspondences") */
158 
159  /** All the found correspondences (not consistent) */
161 
163  {
164  TPairPlusDistance(size_t i1, size_t i2, float d) :
165  idx_this(i1), idx_other(i2), dist(d)
166  { }
167  size_t idx_this,idx_other;
168  float dist;
169  };
170 
171  std::vector<TPairPlusDistance> correspondences_dists_maha; //!< Mahalanobis distance for each potential correspondence
172 
173  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.
174  };
175 
176  /** The method for aligning a pair of 2D points map.
177  * The meaning of some parameters are implementation dependant,
178  * so look for derived classes for instructions.
179  * The target is to find a PDF for the pose displacement between
180  * maps, thus <b>the pose of m2 relative to m1</b>. This pose
181  * is returned as a PDF rather than a single value.
182  *
183  * NOTE: This method can be configurated with "options"
184  *
185  * \param m1 [IN] The first map (Must be a mrpt::maps::CMultiMetricMap class)
186  * \param m2 [IN] The second map (Must be a mrpt::maps::CMultiMetricMap class)
187  * \param initialEstimationPDF [IN] (IGNORED IN THIS ALGORITHM!)
188  * \param runningTime [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
189  * \param info [OUT] A pointer to a CAlignerFromMotionDraws::TReturnInfo struct, or NULL if result information are not required.
190  *
191  * \note The returned PDF depends on the selected alignment method:
192  * - "amRobustMatch" --> A "poses::CPosePDFSOG" object.
193  * - "amCorrelation" --> A "poses::CPosePDFGrid" object.
194  *
195  * \return A smart pointer to the output estimated pose PDF.
196  * \sa CPointsMapAlignmentAlgorithm, options
197  */
198  mrpt::poses::CPosePDFPtr AlignPDF(
199  const mrpt::maps::CMetricMap *m1,
200  const mrpt::maps::CMetricMap *m2,
201  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
202  float *runningTime = NULL,
203 
204  void *info = NULL );
205 
206 
207  /** Not applicable in this class, will launch an exception. */
208  mrpt::poses::CPose3DPDFPtr Align3DPDF(
209  const mrpt::maps::CMetricMap *m1,
210  const mrpt::maps::CMetricMap *m2,
211  const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF,
212  float *runningTime = NULL,
213  void *info = NULL );
214 
215  };
216 
217  } // End of namespace
218 
219  // Specializations MUST occur at the same namespace:
220  namespace utils
221  {
222  template <>
224  {
226  static void fill(bimap<enum_t,std::string> &m_map)
227  {
228  m_map.insert(slam::CGridMapAligner::amRobustMatch, "amRobustMatch");
229  m_map.insert(slam::CGridMapAligner::amCorrelation, "amCorrelation");
230  m_map.insert(slam::CGridMapAligner::amModifiedRANSAC, "amModifiedRANSAC");
231  }
232  };
233  } // End of namespace
234 
235 } // End of namespace
236 
237 #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...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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.13 for MRPT 1.4.0 SVN: at Wed Mar 15 00:43:31 UTC 2017