MRPT  2.0.3
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  */
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);
mrpt::slam::CGridMapAligner::TReturnInfo::noRobustEstimation
mrpt::poses::CPose2D noRobustEstimation
The "brute" estimation from using all the available correspondences, provided just for comparison pur...
Definition: CGridMapAligner.h:146
mrpt::slam::CGridMapAligner::TReturnInfo::TReturnInfo
TReturnInfo()=default
mrpt::slam::CMetricMapsAlignmentAlgorithm
A base class for any algorithm able of maps alignment.
Definition: CMetricMapsAlignmentAlgorithm.h:41
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:39
mrpt::slam::CGridMapAligner::TReturnInfo::TPairPlusDistance
Definition: CGridMapAligner.h:166
mrpt::slam::CGridMapAligner::TReturnInfo::TPairPlusDistance::idx_other
size_t idx_other
Definition: CGridMapAligner.h:172
MRPT_ENUM_TYPE_END
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
mrpt::slam::CGridMapAligner::TConfigParams::ransac_prob_good_inliers
double ransac_prob_good_inliers
Probability of having a good inliers (def:0,9999), used for automatic number of iterations.
Definition: CGridMapAligner.h:100
mrpt::vision::descPolarImages
@ descPolarImages
Polar image descriptor.
Definition: vision/include/mrpt/vision/types.h:87
mrpt::slam::CGridMapAligner::TConfigParams::max_ICP_mahadist
double max_ICP_mahadist
The maximum Mahalanobis distance between the initial and final poses in the ICP not to discard the hy...
Definition: CGridMapAligner.h:115
mrpt::slam::CGridMapAligner::TReturnInfo::TPairPlusDistance::TPairPlusDistance
TPairPlusDistance(size_t i1, size_t i2, float d)
Definition: CGridMapAligner.h:168
mrpt::slam::COccupancyGridMapFeatureExtractor
A class for detecting features from occupancy grid maps.
Definition: COccupancyGridMapFeatureExtractor.h:32
CPosePDFSOG.h
mrpt::slam::CGridMapAligner::TConfigParams::debug_show_corrs
bool debug_show_corrs
DEBUG - Show graphs with the details of each feature correspondences.
Definition: CGridMapAligner.h:124
mrpt::slam::CGridMapAligner::TReturnInfo::TPairPlusDistance::dist
float dist
Definition: CGridMapAligner.h:173
mrpt::slam::CGridMapAligner::TConfigParams::feature_detector_options
mrpt::vision::CFeatureExtraction::TOptions feature_detector_options
All the parameters for the feature detector.
Definition: CGridMapAligner.h:77
COccupancyGridMapFeatureExtractor.h
mrpt::slam::CGridMapAligner::TConfigParams::threshold_max
float threshold_max
Correspondences are considered if their distances are below this threshold (in the range [0,...
Definition: CGridMapAligner.h:105
mrpt::slam::CGridMapAligner::CGridMapAligner
CGridMapAligner()
Definition: CGridMapAligner.h:43
mrpt::slam::CGridMapAligner::options
mrpt::slam::CGridMapAligner::TConfigParams options
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::slam::CGridMapAligner::TConfigParams::ransac_SOG_sigma_m
float ransac_SOG_sigma_m
The square root of the uncertainty normalization variance var_m (see papers...)
Definition: CGridMapAligner.h:87
MRPT_FILL_ENUM_MEMBER
MRPT_FILL_ENUM_MEMBER(CGridMapAligner, amRobustMatch)
mrpt::slam::CGridMapAligner::TReturnInfo::sog2
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr > sog2
Definition: CGridMapAligner.h:157
mrpt::slam::CGridMapAligner::TConfigParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form,...
Definition: CGridMapAligner.cpp:1098
MRPT_ENUM_TYPE_BEGIN
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
mrpt::slam::CGridMapAligner::amRobustMatch
@ amRobustMatch
Definition: CGridMapAligner.h:48
mrpt::slam::CGridMapAligner::TConfigParams::loadFromConfigFile
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.
Definition: CGridMapAligner.cpp:1128
mrpt::slam::CGridMapAligner::TReturnInfo::icp_goodness_all_sog_modes
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...
Definition: CGridMapAligner.h:181
mrpt::slam::CGridMapAligner::TConfigParams::TConfigParams
TConfigParams()
Initializer for default values:
Definition: CGridMapAligner.cpp:1094
mrpt::slam::CGridMapAligner::TConfigParams::ransac_chi2_quantile
double ransac_chi2_quantile
[amModifiedRANSAC method only] The quantile used for chi-square thresholding (default=0....
Definition: CGridMapAligner.h:96
mrpt::slam::CGridMapAligner::Align3DPDF
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.
Definition: CGridMapAligner.cpp:1163
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
mrpt::slam::CGridMapAligner::TReturnInfo::correspondences
mrpt::tfest::TMatchingPairList correspondences
All the found correspondences (not consistent)
Definition: CGridMapAligner.h:164
mrpt::slam::CGridMapAligner::TConfigParams
The ICP algorithm configuration data.
Definition: CGridMapAligner.h:55
mrpt::slam::CGridMapAligner::AlignPDF_correlation
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.
Definition: CGridMapAligner.cpp:958
CLandmarksMap.h
mrpt::slam::CGridMapAligner::TConfigParams::featsPerSquareMeter
float featsPerSquareMeter
Features extraction from grid map: How many features to extract.
Definition: CGridMapAligner.h:102
mrpt::maps::CLandmarksMap::Ptr
std::shared_ptr< mrpt::maps ::CLandmarksMap > Ptr
Definition: CLandmarksMap.h:76
mrpt::slam::TMetricMapAlignmentResult
Used as base class for other result structures of each particular algorithm in CMetricMapsAlignmentAl...
Definition: CMetricMapsAlignmentAlgorithm.h:26
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::slam::CGridMapAligner::TReturnInfo::sog1
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr > sog1
The different SOG densities at different steps of the algorithm:
Definition: CGridMapAligner.h:156
CMetricMapsAlignmentAlgorithm.h
TEnumType.h
CFeatureExtraction.h
mrpt::slam::CGridMapAligner::TReturnInfo::TPairPlusDistance::idx_this
size_t idx_this
Definition: CGridMapAligner.h:172
mrpt::config::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: config/CLoadableOptions.h:26
mrpt::vision::CFeatureExtraction::TOptions
The set of parameters for all the detectors & descriptor algorithms.
Definition: CFeatureExtraction.h:87
mrpt::slam::CGridMapAligner::TReturnInfo::goodness
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
Definition: CGridMapAligner.h:140
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr >
mrpt::slam::CGridMapAligner::TConfigParams::debug_save_map_pairs
bool debug_save_map_pairs
DEBUG - Save the pair of maps with all the pairings.
Definition: CGridMapAligner.h:126
mrpt::slam::CGridMapAligner::TConfigParams::threshold_delta
float threshold_delta
Correspondences are considered if their distances to the best match are below this threshold (in the ...
Definition: CGridMapAligner.h:108
mrpt::slam::CGridMapAligner::TConfigParams::ransac_mahalanobisDistanceThreshold
float ransac_mahalanobisDistanceThreshold
[amRobustMatch method only] RANSAC-step options:
Definition: CGridMapAligner.h:92
mrpt::slam::CGridMapAligner::m_grid_feat_extr
COccupancyGridMapFeatureExtractor m_grid_feat_extr
Grid map features extractor.
Definition: CGridMapAligner.h:242
mrpt::slam::CGridMapAligner::TConfigParams::ransac_minSetSizeRatio
float ransac_minSetSizeRatio
RANSAC-step options:
Definition: CGridMapAligner.h:84
mrpt::slam::CGridMapAligner::AlignPDF_robustMatch
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.
Definition: CGridMapAligner.cpp:85
mrpt::slam::CGridMapAligner
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map,...
Definition: CGridMapAligner.h:40
mrpt::slam::CGridMapAligner::TConfigParams::min_ICP_goodness
float min_ICP_goodness
The minimum goodness (0-1) of the post-matching ICP to accept a hypothesis as good (default=0....
Definition: CGridMapAligner.h:112
mrpt::slam::CGridMapAligner::TReturnInfo::sog3
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDFSOG::Ptr > sog3
Definition: CGridMapAligner.h:157
mrpt::slam::CGridMapAligner::TConfigParams::methodSelection
TAlignerMethod methodSelection
The aligner method:
Definition: CGridMapAligner.h:69
mrpt::slam::CGridMapAligner::TConfigParams::maxKLd_for_merge
double maxKLd_for_merge
Maximum KL-divergence for merging modes of the SOG (default=0.9)
Definition: CGridMapAligner.h:117
mrpt::vision::TDescriptorType
TDescriptorType
The bitwise OR combination of values of TDescriptorType are used in CFeatureExtraction::computeDescri...
Definition: vision/include/mrpt/vision/types.h:76
mrpt::slam::CGridMapAligner::TReturnInfo
The ICP algorithm return information.
Definition: CGridMapAligner.h:132
CLoadableOptions.h
poses_frwds.h
deepcopy_poly_ptr.h
mrpt::poses::CPose3DPDF::Ptr
std::shared_ptr< CPose3DPDF > Ptr
Definition: CPose3DPDF.h:42
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
mrpt::slam::CGridMapAligner::TReturnInfo::~TReturnInfo
virtual ~TReturnInfo() override=default
mrpt::slam::CGridMapAligner::TReturnInfo::landmarks_map1
mrpt::maps::CLandmarksMap::Ptr landmarks_map1
The landmarks of each map (the indices of these landmarks correspond to those in "correspondences")
Definition: CGridMapAligner.h:161
mrpt::slam::CGridMapAligner::TReturnInfo::correspondences_dists_maha
std::vector< TPairPlusDistance > correspondences_dists_maha
Mahalanobis distance for each potential correspondence.
Definition: CGridMapAligner.h:177
mrpt::slam::CGridMapAligner::amModifiedRANSAC
@ amModifiedRANSAC
Definition: CGridMapAligner.h:50
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:28
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:41
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::slam::CGridMapAligner::TReturnInfo::landmarks_map2
mrpt::maps::CLandmarksMap::Ptr landmarks_map2
Definition: CGridMapAligner.h:161
mrpt::slam::CGridMapAligner::TConfigParams::feature_descriptor
mrpt::vision::TDescriptorType feature_descriptor
The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF,...
Definition: CGridMapAligner.h:73
mrpt::optional_ref
std::optional< std::reference_wrapper< T > > optional_ref
Shorter name for std::optional<std::reference_wrapper<T>>
Definition: optional_ref.h:20
mrpt::slam::CGridMapAligner::TAlignerMethod
TAlignerMethod
The type for selecting the grid-map alignment algorithm.
Definition: CGridMapAligner.h:46
mrpt::slam::CGridMapAligner::AlignPDF
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.
Definition: CGridMapAligner.cpp:41
mrpt::slam::CGridMapAligner::TConfigParams::save_feat_coors
bool save_feat_coors
DEBUG - Dump all feature correspondences in a directory "grid_feats".
Definition: CGridMapAligner.h:121
mrpt::slam::CGridMapAligner::amCorrelation
@ amCorrelation
Definition: CGridMapAligner.h:49



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020