62 template <
typename T, TDataAssociationMetric METRIC>
74 std::vector<size_t> indices_pred(
76 std::vector<size_t> indices_obs(N);
84 indices_obs[i] = it->first;
85 indices_pred[i] = it->second;
94 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> COV;
95 Y_predictions_cov.extractSubmatrixSymmetricalBlocks(
104 Eigen::Matrix<T, Eigen::Dynamic, 1> innovations(N * info.
length_O);
105 T* dst_ptr = &innovations[0];
110 const T* pred_i_mean = Y_predictions_mean.get_unsafe_row(it->second);
111 const T* obs_i_mean = Z_observations_mean.get_unsafe_row(it->first);
113 for (
unsigned int k = 0; k < info.
length_O; k++)
114 *dst_ptr++ = pred_i_mean[k] - obs_i_mean[k];
119 COV.inv_fast(COV_inv);
129 const T cov_det = COV.det();
130 const double ml = exp(-0.5 * d2) / (std::pow(
M_2PI, info.
length_O * 0.5) *
135 template <TDataAssociationMetric METRIC>
157 template <
typename T, TDataAssociationMetric METRIC>
172 results.distance = joint_pdf_metric<T, METRIC>(
173 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
182 const double d2 = joint_pdf_metric<T, METRIC>(
183 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
186 if (isCloser<METRIC>(d2,
results.distance))
188 results.associations = info.currentAssociation;
198 const size_t nPreds =
results.indiv_compatibility.rows();
205 const size_t potentials = std::accumulate(
206 results.indiv_compatibility_counts.begin() + (obsIdx + 1),
207 results.indiv_compatibility_counts.end(), 0);
214 if (
results.indiv_compatibility(predIdx, obsIdx))
217 bool already_asigned =
false;
222 if (itS->second == predIdx)
224 already_asigned =
true;
229 if (!already_asigned)
235 results.nNodesExploredInJCBB++;
237 JCBB_recursive<T, METRIC>(
238 Z_observations_mean, Y_predictions_mean,
239 Y_predictions_cov,
results, new_info,
253 results.nNodesExploredInJCBB++;
254 JCBB_recursive<T, METRIC>(
255 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
307 const bool DAT_ASOC_USE_KDTREE,
308 const std::vector<prediction_index_t>& predictions_IDs,
310 const double log_ML_compat_test_threshold)
321 const size_t nPredictions = Y_predictions_mean.rows();
322 const size_t nObservations = Z_observations_mean.rows();
324 const size_t length_O = Z_observations_mean.cols();
328 ASSERT_(length_O == (
size_t)Y_predictions_mean.cols());
329 ASSERT_(length_O * nPredictions == (
size_t)Y_predictions_cov.rows());
330 ASSERT_(Y_predictions_cov.isSquare());
331 ASSERT_(chi2quantile > 0 && chi2quantile < 1);
338 using KDTreeMatrixPtr =
339 std::unique_ptr<KDTreeEigenMatrixAdaptor<CMatrixDouble>>;
340 KDTreeMatrixPtr kd_tree;
341 const size_t N_KD_RESULTS = nPredictions;
342 std::vector<double> kd_result_distances(
343 DAT_ASOC_USE_KDTREE ? N_KD_RESULTS : 0);
344 std::vector<CMatrixDouble::Index> kd_result_indices(
345 DAT_ASOC_USE_KDTREE ? N_KD_RESULTS : 0);
346 std::vector<double> kd_queryPoint(DAT_ASOC_USE_KDTREE ? length_O : 0);
348 if (DAT_ASOC_USE_KDTREE)
351 kd_tree = KDTreeMatrixPtr(
new KDTreeEigenMatrixAdaptor<CMatrixDouble>(
352 length_O, Y_predictions_mean));
357 (metric ==
metricML) ? 0 : std::numeric_limits<double>::max();
362 results.indiv_distances.resize(nPredictions, nObservations);
363 results.indiv_compatibility.setSize(nPredictions, nObservations);
364 results.indiv_compatibility_counts.assign(nObservations, 0);
369 results.indiv_compatibility.fillAll(
false);
373 Eigen::VectorXd diff_means_i_j(length_O);
375 for (
size_t j = 0; j < nObservations; ++j)
377 if (!DAT_ASOC_USE_KDTREE)
380 for (
size_t i = 0; i < nPredictions; ++i)
383 const size_t pred_cov_idx =
385 Y_predictions_cov.extractMatrix(
386 pred_cov_idx, pred_cov_idx, length_O, length_O, pred_i_cov);
388 for (
size_t k = 0; k < length_O; k++)
389 diff_means_i_j[k] = Z_observations_mean.get_unsafe(j, k) -
390 Y_predictions_mean.get_unsafe(i, k);
396 diff_means_i_j, pred_i_cov, d2, ml);
404 const bool IC = (compatibilityTestMetric ==
metricML)
405 ? (ml > log_ML_compat_test_threshold)
407 results.indiv_compatibility(i, j) = IC;
408 if (IC)
results.indiv_compatibility_counts[j]++;
414 for (
size_t k = 0; k < length_O; k++)
415 kd_queryPoint[k] = Z_observations_mean.get_unsafe(j, k);
418 &kd_queryPoint[0], N_KD_RESULTS, &kd_result_indices[0],
419 &kd_result_distances[0]);
422 for (
size_t w = 0;
w < N_KD_RESULTS;
w++)
424 const size_t i = kd_result_indices[
w];
429 const size_t pred_cov_idx =
431 Y_predictions_cov.extractMatrix(
432 pred_cov_idx, pred_cov_idx, length_O, length_O, pred_i_cov);
434 for (
size_t k = 0; k < length_O; k++)
435 diff_means_i_j[k] = Z_observations_mean.get_unsafe(j, k) -
436 Y_predictions_mean.get_unsafe(i, k);
442 diff_means_i_j, pred_i_cov, d2, ml);
444 if (d2 > 6 * chi2thres)
454 const bool IC = (compatibilityTestMetric ==
metricML)
455 ? (ml > log_ML_compat_test_threshold)
457 results.indiv_compatibility(i, j) = IC;
458 if (IC)
results.indiv_compatibility_counts[j]++;
464 cout <<
"Distances: " << endl <<
results.indiv_distances << endl;
483 using TListAllICs = multimap<
487 TListAllICs lst_all_ICs;
491 multimap<double, prediction_index_t> ICs;
495 if (
results.indiv_compatibility.get_unsafe(i, j))
497 double d2 =
results.indiv_distances.get_unsafe(i, j);
499 ICs.insert(make_pair(d2, i));
505 const double best_dist = ICs.begin()->first;
506 lst_all_ICs.insert(make_pair(best_dist, make_pair(j, ICs)));
514 std::set<prediction_index_t> lst_already_taken_preds;
517 it != lst_all_ICs.end(); ++it)
520 const multimap<double, prediction_index_t>& lstCompats =
525 itP != lstCompats.end(); ++itP)
527 if (lst_already_taken_preds.find(itP->second) ==
528 lst_already_taken_preds.end())
531 results.associations[obs_id] = itP->second;
532 lst_already_taken_preds.insert(itP->second);
552 JCBB_recursive<CMatrixDouble::Scalar, metricMaha>(
553 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
556 JCBB_recursive<CMatrixDouble::Scalar, metricML>(
557 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
568 if (!predictions_IDs.empty())
570 ASSERT_(predictions_IDs.size() == nPredictions);
573 itAssoc !=
results.associations.end(); ++itAssoc)
574 itAssoc->second = predictions_IDs[itAssoc->second];
590 const bool DAT_ASOC_USE_KDTREE,
591 const std::vector<prediction_index_t>& predictions_IDs,
593 const double log_ML_compat_test_threshold)
599 const size_t nPredictions = Y_predictions_mean.rows();
600 const size_t nObservations = Z_observations_mean.rows();
602 const size_t length_O = Z_observations_mean.cols();
606 ASSERT_(length_O == (
size_t)Y_predictions_mean.cols());
608 length_O * nPredictions == (
size_t)Y_predictions_cov_stacked.rows());
609 ASSERT_(chi2quantile > 0 && chi2quantile < 1);
615 length_O * nPredictions, length_O * nPredictions);
617 for (
size_t i = 0; i < nPredictions; i++)
619 const size_t idx = i * length_O;
620 Y_predictions_cov_stacked.extractSubmatrix(
621 idx, idx + length_O - 1, 0, length_O - 1, COV_i);
622 Y_predictions_cov_full.insertMatrix(idx, idx, COV_i);
626 Z_observations_mean, Y_predictions_mean, Y_predictions_cov_full,
627 results, method, metric, chi2quantile, DAT_ASOC_USE_KDTREE,
628 predictions_IDs, compatibilityTestMetric, log_ML_compat_test_threshold);
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.
#define THROW_EXCEPTION(msg)
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)
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
size_t nPredictions
Just to avoid recomputing them all the time.
#define ASSERT_(f)
Defines an assertion mechanism.
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...
TDataAssociationMetric
Different metrics for data association, used in mrpt::slam::data_association For a comparison of both...
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...
size_t observation_index_t
Used in mrpt::slam::TDataAssociationResults.
A matrix of dynamic size.
GLfloat GLfloat GLfloat v2
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
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.