MRPT  2.0.5
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-2020, 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  public:
44  /** The type for selecting the grid-map alignment algorithm.
45  */
47  {
51  };
52 
53  /** The ICP algorithm configuration data
54  */
56  {
57  public:
58  /** Initializer for default values:
59  */
60  TConfigParams();
61 
62  void loadFromConfigFile(
63  const mrpt::config::CConfigFileBase& source,
64  const std::string& section) override; // See base docs
65  void dumpToTextStream(
66  std::ostream& out) const override; // See base docs
67 
68  /** The aligner method: */
70 
71  /** The feature descriptor to use: 0=detector already has descriptor, 1=
72  * SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images */
75 
76  /** All the parameters for the feature detector. */
78 
79  /** RANSAC-step options:
80  * \sa CICP::robustRigidTransformation
81  */
82  /** The ratio of landmarks that must be inliers to accepto an hypotheses
83  * (typ: 0.20) */
84  float ransac_minSetSizeRatio{0.20f};
85  /** The square root of the uncertainty normalization variance var_m (see
86  * papers...) */
87  float ransac_SOG_sigma_m{0.10f};
88 
89  /** [amRobustMatch method only] RANSAC-step options:
90  * \sa CICP::robustRigidTransformation
91  */
93 
94  /** [amModifiedRANSAC method only] The quantile used for chi-square
95  * thresholding (default=0.99) */
96  double ransac_chi2_quantile{0.99};
97 
98  /** Probability of having a good inliers (def:0,9999), used for
99  * automatic number of iterations */
100  double ransac_prob_good_inliers{0.9999};
101  /** Features extraction from grid map: How many features to extract */
102  float featsPerSquareMeter{0.015f};
103  /** Correspondences are considered if their distances are below this
104  * threshold (in the range [0,1]) (default=0.15). */
105  float threshold_max{0.15f};
106  /** Correspondences are considered if their distances to the best match
107  * are below this threshold (in the range [0,1]) (default=0.15). */
108  float threshold_delta{0.10f};
109 
110  /** The minimum goodness (0-1) of the post-matching ICP to accept a
111  * hypothesis as good (default=0.30) */
112  float min_ICP_goodness{0.30f};
113  /** The maximum Mahalanobis distance between the initial and final poses
114  * in the ICP not to discard the hypothesis (default=10) */
115  double max_ICP_mahadist{10.0};
116  /** Maximum KL-divergence for merging modes of the SOG (default=0.9) */
117  double maxKLd_for_merge{0.9};
118 
119  /** DEBUG - Dump all feature correspondences in a directory "grid_feats"
120  */
121  bool save_feat_coors{false};
122  /** DEBUG - Show graphs with the details of each feature correspondences
123  */
124  bool debug_show_corrs{false};
125  /** DEBUG - Save the pair of maps with all the pairings. */
126  bool debug_save_map_pairs{false};
127 
128  } options;
129 
130  /** The ICP algorithm return information.
131  */
133  {
134  TReturnInfo() = default;
135  virtual ~TReturnInfo() override = default;
136 
137  /** A goodness measure for the alignment, it is a [0,1] range indicator
138  * of percentage of correspondences.
139  */
140  float goodness{0};
141 
142  /** The "brute" estimation from using all the available correspondences,
143  * provided just for comparison purposes (it is not the robust
144  * estimation, available as the result of the Align method).
145  */
147 
148  /** The different SOG densities at different steps of the algorithm:
149  * - sog1 : Directly from the matching of features
150  * - sog2 : Merged of sog1
151  * - sog3 : sog2 refined with ICP
152  *
153  * - The final sog is the merge of sog3.
154  *
155  */
157  sog2, sog3;
158 
159  /** The landmarks of each map (the indices of these landmarks correspond
160  * to those in "correspondences") */
162 
163  /** All the found correspondences (not consistent) */
165 
167  {
168  TPairPlusDistance(size_t i1, size_t i2, float d)
169  : idx_this(i1), idx_other(i2), dist(d)
170  {
171  }
173  float dist;
174  };
175 
176  /** Mahalanobis distance for each potential correspondence */
177  std::vector<TPairPlusDistance> correspondences_dists_maha;
178 
179  /** The ICP goodness of all potential SOG modes at the stage "sog2",
180  * thus before the removing of "bad" ICP matches. */
181  std::vector<double> icp_goodness_all_sog_modes;
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
194  *mrpt::maps::CMultiMetricMap
195  *class)
196  * \param m2 [IN] The second map (Must be a
197  *mrpt::maps::CMultiMetricMap
198  *class)
199  * \param initialEstimationPDF [IN] (IGNORED IN THIS ALGORITHM!)
200  * \param runningTime [OUT] A pointer to a container for obtaining the
201  *algorithm running time in seconds, or NULL if you don't need it.
202  * \param info [OUT] A pointer to a TReturnInfo struct, or NULL if
203  *result information are not required.
204  *
205  * \note The returned PDF depends on the selected alignment method:
206  * - "amRobustMatch" --> A "poses::CPosePDFSOG" object.
207  * - "amCorrelation" --> A "poses::CPosePDFGrid" object.
208  *
209  * \return A smart pointer to the output estimated pose PDF.
210  * \sa CPointsMapAlignmentAlgorithm, options
211  */
213  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
214  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
216  std::nullopt) override;
217 
218  /** Not applicable in this class, will launch an exception if used. */
220  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
221  const mrpt::poses::CPose3DPDFGaussian& initialEstimationPDF,
223  std::nullopt) override;
224 
225  private:
226  /** Private member, implements one the algorithms.
227  */
229  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
230  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
231  TReturnInfo& info);
232 
233  /** Private member, implements both, the "robustMatch" and the newer
234  * "modifiedRANSAC" algorithms.
235  */
237  const mrpt::maps::CMetricMap* m1, const mrpt::maps::CMetricMap* m2,
238  const mrpt::poses::CPosePDFGaussian& initialEstimationPDF,
239  TReturnInfo& info);
240 
241  /** Grid map features extractor */
243 };
244 
245 } // namespace mrpt::slam
247 using namespace mrpt::slam;
248 MRPT_FILL_ENUM_MEMBER(CGridMapAligner, amRobustMatch);
249 MRPT_FILL_ENUM_MEMBER(CGridMapAligner, amCorrelation);
250 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.
mrpt::poses::CPosePDF::Ptr AlignPDF_correlation(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &info)
Private member, implements one the algorithms.
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.
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
The ICP algorithm return information.
mrpt::poses::CPosePDF::Ptr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt) override
The method for aligning a pair of 2D points map.
Used as base class for other result structures of each particular algorithm in CMetricMapsAlignmentAl...
MRPT_FILL_ENUM_MEMBER(CGridMapAligner, amRobustMatch)
virtual ~TReturnInfo() override=default
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.
mrpt::poses::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt) override
Not applicable in this class, will launch an exception if used.
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...
mrpt::poses::CPosePDF::Ptr AlignPDF_robustMatch(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &info)
Private member, implements both, the "robustMatch" and the newer "modifiedRANSAC" algorithms...



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: b26ce1112 Mon Jul 6 09:39:08 2020 +0200 at lun jul 6 09:45:13 CEST 2020