MRPT  1.9.9
CGridMapAligner.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
14 #include <mrpt/poses/CPosePDFSOG.h>
15 #include <mrpt/poses/poses_frwds.h>
20 
21 namespace mrpt::slam
22 {
23 /** A class for aligning two multi-metric maps (with an occupancy grid maps and
24  * a points map, at least) based on features extraction and matching.
25  * The matching pose is returned as a Sum of Gaussians (poses::CPosePDFSOG).
26  *
27  * This class can use three methods (see options.methodSelection):
28  * - amCorrelation: "Brute-force" correlation of the two maps over a
29  * 2D+orientation grid of possible 2D poses.
30  * - amRobustMatch: Detection of features + RANSAC matching
31  * - amModifiedRANSAC: Detection of features + modified multi-hypothesis
32  * RANSAC matching as described in was reported in the paper
33  * https://www.mrpt.org/Paper%3AOccupancy_Grid_Matching
34  *
35  * See CGridMapAligner::Align for more instructions.
36  *
37  * \sa CMetricMapsAlignmentAlgorithm
38  * \ingroup mrpt_slam_grp
39  */
41 {
42  private:
43  /** Private member, implements one the algorithms.
44  */
47  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
48  float* runningTime = nullptr, void* info = nullptr);
49 
50  /** Private member, implements both, the "robustMatch" and the newer
51  * "modifiedRANSAC" algorithms.
52  */
55  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
56  float* runningTime = nullptr, void* info = nullptr);
57 
58  /** Grid map features extractor */
60 
61  public:
63  /** The type for selecting the grid-map alignment algorithm.
64  */
66  {
70  };
71 
72  /** The ICP algorithm configuration data
73  */
75  {
76  public:
77  /** Initializer for default values:
78  */
79  TConfigParams();
80 
81  void loadFromConfigFile(
82  const mrpt::config::CConfigFileBase& source,
83  const std::string& section) override; // See base docs
84  void dumpToTextStream(
85  std::ostream& out) const override; // See base docs
86 
87  /** The aligner method: */
89 
90  /** The feature descriptor to use: 0=detector already has descriptor, 1=
91  * SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images */
94 
95  /** All the parameters for the feature detector. */
97 
98  /** RANSAC-step options:
99  * \sa CICP::robustRigidTransformation
100  */
101  /** The ratio of landmarks that must be inliers to accepto an hypotheses
102  * (typ: 0.20) */
104  /** The square root of the uncertainty normalization variance var_m (see
105  * papers...) */
106  float ransac_SOG_sigma_m{0.10f};
107 
108  /** [amRobustMatch method only] RANSAC-step options:
109  * \sa CICP::robustRigidTransformation
110  */
112 
113  /** [amModifiedRANSAC method only] The quantile used for chi-square
114  * thresholding (default=0.99) */
115  double ransac_chi2_quantile{0.99};
116 
117  /** Probability of having a good inliers (def:0,9999), used for
118  * automatic number of iterations */
119  double ransac_prob_good_inliers{0.9999};
120  /** Features extraction from grid map: How many features to extract */
121  float featsPerSquareMeter{0.015f};
122  /** Correspondences are considered if their distances are below this
123  * threshold (in the range [0,1]) (default=0.15). */
124  float threshold_max{0.15f};
125  /** Correspondences are considered if their distances to the best match
126  * are below this threshold (in the range [0,1]) (default=0.15). */
127  float threshold_delta{0.10f};
128 
129  /** The minimum goodness (0-1) of the post-matching ICP to accept a
130  * hypothesis as good (default=0.30) */
131  float min_ICP_goodness{0.30f};
132  /** The maximum Mahalanobis distance between the initial and final poses
133  * in the ICP not to discard the hypothesis (default=10) */
134  double max_ICP_mahadist{10.0};
135  /** Maximum KL-divergence for merging modes of the SOG (default=0.9) */
136  double maxKLd_for_merge{0.9};
137 
138  /** DEBUG - Dump all feature correspondences in a directory "grid_feats"
139  */
140  bool save_feat_coors{false};
141  /** DEBUG - Show graphs with the details of each feature correspondences
142  */
143  bool debug_show_corrs{false};
144  /** DEBUG - Save the pair of maps with all the pairings. */
145  bool debug_save_map_pairs{false};
146 
147  } options;
148 
149  /** The ICP algorithm return information.
150  */
151  struct TReturnInfo
152  {
154  /** A goodness measure for the alignment, it is a [0,1] range indicator
155  * of percentage of correspondences.
156  */
157  float goodness{0};
158 
159  /** The "brute" estimation from using all the available correspondences,
160  * provided just for comparison purposes (it is not the robust
161  * estimation, available as the result of the Align method).
162  */
164 
165  /** The different SOG densities at different steps of the algorithm:
166  * - sog1 : Directly from the matching of features
167  * - sog2 : Merged of sog1
168  * - sog3 : sog2 refined with ICP
169  *
170  * - The final sog is the merge of sog3.
171  *
172  */
174  sog2, sog3;
175 
176  /** The landmarks of each map (the indices of these landmarks correspond
177  * to those in "correspondences") */
179 
180  /** All the found correspondences (not consistent) */
182 
184  {
185  TPairPlusDistance(size_t i1, size_t i2, float d)
186  : idx_this(i1), idx_other(i2), dist(d)
187  {
188  }
190  float dist;
191  };
192 
193  /** Mahalanobis distance for each potential correspondence */
194  std::vector<TPairPlusDistance> correspondences_dists_maha;
195 
196  /** The ICP goodness of all potential SOG modes at the stage "sog2",
197  * thus before the removing of "bad" ICP matches. */
198  std::vector<double> icp_goodness_all_sog_modes;
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
211  *mrpt::maps::CMultiMetricMap
212  *class)
213  * \param m2 [IN] The second map (Must be a
214  *mrpt::maps::CMultiMetricMap
215  *class)
216  * \param initialEstimationPDF [IN] (IGNORED IN THIS ALGORITHM!)
217  * \param runningTime [OUT] A pointer to a container for obtaining the
218  *algorithm running time in seconds, or NULL if you don't need it.
219  * \param info [OUT] A pointer to a TReturnInfo struct, or NULL if
220  *result information are not required.
221  *
222  * \note The returned PDF depends on the selected alignment method:
223  * - "amRobustMatch" --> A "poses::CPosePDFSOG" object.
224  * - "amCorrelation" --> A "poses::CPosePDFGrid" object.
225  *
226  * \return A smart pointer to the output estimated pose PDF.
227  * \sa CPointsMapAlignmentAlgorithm, options
228  */
230  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
231  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
232  float* runningTime = nullptr,
233 
234  void* info = nullptr) override;
235 
236  /** Not applicable in this class, will launch an exception. */
238  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
239  const mrpt::poses::CPose3DPDFGaussian& initialEstimationPDF,
240  float* runningTime = nullptr, void* info = nullptr) override;
241 };
242 
243 } // namespace mrpt::slam
245 using namespace mrpt::slam;
246 MRPT_FILL_ENUM_MEMBER(CGridMapAligner, amRobustMatch);
247 MRPT_FILL_ENUM_MEMBER(CGridMapAligner, amCorrelation);
248 MRPT_FILL_ENUM_MEMBER(CGridMapAligner, amModifiedRANSAC);
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.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Wrapper to a std::shared_ptr<>, adding deep-copy semantics to copy ctor and copy operator, suitable for polymorphic classes with a clone() method.
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.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr > sog3
The set of parameters for all the detectors & descriptor algorithms.
mrpt::poses::CPosePDF::Ptr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr) override
The method for aligning a pair of 2D points map.
mrpt::poses::CPosePDF::Ptr AlignPDF_robustMatch(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr)
Private member, implements both, the "robustMatch" and the newer "modifiedRANSAC" algorithms...
The ICP algorithm return information.
mrpt::poses::CPosePDF::Ptr AlignPDF_correlation(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr)
Private member, implements one the algorithms.
mrpt::poses::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr) override
Not applicable in this class, will launch an exception.
MRPT_FILL_ENUM_MEMBER(CGridMapAligner, amRobustMatch)
mrpt::tfest::TMatchingPairList correspondences
All the found correspondences (not consistent)
This class allows loading and storing values and vectors of different types from a configuration text...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
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.
bool save_feat_coors
DEBUG - Dump all feature correspondences in a directory "grid_feats".
mrpt::vision::CFeatureExtraction::TOptions feature_detector_options
All the parameters for the feature detector.
COccupancyGridMapFeatureExtractor m_grid_feat_extr
Grid map features extractor.
A list of TMatchingPair.
Definition: TMatchingPair.h:70
TDescriptorType
The bitwise OR combination of values of TDescriptorType are used in CFeatureExtraction::computeDescri...
float ransac_minSetSizeRatio
RANSAC-step options:
TConfigParams()
Initializer for default values:
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
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 ...
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
mrpt::maps::CLandmarksMap::Ptr landmarks_map1
The landmarks of each map (the indices of these landmarks correspond to those in "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...
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.
Definition: CMetricMap.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
mrpt::vision::TStereoCalibResults out
A base class for any algorithm able of maps alignment.
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:
mrpt::slam::CGridMapAligner::TConfigParams options
mrpt::maps::CLandmarksMap::Ptr landmarks_map2
A class for detecting features from occupancy grid maps.
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr > sog2
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
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...
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr > sog1
The different SOG densities at different steps of the algorithm:
float min_ICP_goodness
The minimum goodness (0-1) of the post-matching ICP to accept a hypothesis as good (default=0...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019