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