36 #include <mrpt/otherlibs/nanoflann/nanoflann.hpp> 65 template <
typename T, TDataAssociationMetric METRIC>
87 indices_obs[i] = it->first;
88 indices_pred[i] = it->second;
97 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> COV;
98 Y_predictions_cov.extractSubmatrixSymmetricalBlocks(
107 Eigen::Matrix<T, Eigen::Dynamic, 1> innovations(N * info.
length_O);
108 T* dst_ptr = &innovations[0];
113 const T* pred_i_mean = Y_predictions_mean.get_unsafe_row(it->second);
114 const T* obs_i_mean = Z_observations_mean.get_unsafe_row(it->first);
116 for (
unsigned int k = 0; k < info.
length_O; k++)
117 *dst_ptr++ = pred_i_mean[k] - obs_i_mean[k];
122 COV.inv_fast(COV_inv);
132 const T cov_det = COV.det();
133 const double ml = exp(-0.5 * d2) / (std::pow(
M_2PI, info.
length_O * 0.5) *
138 template <TDataAssociationMetric METRIC>
160 template <
typename T, TDataAssociationMetric METRIC>
175 results.
distance = joint_pdf_metric<T, METRIC>(
176 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
185 const double d2 = joint_pdf_metric<T, METRIC>(
186 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
189 if (isCloser<METRIC>(d2, results.
distance))
208 const size_t potentials = std::accumulate(
220 bool already_asigned =
false;
225 if (itS->second == predIdx)
227 already_asigned =
true;
232 if (!already_asigned)
240 JCBB_recursive<T, METRIC>(
241 Z_observations_mean, Y_predictions_mean,
242 Y_predictions_cov, results, new_info,
257 JCBB_recursive<T, METRIC>(
258 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
259 results, info, curObsIdx + 1);
311 const bool DAT_ASOC_USE_KDTREE,
312 const std::vector<prediction_index_t>& predictions_IDs,
314 const double log_ML_compat_test_threshold)
319 using nanoflann::KDTreeEigenMatrixAdaptor;
325 const size_t nPredictions =
size(Y_predictions_mean, 1);
326 const size_t nObservations =
size(Z_observations_mean, 1);
328 const size_t length_O =
size(Z_observations_mean, 2);
333 ASSERT_(length_O * nPredictions ==
size(Y_predictions_cov, 1))
334 ASSERT_(Y_predictions_cov.isSquare())
336 ASSERT_(chi2quantile > 0 && chi2quantile < 1)
344 typedef std::unique_ptr<KDTreeEigenMatrixAdaptor<CMatrixDouble>>
346 KDTreeMatrixPtr kd_tree;
347 const size_t N_KD_RESULTS = nPredictions;
348 std::vector<double> kd_result_distances(
349 DAT_ASOC_USE_KDTREE ? N_KD_RESULTS : 0);
350 std::vector<CMatrixDouble::Index> kd_result_indices(
351 DAT_ASOC_USE_KDTREE ? N_KD_RESULTS : 0);
352 std::vector<double> kd_queryPoint(DAT_ASOC_USE_KDTREE ? length_O : 0);
354 if (DAT_ASOC_USE_KDTREE)
357 kd_tree = KDTreeMatrixPtr(
358 new KDTreeEigenMatrixAdaptor<CMatrixDouble>(
359 length_O, Y_predictions_mean));
364 (metric ==
metricML) ? 0 : std::numeric_limits<double>::max();
380 Eigen::VectorXd diff_means_i_j(length_O);
382 for (
size_t j = 0; j < nObservations; ++j)
384 if (!DAT_ASOC_USE_KDTREE)
387 for (
size_t i = 0; i < nPredictions; ++i)
390 const size_t pred_cov_idx =
392 Y_predictions_cov.extractMatrix(
393 pred_cov_idx, pred_cov_idx, length_O, length_O, pred_i_cov);
395 for (
size_t k = 0; k < length_O; k++)
396 diff_means_i_j[k] = Z_observations_mean.get_unsafe(j, k) -
397 Y_predictions_mean.get_unsafe(i, k);
403 diff_means_i_j, pred_i_cov, d2, ml);
411 const bool IC = (compatibilityTestMetric ==
metricML)
412 ? (ml > log_ML_compat_test_threshold)
421 for (
size_t k = 0; k < length_O; k++)
422 kd_queryPoint[k] = Z_observations_mean.get_unsafe(j, k);
425 &kd_queryPoint[0], N_KD_RESULTS, &kd_result_indices[0],
426 &kd_result_distances[0]);
429 for (
size_t w = 0;
w < N_KD_RESULTS;
w++)
431 const size_t i = kd_result_indices[
w];
436 const size_t pred_cov_idx =
438 Y_predictions_cov.extractMatrix(
439 pred_cov_idx, pred_cov_idx, length_O, length_O, pred_i_cov);
441 for (
size_t k = 0; k < length_O; k++)
442 diff_means_i_j[k] = Z_observations_mean.get_unsafe(j, k) -
443 Y_predictions_mean.get_unsafe(i, k);
449 diff_means_i_j, pred_i_cov, d2, ml);
451 if (d2 > 6 * chi2thres)
461 const bool IC = (compatibilityTestMetric ==
metricML)
462 ? (ml > log_ML_compat_test_threshold)
491 multimap<double, prediction_index_t>>>
493 TListAllICs lst_all_ICs;
497 multimap<double, prediction_index_t> ICs;
505 ICs.insert(make_pair(d2, i));
511 const double best_dist = ICs.begin()->first;
512 lst_all_ICs.insert(make_pair(best_dist, make_pair(j, ICs)));
520 std::set<prediction_index_t> lst_already_taken_preds;
523 it != lst_all_ICs.end(); ++it)
526 const multimap<double, prediction_index_t>& lstCompats =
531 itP != lstCompats.end(); ++itP)
533 if (lst_already_taken_preds.find(itP->second) ==
534 lst_already_taken_preds.end())
538 lst_already_taken_preds.insert(itP->second);
558 JCBB_recursive<CMatrixDouble::Scalar, metricMaha>(
559 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
562 JCBB_recursive<CMatrixDouble::Scalar, metricML>(
563 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
574 if (!predictions_IDs.empty())
576 ASSERT_(predictions_IDs.size() == nPredictions);
580 itAssoc->second = predictions_IDs[itAssoc->second];
596 const bool DAT_ASOC_USE_KDTREE,
597 const std::vector<prediction_index_t>& predictions_IDs,
599 const double log_ML_compat_test_threshold)
605 const size_t nPredictions =
size(Y_predictions_mean, 1);
606 const size_t nObservations =
size(Z_observations_mean, 1);
608 const size_t length_O =
size(Z_observations_mean, 2);
613 ASSERT_(length_O * nPredictions ==
size(Y_predictions_cov_stacked, 1))
615 ASSERT_(chi2quantile > 0 && chi2quantile < 1)
622 length_O * nPredictions, length_O * nPredictions);
624 for (
size_t i = 0; i < nPredictions; i++)
626 const size_t idx = i * length_O;
627 Y_predictions_cov_stacked.extractSubmatrix(
628 idx, idx + length_O - 1, 0, length_O - 1, COV_i);
629 Y_predictions_cov_full.insertMatrix(idx, idx, COV_i);
633 Z_observations_mean, Y_predictions_mean, Y_predictions_cov_full,
634 results, method, metric, chi2quantile, DAT_ASOC_USE_KDTREE,
635 predictions_IDs, compatibilityTestMetric, log_ML_compat_test_threshold);
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.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
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)
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)
const Scalar * const_iterator
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.
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.
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...
#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...
std::map< size_t, size_t > currentAssociation
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...
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...
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...
A matrix of dynamic size.
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
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
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 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)