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  */
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 <>
223  struct TEnumTypeFiller<slam::CGridMapAligner::TAlignerMethod>
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
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose.
Definition: CPose2D.h:37
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
The ICP algorithm configuration data.
float threshold_delta
Correspondences are considered if their distances to the best match are below this threshold (in the ...
float ransac_minSetSizeRatio
RANSAC-step options:
mrpt::vision::TDescriptorType feature_descriptor
The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF,...
bool save_feat_coors
DEBUG - Dump all feature correspondences in a directory "grid_feats".
double max_ICP_mahadist
The maximum Mahalanobis distance between the initial and final poses in the ICP not to discard the hy...
bool debug_save_map_pairs
DEBUG - Save the pair of maps with all the pairings.
float featsPerSquareMeter
Features extraction from grid map: How many features to extract.
double maxKLd_for_merge
Maximum KL-divergence for merging modes of the SOG (default=0.9)
double ransac_chi2_quantile
[amModifiedRANSAC method only] The quantile used for chi-square thresholding (default=0....
bool debug_show_corrs
DEBUG - Show graphs with the details of each feature correspondences.
mrpt::vision::CFeatureExtraction::TOptions feature_detector_options
All the parameters for the feature detector.
TConfigParams()
Initializer for default values:
float ransac_mahalanobisDistanceThreshold
[amRobustMatch method only] RANSAC-step options:
float min_ICP_goodness
The minimum goodness (0-1) of the post-matching ICP to accept a hypothesis as good (default=0....
TAlignerMethod methodSelection
The aligner method:
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
float threshold_max
Correspondences are considered if their distances are below this threshold (in the range [0,...
double ransac_prob_good_inliers
Probability of having a good inliers (def:0,9999), used for automatic number of iterations.
float ransac_SOG_sigma_m
The square root of the uncertainty normalization variance var_m (see papers...)
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
A class for aligning two multi-metric maps (with an occupancy grid maps and a points map,...
mrpt::poses::CPosePDFPtr AlignPDF_robustMatch(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=NULL, void *info=NULL)
Private member, implements both, the "robustMatch" and the newer "modifiedRANSAC" algorithms.
mrpt::poses::CPose3DPDFPtr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, float *runningTime=NULL, void *info=NULL)
Not applicable in this class, will launch an exception.
mrpt::poses::CPosePDFPtr AlignPDF_correlation(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=NULL, void *info=NULL)
Private member, implements one the algorithms.
COccupancyGridMapFeatureExtractor m_grid_feat_extr
Grid map features extractor.
TAlignerMethod
The type for selecting the grid-map alignment algorithm.
mrpt::poses::CPosePDFPtr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=NULL, void *info=NULL)
The method for aligning a pair of 2D points map.
A base class for any algorithm able of maps alignment.
A class for detecting features from occupancy grid maps.
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
A list of TMatchingPair.
Definition: TMatchingPair.h:67
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:29
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
TDescriptorType
The bitwise OR combination of values of TDescriptorType are used in CFeatureExtraction::computeDescri...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
mrpt::maps::CLandmarksMapPtr CLandmarksMapPtr
Backward compatible typedef.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The ICP algorithm return information.
mrpt::poses::CPosePDFSOGPtr sog1
The different SOG densities at different steps of the algorithm:
mrpt::maps::CLandmarksMapPtr landmarks_map1
The landmarks of each map (the indices of these landmarks correspond to those in "correspondences")
mrpt::utils::TMatchingPairList correspondences
All the found correspondences (not consistent)
std::vector< TPairPlusDistance > correspondences_dists_maha
Mahalanobis distance for each potential correspondence.
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...
size_t cbSize
Size of the structure, do not change, it's set automatically.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
mrpt::poses::CPose2D noRobustEstimation
The "brute" estimation from using all the available correspondences, provided just for comparison pur...
static void fill(bimap< enum_t, std::string > &m_map)
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:24
The set of parameters for all the detectors & descriptor algorithms.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 04:08:57 UTC 2022