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



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019