MRPT  1.9.9
data_association.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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 
10 #include "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/KDTreeCapable.h> // For kd-tree's
13 #include <mrpt/math/data_utils.h>
14 #include <mrpt/math/distributions.h> // for chi2inv
15 #include <mrpt/math/ops_matrices.h> // extractSubmatrix
19 #include <Eigen/Dense>
20 #include <memory>
21 #include <memory> // unique_ptr
22 #include <nanoflann.hpp> // For kd-tree's
23 #include <numeric> // accumulate
24 #include <set>
25 
26 /*
27  For all data association algorithms, the individual compatibility is
28  estabished by
29  the chi2 test (Mahalanobis distance), but later on one of the potential
30  pairings is
31  picked by the metric given by the user (maha vs. match. lik.)
32 
33  Related papers:
34  - Matching likelihood. See: https://www.mrpt.org/Paper:Matching_Likelihood
35 
36  - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
37 
38 */
39 
40 using namespace std;
41 using namespace mrpt;
42 using namespace mrpt::math;
43 using namespace mrpt::poses;
44 using namespace mrpt::slam;
45 
46 namespace mrpt::slam
47 {
49 {
50  /** Just to avoid recomputing them all the time. */
51  size_t nPredictions, nObservations, length_O;
52  std::map<size_t, size_t> currentAssociation;
53 };
54 
55 /** Computes the joint distance metric (mahalanobis or matching likelihood)
56  * between two a set of associations
57  *
58  * On "currentAssociation": maps "ID_obs" -> "ID_pred"
59  * For each landmark ID in the observations (ID_obs), its association
60  * in the predictions, that is: ID_pred = associations[ID_obs]
61  *
62  */
63 template <typename T, TDataAssociationMetric METRIC>
65  const CMatrixDynamic<T>& Z_observations_mean,
66  const CMatrixDynamic<T>& Y_predictions_mean,
67  const CMatrixDynamic<T>& Y_predictions_cov,
68  const TAuxDataRecursiveJCBB& info, const TDataAssociationResults& aux_data)
69 {
70  MRPT_UNUSED_PARAM(aux_data);
71  // Make a list of the indices of the predictions that appear in
72  // "currentAssociation":
73  const size_t N = info.currentAssociation.size();
74  ASSERT_(N > 0);
75  std::vector<size_t> indices_pred(
76  N); // Appearance order indices in the std::maps
77  std::vector<size_t> indices_obs(N);
78 
79  {
80  size_t i = 0;
81  for (auto it : info.currentAssociation)
82  {
83  indices_obs[i] = it.first;
84  indices_pred[i] = it.second;
85  i++;
86  }
87  }
88 
89  // ----------------------------------------------------------------------
90  // Extract submatrix of the covariances involved here:
91  // COV = PREDICTIONS_COV(INDX,INDX) + OBSERVATIONS_COV(INDX2,INDX2)
92  // ----------------------------------------------------------------------
95  Y_predictions_cov,
96  info.length_O, // dims of cov. submatrices
97  indices_pred, COV);
98 
99  // ----------------------------------------------------------------------
100  // Mean:
101  // The same for the vector of "errors" or "innovation" between predictions
102  // and observations:
103  // ----------------------------------------------------------------------
104  Eigen::Matrix<T, Eigen::Dynamic, 1> innovations(N * info.length_O);
105  T* dst_ptr = &innovations[0];
106  for (auto it = info.currentAssociation.begin();
107  it != info.currentAssociation.end(); ++it)
108  {
109  const T* pred_i_mean = &Y_predictions_mean(it->second, 0);
110  const T* obs_i_mean = &Z_observations_mean(it->first, 0);
111 
112  for (unsigned int k = 0; k < info.length_O; k++)
113  *dst_ptr++ = pred_i_mean[k] - obs_i_mean[k];
114  }
115 
116  // Compute mahalanobis distance squared:
117  const CMatrixDynamic<T> COV_inv = COV.inverse_LLt();
118 
119  const double d2 = mrpt::math::multiply_HtCH_scalar(innovations, COV_inv);
120 
121  if (METRIC == metricMaha) return d2;
122 
123  ASSERT_(METRIC == metricML);
124 
125  // Matching likelihood: The evaluation at 0 of the PDF of the difference
126  // between the two Gaussians:
127  const T cov_det = COV.det();
128  const double ml = exp(-0.5 * d2) / (std::pow(M_2PI, info.length_O * 0.5) *
129  std::sqrt(cov_det));
130  return ml;
131 }
132 
133 template <TDataAssociationMetric METRIC>
134 bool isCloser(const double v1, const double v2);
135 
136 template <>
137 bool isCloser<metricMaha>(const double v1, const double v2)
138 {
139  return v1 < v2;
140 }
141 
142 template <>
143 bool isCloser<metricML>(const double v1, const double v2)
144 {
145  return v1 > v2;
146 }
147 
148 /* Based on MATLAB code by:
149  University of Zaragoza
150  Centro Politecnico Superior
151  Robotics and Real Time Group
152  Authors of the original MATLAB code: J. Neira, J. Tardos
153  C++ version: J.L. Blanco Claraco
154 */
155 template <typename T, TDataAssociationMetric METRIC>
157  const mrpt::math::CMatrixDynamic<T>& Z_observations_mean,
158  const mrpt::math::CMatrixDynamic<T>& Y_predictions_mean,
159  const mrpt::math::CMatrixDynamic<T>& Y_predictions_cov,
161  const observation_index_t curObsIdx)
162 {
163  // End of iteration?
164  if (curObsIdx >= info.nObservations)
165  {
166  if (info.currentAssociation.size() > results.associations.size())
167  {
168  // It's a better choice since more features are matched.
169  results.associations = info.currentAssociation;
170  results.distance = joint_pdf_metric<T, METRIC>(
171  Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
172  info, results);
173  }
174  else if (
175  !info.currentAssociation.empty() &&
176  info.currentAssociation.size() == results.associations.size())
177  {
178  // The same # of features matched than the previous best one...
179  // decide by better distance:
180  const double d2 = joint_pdf_metric<T, METRIC>(
181  Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
182  info, results);
183 
184  if (isCloser<METRIC>(d2, results.distance))
185  {
186  results.associations = info.currentAssociation;
187  results.distance = d2;
188  }
189  }
190  }
191  else // A normal iteration:
192  {
193  // Iterate for all compatible landmarsk of "curObsIdx"
194  const observation_index_t obsIdx = curObsIdx;
195 
196  const size_t nPreds = results.indiv_compatibility.rows();
197 
198  // Can we do it better than the current "results.associations"?
199  // This can be checked by counting the potential new pairings+the so-far
200  // established ones.
201  // Matlab: potentials = pairings(compatibility.AL(i+1:end))
202  // Moved up by Kasra Khosoussi
203  const size_t potentials = std::accumulate(
204  results.indiv_compatibility_counts.begin() + (obsIdx + 1),
205  results.indiv_compatibility_counts.end(), 0);
206  for (prediction_index_t predIdx = 0; predIdx < nPreds; predIdx++)
207  {
208  if ((info.currentAssociation.size() + potentials) >=
209  results.associations.size())
210  {
211  // Only if predIdx is NOT already assigned:
212  if (results.indiv_compatibility(predIdx, obsIdx))
213  {
214  // Only if predIdx is NOT already assigned:
215  bool already_asigned = false;
216  for (auto itS : info.currentAssociation)
217  {
218  if (itS.second == predIdx)
219  {
220  already_asigned = true;
221  break;
222  }
223  }
224 
225  if (!already_asigned)
226  {
227  // Launch a new recursive line for this hipothesis:
228  TAuxDataRecursiveJCBB new_info = info;
229  new_info.currentAssociation[curObsIdx] = predIdx;
230 
231  results.nNodesExploredInJCBB++;
232 
233  JCBB_recursive<T, METRIC>(
234  Z_observations_mean, Y_predictions_mean,
235  Y_predictions_cov, results, new_info,
236  curObsIdx + 1);
237  }
238  }
239  }
240  }
241 
242  // Can we do it better than the current "results.associations"?
243  if ((info.currentAssociation.size() + potentials) >=
244  results.associations.size())
245  {
246  // Yes we can </obama>
247 
248  // star node: Ei not paired
249  results.nNodesExploredInJCBB++;
250  JCBB_recursive<T, METRIC>(
251  Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
252  results, info, curObsIdx + 1);
253  }
254  }
255 }
256 
257 } // namespace mrpt::slam
258 
259 /* ==================================================================================================
260 Computes the data-association between the prediction of a set of landmarks and
261 their observations, all of them with covariance matrices.
262 * Implemented methods include (see TDataAssociation)
263 * - NN: Nearest-neighbor
264 * - JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]
265 *
266 * With both a Mahalanobis-distance or Matching-likelihood metric (See paper:
267 http://www.mrpt.org/Paper:Matching_Likelihood )
268 *
269 * \param Z_observations_mean [IN] An MxO matrix with the M observations, each
270 row containing the observation "mean".
271 * \param Y_predictions_mean [IN ] An NxO matrix with the N predictions, each row
272 containing the mean of one prediction.
273 * \param Y_predictions_cov [IN ] An N·OxN·O matrix with the full covariance
274 matrix of all the N predictions.
275 
276 * \param predictions_mean [IN] The list of predicted locations of
277 landmarks/features, indexed by their ID. The 2D/3D locations are in the same
278 coordinate framework than "observations".
279 * \param predictions_cov [IN] The full covariance matrix of predictions, in
280 blocks of 2x2 matrices. The order of the submatrices is the appearance order of
281 lanmarks in "predictions_mean".
282 * \param results [OUT] The output data association hypothesis, and other useful
283 information.
284 * \param method [IN, optional] The selected method to make the associations.
285 * \param chi2quantile [IN, optional] The threshold for considering a match
286 between two close Gaussians for two landmarks, in the range [0,1]. It is used to
287 call mrpt::math::chi2inv
288 * \param use_kd_tree [IN, optional] Build a KD-tree to speed-up the evaluation
289 of individual compatibility (IC). It's perhaps more efficient to disable it for
290 a small number of features. (default=true).
291 * \param predictions_IDs [IN, optional] (default:none) An N-vector. If provided,
292 the resulting associations in "results.associations" will not contain prediction
293 indices "i", but "predictions_IDs[i]".
294 *
295  ==================================================================================================
296 */
298  const mrpt::math::CMatrixDouble& Z_observations_mean,
299  const mrpt::math::CMatrixDouble& Y_predictions_mean,
300  const mrpt::math::CMatrixDouble& Y_predictions_cov,
302  const TDataAssociationMetric metric, const double chi2quantile,
303  const bool DAT_ASOC_USE_KDTREE,
304  const std::vector<prediction_index_t>& predictions_IDs,
305  const TDataAssociationMetric compatibilityTestMetric,
306  const double log_ML_compat_test_threshold)
307 {
308  // For details on the theory, see the papers cited at the beginning of this
309  // file.
310 
312 
313  MRPT_START
314 
315  results.clear();
316 
317  const size_t nPredictions = Y_predictions_mean.rows();
318  const size_t nObservations = Z_observations_mean.rows();
319 
320  const size_t length_O = Z_observations_mean.cols();
321 
322  ASSERT_(nPredictions != 0);
323  ASSERT_(nObservations != 0);
324  ASSERT_(length_O == (size_t)Y_predictions_mean.cols());
325  ASSERT_(length_O * nPredictions == (size_t)Y_predictions_cov.rows());
326  ASSERT_(Y_predictions_cov.isSquare());
327  ASSERT_(chi2quantile > 0 && chi2quantile < 1);
328  ASSERT_(metric == metricMaha || metric == metricML);
329  const double chi2thres = mrpt::math::chi2inv(chi2quantile, length_O);
330 
331  // ------------------------------------------------------------
332  // Build a KD-tree of the predictions for quick look-up:
333  // ------------------------------------------------------------
334  using KDTreeMatrixPtr =
335  std::unique_ptr<KDTreeEigenMatrixAdaptor<CMatrixDouble>>;
336  KDTreeMatrixPtr kd_tree;
337  const size_t N_KD_RESULTS = nPredictions;
338  std::vector<double> kd_result_distances(
339  DAT_ASOC_USE_KDTREE ? N_KD_RESULTS : 0);
340  std::vector<CMatrixDouble::Index> kd_result_indices(
341  DAT_ASOC_USE_KDTREE ? N_KD_RESULTS : 0);
342  std::vector<double> kd_queryPoint(DAT_ASOC_USE_KDTREE ? length_O : 0);
343 
344  if (DAT_ASOC_USE_KDTREE)
345  {
346  // Construct kd-tree for the predictions:
347  kd_tree = std::make_unique<KDTreeEigenMatrixAdaptor<CMatrixDouble>>(
348  length_O, Y_predictions_mean);
349  }
350 
351  // Initialize with the worst possible distance:
352  results.distance =
353  (metric == metricML) ? 0 : std::numeric_limits<double>::max();
354 
355  //-------------------------------------------
356  // Compute the individual compatibility:
357  //-------------------------------------------
358  results.indiv_distances.resize(nPredictions, nObservations);
359  results.indiv_compatibility.setSize(nPredictions, nObservations);
360  results.indiv_compatibility_counts.assign(nObservations, 0);
361 
362  results.indiv_distances.fill(
363  metric == metricMaha ? 1000 /*A very large Sq. Maha. Dist. */
364  : -1000 /*A very small log-likelihoo */);
365  results.indiv_compatibility.fill(false);
366 
367  CMatrixDouble pred_i_cov(length_O, length_O);
368 
369  Eigen::VectorXd diff_means_i_j(length_O);
370 
371  for (size_t j = 0; j < nObservations; ++j)
372  {
373  if (!DAT_ASOC_USE_KDTREE)
374  {
375  // Compute all the distances w/o a KD-tree
376  for (size_t i = 0; i < nPredictions; ++i)
377  {
378  // Evaluate sqr. mahalanobis distance of obs_j -> pred_i:
379  // Extract the submatrix from the diagonal:
380  const size_t pred_cov_idx = i * length_O;
381  pred_i_cov = Y_predictions_cov.asEigen().block(
382  pred_cov_idx, pred_cov_idx, length_O, length_O);
383 
384  for (size_t k = 0; k < length_O; k++)
385  diff_means_i_j[k] =
386  Z_observations_mean(j, k) - Y_predictions_mean(i, k);
387 
388  double d2, ml;
389  // mrpt::math::productIntegralAndMahalanobisTwoGaussians(diff_means_i_j,pred_i_cov,obs_j_cov,
390  // d2,ml);
392  diff_means_i_j, pred_i_cov, d2, ml);
393 
394  // The distance according to the metric
395  double val = (metric == metricMaha) ? d2 : ml;
396 
397  results.indiv_distances(i, j) = val;
398 
399  // Individual compatibility
400  const bool IC = (compatibilityTestMetric == metricML)
401  ? (ml > log_ML_compat_test_threshold)
402  : (d2 < chi2thres);
403  results.indiv_compatibility(i, j) = IC;
404  if (IC) results.indiv_compatibility_counts[j]++;
405  }
406  }
407  else
408  {
409  // Use a kd-tree and compute only the N closest ones:
410  for (size_t k = 0; k < length_O; k++)
411  kd_queryPoint[k] = Z_observations_mean(j, k);
412 
413  kd_tree->query(
414  &kd_queryPoint[0], N_KD_RESULTS, &kd_result_indices[0],
415  &kd_result_distances[0]);
416 
417  // Only compute the distances for these ones:
418  for (size_t w = 0; w < N_KD_RESULTS; w++)
419  {
420  const size_t i = kd_result_indices[w]; // This is the index of
421  // the prediction in
422  // "predictions_mean"
423 
424  // Build the PDF of the prediction:
425  // Extract the submatrix from the diagonal:
426  const size_t pred_cov_idx = i * length_O;
427  pred_i_cov = Y_predictions_cov.asEigen().block(
428  pred_cov_idx, pred_cov_idx, length_O, length_O);
429 
430  for (size_t k = 0; k < length_O; k++)
431  diff_means_i_j[k] =
432  Z_observations_mean(j, k) - Y_predictions_mean(i, k);
433 
434  double d2, ml;
435  // mrpt::math::productIntegralAndMahalanobisTwoGaussians(diff_means_i_j,pred_i_cov,obs_j_cov,
436  // d2,ml);
438  diff_means_i_j, pred_i_cov, d2, ml);
439 
440  if (d2 > 6 * chi2thres)
441  break; // Since kd-tree returns the landmarks by distance
442  // order, we can skip the rest
443 
444  // The distance according to the metric
445  double val = (metric == metricMaha) ? d2 : ml;
446 
447  results.indiv_distances(i, j) = val;
448 
449  // Individual compatibility
450  const bool IC = (compatibilityTestMetric == metricML)
451  ? (ml > log_ML_compat_test_threshold)
452  : (d2 < chi2thres);
453  results.indiv_compatibility(i, j) = IC;
454  if (IC) results.indiv_compatibility_counts[j]++;
455  }
456  } // end use KD-Tree
457  } // end for
458 
459 #if 0
460  cout << "Distances: " << endl << results.indiv_distances << endl;
461  //cout << "indiv compat: " << endl << results.indiv_compatibility << endl;
462 #endif
463 
464  // Do associations:
465  results.associations.clear();
466 
467  switch (method)
468  {
469  // --------------------------
470  // Nearest-neighbor
471  // --------------------------
472  case assocNN:
473  {
474  // 1) For each observation "j", make a list of the indiv. compatible
475  // predictions and their distances, sorted first the best.
476  // NOTE: distances are saved so smaller is always better,
477  // hence "metricML" are made negative.
478  // -------------------------------------------------------------------
479  using TListAllICs = multimap<
480  double,
481  pair<
482  observation_index_t, multimap<double, prediction_index_t>>>;
483  TListAllICs lst_all_ICs;
484 
485  for (observation_index_t j = 0; j < nObservations; ++j)
486  {
487  multimap<double, prediction_index_t> ICs;
488 
489  for (prediction_index_t i = 0; i < nPredictions; ++i)
490  {
491  if (results.indiv_compatibility(i, j))
492  {
493  double d2 = results.indiv_distances(i, j);
494  if (metric == metricML) d2 = -d2;
495  ICs.insert(make_pair(d2, i));
496  }
497  }
498 
499  if (!ICs.empty())
500  {
501  const double best_dist = ICs.begin()->first;
502  lst_all_ICs.insert(make_pair(best_dist, make_pair(j, ICs)));
503  }
504  }
505 
506  // 2) With that lists, start by the best one and make the
507  // assignment.
508  // Remove the prediction from the list of available, and go on.
509  // --------------------------------------------------------------------
510  std::set<prediction_index_t> lst_already_taken_preds;
511 
512  for (auto it = lst_all_ICs.begin(); it != lst_all_ICs.end(); ++it)
513  {
514  const observation_index_t obs_id = it->second.first;
515  const multimap<double, prediction_index_t>& lstCompats =
516  it->second.second;
517 
518  for (auto lstCompat : lstCompats)
519  {
520  if (lst_already_taken_preds.find(lstCompat.second) ==
521  lst_already_taken_preds.end())
522  {
523  // It's free: make the association:
524  results.associations[obs_id] = lstCompat.second;
525  lst_already_taken_preds.insert(lstCompat.second);
526  break;
527  }
528  }
529  }
530  }
531  break;
532 
533  // ------------------------------------
534  // Joint Compatibility Branch & Bound:
535  // ------------------------------------
536  case assocJCBB:
537  {
538  // Call to the recursive method:
540  info.nPredictions = nPredictions;
541  info.nObservations = nObservations;
542  info.length_O = length_O;
543 
544  if (metric == metricMaha)
545  JCBB_recursive<CMatrixDouble::Scalar, metricMaha>(
546  Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
547  results, info, 0);
548  else
549  JCBB_recursive<CMatrixDouble::Scalar, metricML>(
550  Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
551  results, info, 0);
552  }
553  break;
554 
555  default:
556  THROW_EXCEPTION("Unknown value of 'method'");
557  };
558 
559  // If a mapping of prediction indices to IDs was providen, apply it now:
560  // ------------------------------------------------------------------------
561  if (!predictions_IDs.empty())
562  {
563  ASSERT_(predictions_IDs.size() == nPredictions);
564  for (auto& association : results.associations)
565  association.second = predictions_IDs[association.second];
566  }
567 
568  MRPT_END
569 }
570 
571 /* ==================================================================================================
572  data_association_independent_predictions
573  ==================================================================================================
574  */
576  const mrpt::math::CMatrixDouble& Z_observations_mean,
577  const mrpt::math::CMatrixDouble& Y_predictions_mean,
578  const mrpt::math::CMatrixDouble& Y_predictions_cov_stacked,
580  const TDataAssociationMetric metric, const double chi2quantile,
581  const bool DAT_ASOC_USE_KDTREE,
582  const std::vector<prediction_index_t>& predictions_IDs,
583  const TDataAssociationMetric compatibilityTestMetric,
584  const double log_ML_compat_test_threshold)
585 {
586  MRPT_START
587 
588  results.clear();
589 
590  const size_t nPredictions = Y_predictions_mean.rows();
591  const size_t nObservations = Z_observations_mean.rows();
592 
593  const size_t length_O = Z_observations_mean.cols();
594 
595  ASSERT_(nPredictions != 0);
596  ASSERT_(nObservations != 0);
597  ASSERT_(length_O == (size_t)Y_predictions_mean.cols());
598  ASSERT_(
599  length_O * nPredictions == (size_t)Y_predictions_cov_stacked.rows());
600  ASSERT_(chi2quantile > 0 && chi2quantile < 1);
601  ASSERT_(metric == metricMaha || metric == metricML);
602  // const double chi2thres = mrpt::math::chi2inv( chi2quantile, length_O );
603 
604  // TODO: Optimized version!!
605  CMatrixDouble Y_predictions_cov_full(
606  length_O * nPredictions, length_O * nPredictions);
607  CMatrixDouble COV_i(length_O, length_O);
608  for (size_t i = 0; i < nPredictions; i++)
609  {
610  const size_t idx = i * length_O;
611  COV_i =
612  Y_predictions_cov_stacked.extractMatrix(length_O, length_O, idx, 0);
613  Y_predictions_cov_full.insertMatrix(idx, idx, COV_i);
614  }
615 
617  Z_observations_mean, Y_predictions_mean, Y_predictions_cov_full,
618  results, method, metric, chi2quantile, DAT_ASOC_USE_KDTREE,
619  predictions_IDs, compatibilityTestMetric, log_ML_compat_test_threshold);
620 
621  MRPT_END
622 }
bool isCloser< metricML >(const double v1, const double v2)
An L2-metric KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage.
Definition: nanoflann.hpp:1307
Nearest-neighbor.
MAT_C::Scalar multiply_HtCH_scalar(const VECTOR_H &H, const MAT_C &C)
r (scalar) = H^t*C*H (H: column vector, C: symmetric matrix)
Definition: ops_matrices.h:54
#define MRPT_START
Definition: exceptions.h:241
Mahalanobis distance.
#define M_2PI
Definition: common.h:58
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
size_t prediction_index_t
Used in mrpt::slam::TDataAssociationResults.
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
Definition: MatrixBase.h:210
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
bool isSquare() const
returns true if matrix is NxN
STL namespace.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
size_t nPredictions
Just to avoid recomputing them all the time.
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > extractMatrix(const int start_row=0, const int start_col=0) const
Definition: MatrixBase.h:247
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
bool isCloser< metricMaha >(const double v1, const double v2)
JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001].
This base provides a set of functions for maths stuff.
map< string, CVectorDouble > results
void mahalanobisDistance2AndLogPDF(const VECLIKE &diff_mean, const MATRIXLIKE &cov, T &maha2_out, T &log_pdf_out)
Computes both, the logarithm of the PDF and the square Mahalanobis distance between a point (given by...
Definition: data_utils.h:209
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
int val
Definition: mrpt_jpeglib.h:957
std::map< size_t, size_t > currentAssociation
CMatrixDynamic< T > inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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.
void extractSubmatrixSymmetricalBlocksDyn(const MAT &m, const std::size_t BLOCKSIZE, const std::vector< size_t > &block_indices, MATRIX &out)
Definition: ops_matrices.h:266
double chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
Definition: math.cpp:42
size_t observation_index_t
Used in mrpt::slam::TDataAssociationResults.
GLfloat GLfloat v1
Definition: glext.h:4121
#define MRPT_END
Definition: exceptions.h:245
void JCBB_recursive(const mrpt::math::CMatrixDynamic< T > &Z_observations_mean, const mrpt::math::CMatrixDynamic< T > &Y_predictions_mean, const mrpt::math::CMatrixDynamic< T > &Y_predictions_cov, TDataAssociationResults &results, const TAuxDataRecursiveJCBB &info, const observation_index_t curObsIdx)
GLfloat GLfloat GLfloat v2
Definition: glext.h:4123
The results from mrpt::slam::data_association.
double joint_pdf_metric(const CMatrixDynamic< T > &Z_observations_mean, const CMatrixDynamic< T > &Y_predictions_mean, const CMatrixDynamic< T > &Y_predictions_cov, const TAuxDataRecursiveJCBB &info, const TDataAssociationResults &aux_data)
Computes the joint distance metric (mahalanobis or matching likelihood) between two a set of associat...
Matching likelihood (See TDataAssociationMetric for a paper explaining this metric) ...
TDataAssociationMethod
Different algorithms for data association, used in mrpt::slam::data_association.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
This template class provides the basic functionality for a general 2D any-size, resizable container o...
bool isCloser(const double v1, const double v2)
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...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 836b840ab Mon Nov 18 00:58:29 2019 +0100 at lun nov 18 01:00:14 CET 2019