19 #include <Eigen/Dense>
63 template <
typename T, TDataAssociationMetric METRIC>
75 std::vector<size_t> indices_pred(
77 std::vector<size_t> indices_obs(N);
83 indices_obs[i] = it.first;
84 indices_pred[i] = it.second;
105 T* dst_ptr = &innovations[0];
109 const T* pred_i_mean = &Y_predictions_mean(it->second, 0);
110 const T* obs_i_mean = &Z_observations_mean(it->first, 0);
112 for (
unsigned int k = 0; k < info.
length_O; k++)
113 *dst_ptr++ = pred_i_mean[k] - obs_i_mean[k];
127 const T cov_det = COV.
det();
128 const double ml = exp(-0.5 * d2) / (std::pow(
M_2PI, info.
length_O * 0.5) *
133 template <TDataAssociationMetric METRIC>
134 bool isCloser(
const double v1,
const double v2);
155 template <
typename T, TDataAssociationMetric METRIC>
170 results.distance = joint_pdf_metric<T, METRIC>(
171 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
180 const double d2 = joint_pdf_metric<T, METRIC>(
181 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
184 if (isCloser<METRIC>(d2,
results.distance))
196 const size_t nPreds =
results.indiv_compatibility.rows();
203 const size_t potentials = std::accumulate(
204 results.indiv_compatibility_counts.begin() + (obsIdx + 1),
205 results.indiv_compatibility_counts.end(), 0);
212 if (
results.indiv_compatibility(predIdx, obsIdx))
215 bool already_asigned =
false;
218 if (itS.second == predIdx)
220 already_asigned =
true;
225 if (!already_asigned)
231 results.nNodesExploredInJCBB++;
233 JCBB_recursive<T, METRIC>(
234 Z_observations_mean, Y_predictions_mean,
235 Y_predictions_cov,
results, new_info,
249 results.nNodesExploredInJCBB++;
250 JCBB_recursive<T, METRIC>(
251 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
303 const bool DAT_ASOC_USE_KDTREE,
304 const std::vector<prediction_index_t>& predictions_IDs,
306 const double log_ML_compat_test_threshold)
317 const size_t nPredictions = Y_predictions_mean.
rows();
318 const size_t nObservations = Z_observations_mean.
rows();
320 const size_t length_O = Z_observations_mean.
cols();
324 ASSERT_(length_O == (
size_t)Y_predictions_mean.
cols());
325 ASSERT_(length_O * nPredictions == (
size_t)Y_predictions_cov.
rows());
327 ASSERT_(chi2quantile > 0 && chi2quantile < 1);
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);
344 if (DAT_ASOC_USE_KDTREE)
347 kd_tree = std::make_unique<KDTreeEigenMatrixAdaptor<CMatrixDouble>>(
348 length_O, Y_predictions_mean);
353 (metric ==
metricML) ? 0 : std::numeric_limits<double>::max();
358 results.indiv_distances.resize(nPredictions, nObservations);
359 results.indiv_compatibility.setSize(nPredictions, nObservations);
360 results.indiv_compatibility_counts.assign(nObservations, 0);
365 results.indiv_compatibility.fill(
false);
369 Eigen::VectorXd diff_means_i_j(length_O);
371 for (
size_t j = 0; j < nObservations; ++j)
373 if (!DAT_ASOC_USE_KDTREE)
376 for (
size_t i = 0; i < nPredictions; ++i)
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);
384 for (
size_t k = 0; k < length_O; k++)
386 Z_observations_mean(j, k) - Y_predictions_mean(i, k);
392 diff_means_i_j, pred_i_cov, d2, ml);
400 const bool IC = (compatibilityTestMetric ==
metricML)
401 ? (ml > log_ML_compat_test_threshold)
403 results.indiv_compatibility(i, j) = IC;
404 if (IC)
results.indiv_compatibility_counts[j]++;
410 for (
size_t k = 0; k < length_O; k++)
411 kd_queryPoint[k] = Z_observations_mean(j, k);
414 &kd_queryPoint[0], N_KD_RESULTS, &kd_result_indices[0],
415 &kd_result_distances[0]);
418 for (
size_t w = 0; w < N_KD_RESULTS; w++)
420 const size_t i = kd_result_indices[w];
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);
430 for (
size_t k = 0; k < length_O; k++)
432 Z_observations_mean(j, k) - Y_predictions_mean(i, k);
438 diff_means_i_j, pred_i_cov, d2, ml);
440 if (d2 > 6 * chi2thres)
450 const bool IC = (compatibilityTestMetric ==
metricML)
451 ? (ml > log_ML_compat_test_threshold)
453 results.indiv_compatibility(i, j) = IC;
454 if (IC)
results.indiv_compatibility_counts[j]++;
460 cout <<
"Distances: " << endl <<
results.indiv_distances << endl;
479 using TListAllICs = multimap<
483 TListAllICs lst_all_ICs;
487 multimap<double, prediction_index_t> ICs;
491 if (
results.indiv_compatibility(i, j))
493 double d2 =
results.indiv_distances(i, j);
495 ICs.insert(make_pair(d2, i));
501 const double best_dist = ICs.begin()->first;
502 lst_all_ICs.insert(make_pair(best_dist, make_pair(j, ICs)));
510 std::set<prediction_index_t> lst_already_taken_preds;
512 for (
auto it = lst_all_ICs.begin(); it != lst_all_ICs.end(); ++it)
515 const multimap<double, prediction_index_t>& lstCompats =
518 for (
auto lstCompat : lstCompats)
520 if (lst_already_taken_preds.find(lstCompat.second) ==
521 lst_already_taken_preds.end())
524 results.associations[obs_id] = lstCompat.second;
525 lst_already_taken_preds.insert(lstCompat.second);
545 JCBB_recursive<CMatrixDouble::Scalar, metricMaha>(
546 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
549 JCBB_recursive<CMatrixDouble::Scalar, metricML>(
550 Z_observations_mean, Y_predictions_mean, Y_predictions_cov,
561 if (!predictions_IDs.empty())
563 ASSERT_(predictions_IDs.size() == nPredictions);
564 for (
auto& association :
results.associations)
565 association.second = predictions_IDs[association.second];
581 const bool DAT_ASOC_USE_KDTREE,
582 const std::vector<prediction_index_t>& predictions_IDs,
584 const double log_ML_compat_test_threshold)
590 const size_t nPredictions = Y_predictions_mean.
rows();
591 const size_t nObservations = Z_observations_mean.
rows();
593 const size_t length_O = Z_observations_mean.
cols();
597 ASSERT_(length_O == (
size_t)Y_predictions_mean.
cols());
599 length_O * nPredictions == (
size_t)Y_predictions_cov_stacked.
rows());
600 ASSERT_(chi2quantile > 0 && chi2quantile < 1);
606 length_O * nPredictions, length_O * nPredictions);
608 for (
size_t i = 0; i < nPredictions; i++)
610 const size_t idx = i * length_O;
612 Y_predictions_cov_stacked.
extractMatrix(length_O, length_O, idx, 0);
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);