Main MRPT website > C++ reference for MRPT 1.5.9
data_association.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 data_association_H
10 #define data_association_H
11 
12 #include <mrpt/utils/utils_defs.h>
15 #include <mrpt/math/CMatrixTemplate.h> // mrpt::math::CMatrixBool
16 #include <mrpt/utils/TEnumType.h>
17 
18 #include <mrpt/slam/link_pragmas.h>
19 
20 namespace mrpt
21 {
22  namespace slam
23  {
24  /** \addtogroup data_assoc_grp Data association
25  * \ingroup mrpt_slam_grp
26  * @{ */
27 
28  /** @name Data association
29  @{
30  */
31 
32  /** Different algorithms for data association, used in mrpt::slam::data_association
33  */
35  {
36  assocNN = 0, //!< Nearest-neighbor.
37  assocJCBB //!< JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001].
38  };
39 
40  /** Different metrics for data association, used in mrpt::slam::data_association
41  * For a comparison of both methods see paper:
42  * * J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "An alternative to the Mahalanobis distance for determining optimal correspondences in data association", IEEE Transactions on Robotics (T-RO), (2012) DOI: 10.1109/TRO.2012.2193706 Draft: http://ingmec.ual.es/~jlblanco/papers/blanco2012amd.pdf
43  */
45  {
46  metricMaha= 0, //!< Mahalanobis distance
47  metricML //!< Matching likelihood (See TDataAssociationMetric for a paper explaining this metric)
48  };
49 
50  typedef size_t observation_index_t; //!< Used in mrpt::slam::TDataAssociationResults
51  typedef size_t prediction_index_t; //!< Used in mrpt::slam::TDataAssociationResults
52 
53  /** The results from mrpt::slam::data_association
54  */
56  {
58  associations(),
59  distance(0),
60  indiv_distances(0,0),
61  indiv_compatibility(0,0),
62  indiv_compatibility_counts(),
63  nNodesExploredInJCBB(0)
64  {}
65 
66  void clear()
67  {
68  associations.clear();
69  distance = 0;
70  indiv_distances.setSize(0,0);
71  indiv_compatibility.setSize(0,0);
72  indiv_compatibility_counts.clear();
73  nNodesExploredInJCBB = 0;
74  }
75 
76  /** For each observation (with row index IDX_obs in the input "Z_observations"), its association in the predictions, as the row index in the "Y_predictions_mean" input (or it's mapping to a custom ID, if it was provided).
77  * Note that not all observations may have an associated prediction.
78  * An observation with index "IDX_obs" corresponds to the prediction number "associations[IDX_obs]", or it may not correspond to anyone if it's not present
79  * in the std::map (Tip: Use associations.find(IDX_obs)!=associations.end() )
80  * \note The types observation_index_t and prediction_index_t are just used for clarity, use normal size_t's.
81  */
82  std::map<observation_index_t,prediction_index_t> associations;
83  double distance; //!< The Joint Mahalanobis distance or matching likelihood of the best associations found.
84 
85  /** Individual mahalanobis distances (or matching likelihood, depending on the selected metric) between predictions (row indices) & observations (column indices).
86  * Indices are for the appearing order in the arguments "Y_predictions_mean" & "Z_observations", they are NOT landmark IDs.
87  */
89  mrpt::math::CMatrixBool indiv_compatibility; //!< The result of a chi2 test for compatibility using mahalanobis distance - Indices are like in "indiv_distances".
90  vector_uint indiv_compatibility_counts; //!< The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each observation.
91 
92  size_t nNodesExploredInJCBB; //!< Only for the JCBB method,the number of recursive calls expent in the algorithm.
93  };
94 
95 
96  /** Computes the data-association between the prediction of a set of landmarks and their observations, all of them with covariance matrices - Generic version with prediction full cross-covariances.
97  * Implemented methods include (see TDataAssociation)
98  * - NN: Nearest-neighbor
99  * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
100  *
101  * With both a Mahalanobis-distance or Matching-likelihood metric. For a comparison of both methods, see paper:
102  * * J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "An alternative to the Mahalanobis distance for determining optimal correspondences in data association", IEEE Transactions on Robotics (T-RO), (2012) DOI: 10.1109/TRO.2012.2193706 Draft: http://ingmec.ual.es/~jlblanco/papers/blanco2012amd.pdf
103  *
104  * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean".
105  * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.
106  * \param Y_predictions_cov [IN] An N*OxN*O matrix with the full covariance matrix of all the N predictions.
107  * \param results [OUT] The output data association hypothesis, and other useful information.
108  * \param method [IN, optional] The selected method to make the associations.
109  * \param chi2quantile [IN, optional] The threshold for considering a match between two close Gaussians for two landmarks, in the range [0,1]. It is used to call mrpt::math::chi2inv
110  * \param use_kd_tree [IN, optional] Build a KD-tree to speed-up the evaluation of individual compatibility (IC). It's perhaps more efficient to disable it for a small number of features. (default=true).
111  * \param predictions_IDs [IN, optional] (default:none) An N-vector. If provided, the resulting associations in "results.associations" will not contain prediction indices "i", but "predictions_IDs[i]".
112  *
113  * \sa data_association_independent_predictions, data_association_independent_2d_points, data_association_independent_3d_points
114  */
116  const mrpt::math::CMatrixDouble &Z_observations_mean,
117  const mrpt::math::CMatrixDouble &Y_predictions_mean,
118  const mrpt::math::CMatrixDouble &Y_predictions_cov,
119  TDataAssociationResults &results,
120  const TDataAssociationMethod method = assocJCBB,
121  const TDataAssociationMetric metric = metricMaha,
122  const double chi2quantile = 0.99,
123  const bool DAT_ASOC_USE_KDTREE = true,
124  const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>(),
125  const TDataAssociationMetric compatibilityTestMetric = metricMaha,
126  const double log_ML_compat_test_threshold = 0.0
127  );
128 
129  /** Computes the data-association between the prediction of a set of landmarks and their observations, all of them with covariance matrices - Generic version with NO prediction cross-covariances.
130  * Implemented methods include (see TDataAssociation)
131  * - NN: Nearest-neighbor
132  * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
133  *
134  * With both a Mahalanobis-distance or Matching-likelihood metric. For a comparison of both methods, see paper:
135  * * J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "An alternative to the Mahalanobis distance for determining optimal correspondences in data association", IEEE Transactions on Robotics (T-RO), (2012) DOI: 10.1109/TRO.2012.2193706 Draft: http://ingmec.ual.es/~jlblanco/papers/blanco2012amd.pdf
136  *
137  * \param Z_observations_mean [IN] An MxO matrix with the M observations, each row containing the observation "mean".
138  * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.
139  * \param Y_predictions_cov [IN] An N*OxO matrix: A vertical stack of N covariance matrix, one for each of the N prediction.
140  * \param results [OUT] The output data association hypothesis, and other useful information.
141  * \param method [IN, optional] The selected method to make the associations.
142  * \param chi2quantile [IN, optional] The threshold for considering a match between two close Gaussians for two landmarks, in the range [0,1]. It is used to call mrpt::math::chi2inv
143  * \param use_kd_tree [IN, optional] Build a KD-tree to speed-up the evaluation of individual compatibility (IC). It's perhaps more efficient to disable it for a small number of features. (default=true).
144  * \param predictions_IDs [IN, optional] (default:none) An N-vector. If provided, the resulting associations in "results.associations" will not contain prediction indices "i", but "predictions_IDs[i]".
145  *
146  * \sa data_association_full_covariance, data_association_independent_2d_points, data_association_independent_3d_points
147  */
149  const mrpt::math::CMatrixDouble &Z_observations_mean,
150  const mrpt::math::CMatrixDouble &Y_predictions_mean,
151  const mrpt::math::CMatrixDouble &Y_predictions_cov,
152  TDataAssociationResults &results,
153  const TDataAssociationMethod method = assocJCBB,
154  const TDataAssociationMetric metric = metricMaha,
155  const double chi2quantile = 0.99,
156  const bool DAT_ASOC_USE_KDTREE = true,
157  const std::vector<prediction_index_t> &predictions_IDs = std::vector<prediction_index_t>(),
158  const TDataAssociationMetric compatibilityTestMetric = metricMaha,
159  const double log_ML_compat_test_threshold = 0.0
160  );
161 
162 
163  /** @} */
164 
165  /** @} */ // end of grouping
166 
167  } // end namespace
168 
169  // Specializations MUST occur at the same namespace:
170  namespace utils
171  {
172  template <>
174  {
176  static void fill(bimap<enum_t,std::string> &m_map)
177  {
178  m_map.insert(slam::assocNN, "assocNN");
179  m_map.insert(slam::assocJCBB, "assocJCBB");
180  }
181  };
182 
183  template <>
185  {
187  static void fill(bimap<enum_t,std::string> &m_map)
188  {
189  m_map.insert(slam::metricMaha, "metricMaha");
190  m_map.insert(slam::metricML, "metricML");
191  }
192  };
193 
194  } // End of namespace
195 
196 } // End of namespace
197 
198 #endif
Nearest-neighbor.
Declares a matrix of booleans (non serializable).
std::vector< uint32_t > vector_uint
Definition: types_simple.h:28
Mahalanobis distance.
size_t observation_index_t
Used in mrpt::slam::TDataAssociationResults.
vector_uint indiv_compatibility_counts
The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each ob...
mrpt::math::CMatrixDouble indiv_distances
Individual mahalanobis distances (or matching likelihood, depending on the selected metric) between p...
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:23
std::map< observation_index_t, prediction_index_t > associations
For each observation (with row index IDX_obs in the input "Z_observations"), its association in the p...
JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001].
double distance
The Joint Mahalanobis distance or matching likelihood of the best associations found.
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:28
mrpt::math::CMatrixBool indiv_compatibility
The result of a chi2 test for compatibility using mahalanobis distance - Indices are like in "indiv_d...
void SLAM_IMPEXP data_association_independent_predictions(const mrpt::math::CMatrixDouble &Z_observations_mean, const mrpt::math::CMatrixDouble &Y_predictions_mean, const mrpt::math::CMatrixDouble &Y_predictions_cov, TDataAssociationResults &results, const TDataAssociationMethod method=assocJCBB, const TDataAssociationMetric metric=metricMaha, const double chi2quantile=0.99, const bool DAT_ASOC_USE_KDTREE=true, const std::vector< prediction_index_t > &predictions_IDs=std::vector< prediction_index_t >(), const TDataAssociationMetric compatibilityTestMetric=metricMaha, const double log_ML_compat_test_threshold=0.0)
Computes the data-association between the prediction of a set of landmarks and their observations...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
static void fill(bimap< enum_t, std::string > &m_map)
size_t prediction_index_t
Used in mrpt::slam::TDataAssociationResults.
size_t nNodesExploredInJCBB
Only for the JCBB method,the number of recursive calls expent in the algorithm.
The results from mrpt::slam::data_association.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
Matching likelihood (See TDataAssociationMetric for a paper explaining this metric) ...
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void SLAM_IMPEXP data_association_full_covariance(const mrpt::math::CMatrixDouble &Z_observations_mean, const mrpt::math::CMatrixDouble &Y_predictions_mean, const mrpt::math::CMatrixDouble &Y_predictions_cov, TDataAssociationResults &results, const TDataAssociationMethod method=assocJCBB, const TDataAssociationMetric metric=metricMaha, const double chi2quantile=0.99, const bool DAT_ASOC_USE_KDTREE=true, const std::vector< prediction_index_t > &predictions_IDs=std::vector< prediction_index_t >(), const TDataAssociationMetric compatibilityTestMetric=metricMaha, const double log_ML_compat_test_threshold=0.0)
Computes the data-association between the prediction of a set of landmarks and their observations...
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020