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)
{
MRPT_UNUSED_PARAM(aux_data);
// Make a list of the indices of the predictions that appear in "currentAssociation":
const size_t N = info.currentAssociation.size();
ASSERT_(N>0)
vector_size_t indices_pred(N); // Appearance order indices in the std::maps
vector_size_t indices_obs(N);
{
size_t i=0;
for (map<size_t,size_t>::const_iterator it=info.currentAssociation.begin();it!=info.currentAssociation.end();++it)
{
indices_obs[i] = it->first;
indices_pred[i] = it->second;
i++;
}
}
// ----------------------------------------------------------------------
// Extract submatrix of the covariances involved here:
// COV = PREDICTIONS_COV(INDX,INDX) + OBSERVATIONS_COV(INDX2,INDX2)
// ----------------------------------------------------------------------
Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> COV;
Y_predictions_cov.extractSubmatrixSymmetricalBlocks(
info.length_O, // dims of cov. submatrices
indices_pred,
COV );
// ----------------------------------------------------------------------
// Mean:
// The same for the vector of "errors" or "innovation" between predictions and observations:
// ----------------------------------------------------------------------
Eigen::Matrix<T,Eigen::Dynamic,1> innovations(N * info.length_O);
T *dst_ptr= &innovations[0];
for (map<size_t,size_t>::const_iterator it=info.currentAssociation.begin();it!=info.currentAssociation.end();++it)
{
const T *pred_i_mean = Y_predictions_mean.get_unsafe_row( it->second );
const T *obs_i_mean = Z_observations_mean.get_unsafe_row( it->first );
for (unsigned int k=0;k<info.length_O;k++)
*dst_ptr++ = pred_i_mean[k]-obs_i_mean[k];
}
// Compute mahalanobis distance squared:
CMatrixTemplateNumeric<T> COV_inv;
COV.inv_fast(COV_inv);
const double d2 = mrpt::math::multiply_HCHt_scalar(innovations, COV_inv);
if (METRIC==metricMaha)
return d2;
ASSERT_(METRIC==metricML);
// Matching likelihood: The evaluation at 0 of the PDF of the difference between the two Gaussians:
const T cov_det = COV.det();
const double ml = exp(-0.5*d2) / ( std::pow(M_2PI, info.length_O * 0.5) * std::sqrt(cov_det) );
return ml;
}
