MRPT  1.9.9
data_association.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 
11 #include <mrpt/math/CMatrixDynamic.h> // mrpt::math::CMatrixBool
15 
16 namespace mrpt::slam
17 {
18 /** \addtogroup data_assoc_grp Data association
19  * \ingroup mrpt_slam_grp
20  * @{ */
21 
22 /** @name Data association
23  @{
24  */
25 
26 /** Different algorithms for data association, used in
27  * mrpt::slam::data_association
28  */
30 {
31  /** Nearest-neighbor. */
32  assocNN = 0,
33  /** JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]. */
35 };
36 
37 /** Different metrics for data association, used in mrpt::slam::data_association
38  * For a comparison of both methods see paper:
39  * * J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "An
40  * alternative to the Mahalanobis distance for determining optimal
41  * correspondences in data association", IEEE Transactions on Robotics (T-RO),
42  * (2012) DOI: 10.1109/TRO.2012.2193706 Draft:
43  * http://ingmec.ual.es/~jlblanco/papers/blanco2012amd.pdf
44  */
46 {
47  /** Mahalanobis distance */
49  /** Matching likelihood (See TDataAssociationMetric for a paper explaining
50  this metric) */
52 };
53 
54 /** Used in mrpt::slam::TDataAssociationResults */
55 using observation_index_t = size_t;
56 /** Used in mrpt::slam::TDataAssociationResults */
57 using prediction_index_t = size_t;
58 
59 /** The results from mrpt::slam::data_association
60  */
62 {
64  : associations(),
65 
66  indiv_distances(0, 0),
67  indiv_compatibility(0, 0),
69 
70  {
71  }
72 
73  void clear()
74  {
75  associations.clear();
76  distance = 0;
81  }
82 
83  /** For each observation (with row index IDX_obs in the input
84  * "Z_observations"), its association in the predictions, as the row index
85  * in the "Y_predictions_mean" input (or it's mapping to a custom ID, if it
86  * was provided).
87  * Note that not all observations may have an associated prediction.
88  * An observation with index "IDX_obs" corresponds to the prediction number
89  * "associations[IDX_obs]", or it may not correspond to anyone if it's not
90  * present
91  * in the std::map (Tip: Use
92  * associations.find(IDX_obs)!=associations.end() )
93  * \note The types observation_index_t and prediction_index_t are just used
94  * for clarity, use normal size_t's.
95  */
96  std::map<observation_index_t, prediction_index_t> associations;
97  /** The Joint Mahalanobis distance or matching likelihood of the best
98  * associations found. */
99  double distance{0};
100 
101  /** Individual mahalanobis distances (or matching likelihood, depending on
102  * the selected metric) between predictions (row indices) & observations
103  * (column indices).
104  * Indices are for the appearing order in the arguments
105  * "Y_predictions_mean" & "Z_observations", they are NOT landmark IDs.
106  */
108  /** The result of a chi2 test for compatibility using mahalanobis distance -
109  * Indices are like in "indiv_distances". */
111  /** The sum of each column of indiv_compatibility, that is, the number of
112  * compatible pairings for each observation. */
113  std::vector<uint32_t> indiv_compatibility_counts;
114 
115  /** Only for the JCBB method,the number of recursive calls expent in the
116  * algorithm. */
118 };
119 
120 /** Computes the data-association between the prediction of a set of landmarks
121  *and their observations, all of them with covariance matrices - Generic
122  *version with prediction full cross-covariances.
123  * Implemented methods include (see TDataAssociation)
124  * - NN: Nearest-neighbor
125  * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
126  *
127  * With both a Mahalanobis-distance or Matching-likelihood metric. For a
128  *comparison of both methods, see paper:
129  * * J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "An
130  *alternative to the Mahalanobis distance for determining optimal
131  *correspondences in data association", IEEE Transactions on Robotics (T-RO),
132  *(2012) DOI: 10.1109/TRO.2012.2193706 Draft:
133  *http://ingmec.ual.es/~jlblanco/papers/blanco2012amd.pdf
134  *
135  * \param Z_observations_mean [IN] An MxO matrix with the M observations, each
136  *row containing the observation "mean".
137  * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each
138  *row containing the mean of one prediction.
139  * \param Y_predictions_cov [IN] An N*OxN*O matrix with the full covariance
140  *matrix of all the N predictions.
141  * \param results [OUT] The output data association hypothesis, and other
142  *useful information.
143  * \param method [IN, optional] The selected method to make the associations.
144  * \param chi2quantile [IN, optional] The threshold for considering a match
145  *between two close Gaussians for two landmarks, in the range [0,1]. It is used
146  *to call mrpt::math::chi2inv
147  * \param use_kd_tree [IN, optional] Build a KD-tree to speed-up the evaluation
148  *of individual compatibility (IC). It's perhaps more efficient to disable it
149  *for a small number of features. (default=true).
150  * \param predictions_IDs [IN, optional] (default:none) An N-vector. If
151  *provided, the resulting associations in "results.associations" will not
152  *contain prediction indices "i", but "predictions_IDs[i]".
153  *
154  * \sa data_association_independent_predictions,
155  *data_association_independent_2d_points,
156  *data_association_independent_3d_points
157  */
159  const mrpt::math::CMatrixDouble& Z_observations_mean,
160  const mrpt::math::CMatrixDouble& Y_predictions_mean,
161  const mrpt::math::CMatrixDouble& Y_predictions_cov,
163  const TDataAssociationMethod method = assocJCBB,
164  const TDataAssociationMetric metric = metricMaha,
165  const double chi2quantile = 0.99, const bool DAT_ASOC_USE_KDTREE = true,
166  const std::vector<prediction_index_t>& predictions_IDs =
167  std::vector<prediction_index_t>(),
168  const TDataAssociationMetric compatibilityTestMetric = metricMaha,
169  const double log_ML_compat_test_threshold = 0.0);
170 
171 /** Computes the data-association between the prediction of a set of landmarks
172  *and their observations, all of them with covariance matrices - Generic
173  *version with NO prediction cross-covariances.
174  * Implemented methods include (see TDataAssociation)
175  * - NN: Nearest-neighbor
176  * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
177  *
178  * With both a Mahalanobis-distance or Matching-likelihood metric. For a
179  *comparison of both methods, see paper:
180  * * J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "An
181  *alternative to the Mahalanobis distance for determining optimal
182  *correspondences in data association", IEEE Transactions on Robotics (T-RO),
183  *(2012) DOI: 10.1109/TRO.2012.2193706 Draft:
184  *http://ingmec.ual.es/~jlblanco/papers/blanco2012amd.pdf
185  *
186  * \param Z_observations_mean [IN] An MxO matrix with the M observations, each
187  *row containing the observation "mean".
188  * \param Y_predictions_mean [IN] An NxO matrix with the N predictions, each
189  *row containing the mean of one prediction.
190  * \param Y_predictions_cov [IN] An N*OxO matrix: A vertical stack of N
191  *covariance matrix, one for each of the N prediction.
192  * \param results [OUT] The output data association hypothesis, and other
193  *useful information.
194  * \param method [IN, optional] The selected method to make the associations.
195  * \param chi2quantile [IN, optional] The threshold for considering a match
196  *between two close Gaussians for two landmarks, in the range [0,1]. It is used
197  *to call mrpt::math::chi2inv
198  * \param use_kd_tree [IN, optional] Build a KD-tree to speed-up the evaluation
199  *of individual compatibility (IC). It's perhaps more efficient to disable it
200  *for a small number of features. (default=true).
201  * \param predictions_IDs [IN, optional] (default:none) An N-vector. If
202  *provided, the resulting associations in "results.associations" will not
203  *contain prediction indices "i", but "predictions_IDs[i]".
204  *
205  * \sa data_association_full_covariance,
206  *data_association_independent_2d_points,
207  *data_association_independent_3d_points
208  */
210  const mrpt::math::CMatrixDouble& Z_observations_mean,
211  const mrpt::math::CMatrixDouble& Y_predictions_mean,
212  const mrpt::math::CMatrixDouble& Y_predictions_cov,
214  const TDataAssociationMethod method = assocJCBB,
215  const TDataAssociationMetric metric = metricMaha,
216  const double chi2quantile = 0.99, const bool DAT_ASOC_USE_KDTREE = true,
217  const std::vector<prediction_index_t>& predictions_IDs =
218  std::vector<prediction_index_t>(),
219  const TDataAssociationMetric compatibilityTestMetric = metricMaha,
220  const double log_ML_compat_test_threshold = 0.0);
221 
222 /** @} */
223 
224 /** @} */ // end of grouping
225 
226 } // namespace mrpt::slam
228 using namespace mrpt::slam;
232 
234 using namespace mrpt::slam;
Nearest-neighbor.
Mahalanobis distance.
size_t prediction_index_t
Used in mrpt::slam::TDataAssociationResults.
mrpt::math::CMatrixDouble indiv_distances
Individual mahalanobis distances (or matching likelihood, depending on the selected metric) between p...
MRPT_FILL_ENUM(assocNN)
JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001].
double distance
The Joint Mahalanobis distance or matching likelihood of the best associations found.
map< string, CVectorDouble > results
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
mrpt::math::CMatrixBool indiv_compatibility
The result of a chi2 test for compatibility using mahalanobis distance - Indices are like in "indiv_d...
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
void 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.
size_t observation_index_t
Used in mrpt::slam::TDataAssociationResults.
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...
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
size_t nNodesExploredInJCBB
Only for the JCBB method,the number of recursive calls expent in the algorithm.
The results from mrpt::slam::data_association.
Matching likelihood (See TDataAssociationMetric for a paper explaining this metric) ...
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
void 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...
std::vector< uint32_t > indiv_compatibility_counts
The sum of each column of indiv_compatibility, that is, the number of compatible pairings for each ob...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020