Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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>
17 #include <mrpt/poses/CPosePDFSOG.h>
18 #include <mrpt/poses/poses_frwds.h>
21 
22 namespace mrpt
23 {
24  namespace slam
25  {
26  /** 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.
27  * The matching pose is returned as a Sum of Gaussians (poses::CPosePDFSOG).
28  *
29  * This class can use three methods (see options.methodSelection):
30  * - amCorrelation: "Brute-force" correlation of the two maps over a 2D+orientation grid of possible 2D poses.
31  * - amRobustMatch: Detection of features + RANSAC matching
32  * - 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
33  *
34  * See CGridMapAligner::Align for more instructions.
35  *
36  * \sa CMetricMapsAlignmentAlgorithm
37  * \ingroup mrpt_slam_grp
38  */
40  {
41  private:
42  /** Private member, implements one the algorithms.
43  */
44  mrpt::poses::CPosePDFPtr AlignPDF_correlation(
45  const mrpt::maps::CMetricMap *m1,
46  const mrpt::maps::CMetricMap *m2,
47  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
48  float *runningTime = NULL,
49  void *info = NULL );
50 
51  /** Private member, implements both, the "robustMatch" and the newer "modifiedRANSAC" algorithms.
52  */
53  mrpt::poses::CPosePDFPtr AlignPDF_robustMatch(
54  const mrpt::maps::CMetricMap *m1,
55  const mrpt::maps::CMetricMap *m2,
56  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
57  float *runningTime = NULL,
58  void *info = NULL );
59 
60  COccupancyGridMapFeatureExtractor m_grid_feat_extr; //!< Grid map features extractor
61  public:
62 
64  options()
65  {}
66 
67  /** The type for selecting the grid-map alignment algorithm.
68  */
70  {
71  amRobustMatch = 0,
73  amModifiedRANSAC
74  };
75 
76  /** The ICP algorithm configuration data
77  */
79  {
80  public:
81  /** Initializer for default values:
82  */
83  TConfigParams();
84 
85  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
86  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
87 
88  TAlignerMethod methodSelection; //!< The aligner method:
89 
90  /** The feature descriptor to use: 0=detector already has descriptor, 1= SIFT, 2=SURF, 4=Spin images, 8=Polar images, 16=log-polar images */
92 
93  /** All the parameters for the feature detector. */
95 
96  /** RANSAC-step options:
97  * \sa CICP::robustRigidTransformation
98  */
99  float ransac_minSetSizeRatio; //!< The ratio of landmarks that must be inliers to accepto an hypotheses (typ: 0.20)
100  float ransac_SOG_sigma_m; //!< The square root of the uncertainty normalization variance var_m (see papers...)
101 
102  /** [amRobustMatch method only] RANSAC-step options:
103  * \sa CICP::robustRigidTransformation
104  */
106 
107  double ransac_chi2_quantile; //!< [amModifiedRANSAC method only] The quantile used for chi-square thresholding (default=0.99)
108 
109  double ransac_prob_good_inliers; //!< Probability of having a good inliers (def:0,9999), used for automatic number of iterations
110  float featsPerSquareMeter; //!< Features extraction from grid map: How many features to extract
111  float threshold_max; //!< Correspondences are considered if their distances are below this threshold (in the range [0,1]) (default=0.15).
112  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).
113 
114  float min_ICP_goodness; //!< The minimum goodness (0-1) of the post-matching ICP to accept a hypothesis as good (default=0.30)
115  double max_ICP_mahadist; //!< The maximum Mahalanobis distance between the initial and final poses in the ICP not to discard the hypothesis (default=10)
116  double maxKLd_for_merge; //!< Maximum KL-divergence for merging modes of the SOG (default=0.9)
117 
118  bool save_feat_coors; //!< DEBUG - Dump all feature correspondences in a directory "grid_feats"
119  bool debug_show_corrs; //!< DEBUG - Show graphs with the details of each feature correspondences
120  bool debug_save_map_pairs; //!< DEBUG - Save the pair of maps with all the pairings.
121 
122  } options;
123 
124  /** The ICP algorithm return information.
125  */
127  {
129  goodness(0),
130  noRobustEstimation()
131  {
132  }
133 
134  /** A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
135  */
136  float goodness;
137 
138  /** 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).
139  */
141 
142  /** The different SOG densities at different steps of the algorithm:
143  * - sog1 : Directly from the matching of features
144  * - sog2 : Merged of sog1
145  * - sog3 : sog2 refined with ICP
146  *
147  * - The final sog is the merge of sog3.
148  *
149  */
151 
152  /** The landmarks of each map (the indices of these landmarks correspond to those in "correspondences") */
154 
155  /** All the found correspondences (not consistent) */
157 
159  {
160  TPairPlusDistance(size_t i1, size_t i2, float d) :
161  idx_this(i1), idx_other(i2), dist(d)
162  { }
163  size_t idx_this,idx_other;
164  float dist;
165  };
166 
167  std::vector<TPairPlusDistance> correspondences_dists_maha; //!< Mahalanobis distance for each potential correspondence
168 
169  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.
170  };
171 
172  /** The method for aligning a pair of 2D points map.
173  * The meaning of some parameters are implementation dependant,
174  * so look for derived classes for instructions.
175  * The target is to find a PDF for the pose displacement between
176  * maps, thus <b>the pose of m2 relative to m1</b>. This pose
177  * is returned as a PDF rather than a single value.
178  *
179  * NOTE: This method can be configurated with "options"
180  *
181  * \param m1 [IN] The first map (Must be a mrpt::maps::CMultiMetricMap class)
182  * \param m2 [IN] The second map (Must be a mrpt::maps::CMultiMetricMap class)
183  * \param initialEstimationPDF [IN] (IGNORED IN THIS ALGORITHM!)
184  * \param runningTime [OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
185  * \param info [OUT] A pointer to a TReturnInfo struct, or NULL if result information are not required.
186  *
187  * \note The returned PDF depends on the selected alignment method:
188  * - "amRobustMatch" --> A "poses::CPosePDFSOG" object.
189  * - "amCorrelation" --> A "poses::CPosePDFGrid" object.
190  *
191  * \return A smart pointer to the output estimated pose PDF.
192  * \sa CPointsMapAlignmentAlgorithm, options
193  */
194  mrpt::poses::CPosePDFPtr AlignPDF(
195  const mrpt::maps::CMetricMap *m1,
196  const mrpt::maps::CMetricMap *m2,
197  const mrpt::poses::CPosePDFGaussian &initialEstimationPDF,
198  float *runningTime = NULL,
199 
200  void *info = NULL );
201 
202 
203  /** Not applicable in this class, will launch an exception. */
204  mrpt::poses::CPose3DPDFPtr Align3DPDF(
205  const mrpt::maps::CMetricMap *m1,
206  const mrpt::maps::CMetricMap *m2,
207  const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF,
208  float *runningTime = NULL,
209  void *info = NULL );
210 
211  };
212 
213  } // End of namespace
214 
215  // Specializations MUST occur at the same namespace:
216  namespace utils
217  {
218  template <>
219  struct TEnumTypeFiller<slam::CGridMapAligner::TAlignerMethod>
220  {
222  static void fill(bimap<enum_t,std::string> &m_map)
223  {
224  m_map.insert(slam::CGridMapAligner::amRobustMatch, "amRobustMatch");
225  m_map.insert(slam::CGridMapAligner::amCorrelation, "amCorrelation");
226  m_map.insert(slam::CGridMapAligner::amModifiedRANSAC, "amModifiedRANSAC");
227  }
228  };
229  } // End of namespace
230 
231 } // End of namespace
232 
233 #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.
mrpt::utils::poly_ptr_ptr< mrpt::poses::CPosePDFSOGPtr > sog3
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...
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:78
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:
GLsizei const GLchar ** string
Definition: glext.h:3919
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, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A base class for any algorithm able of maps alignment.
static void fill(bimap< enum_t, std::string > &m_map)
backing_store_ptr info
Definition: jmemsys.h:170
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.
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
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.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019