Data association

// typedefs

typedef size_t mrpt::slam::observation_index_t;
typedef size_t mrpt::slam::prediction_index_t;

// enums

enum mrpt::slam::TDataAssociationMethod;
enum mrpt::slam::TDataAssociationMetric;

// structs

struct mrpt::slam::TDataAssociationResults;

// global functions

void mrpt::slam::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
    );

void mrpt::slam::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
    );

Typedefs

typedef size_t mrpt::slam::observation_index_t

Used in mrpt::slam::TDataAssociationResults.

typedef size_t mrpt::slam::prediction_index_t

Used in mrpt::slam::TDataAssociationResults.

Global Functions

void mrpt::slam::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, all of them with covariance matrices - Generic version with prediction full cross-covariances.

Implemented methods include (see TDataAssociation)

  • NN: Nearest-neighbor

  • JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]

With both a Mahalanobis-distance or Matching-likelihood metric. For a comparison of both methods, see paper:

  • J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, “An alternative to the Mahalanobis distance for determining optimal correspondences in data association”, IEEE Transactions on Robotics (T-RO), (2012) DOI: 10.1109/TRO.2012.2193706 Draft: http://ingmec.ual.es/~jlblanco/papers/blanco2012amd.pdf

Parameters:

Z_observations_mean

[IN] An MxO matrix with the M observations, each row containing the observation “mean”.

Y_predictions_mean

[IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.

Y_predictions_cov

[IN] An N*OxN*O matrix with the full covariance matrix of all the N predictions.

results

[OUT] The output data association hypothesis, and other useful information.

method

[IN, optional] The selected method to make the associations.

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

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).

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]”.

See also:

data_association_independent_predictions, data_association_independent_2d_points, data_association_independent_3d_points

void mrpt::slam::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, all of them with covariance matrices - Generic version with NO prediction cross-covariances.

Implemented methods include (see TDataAssociation)

  • NN: Nearest-neighbor

  • JCBB: Joint Compatibility Branch & Bound [Neira, Tardos 2001]

With both a Mahalanobis-distance or Matching-likelihood metric. For a comparison of both methods, see paper:

  • J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, “An alternative to the Mahalanobis distance for determining optimal correspondences in data association”, IEEE Transactions on Robotics (T-RO), (2012) DOI: 10.1109/TRO.2012.2193706 Draft: http://ingmec.ual.es/~jlblanco/papers/blanco2012amd.pdf

Parameters:

Z_observations_mean

[IN] An MxO matrix with the M observations, each row containing the observation “mean”.

Y_predictions_mean

[IN] An NxO matrix with the N predictions, each row containing the mean of one prediction.

Y_predictions_cov

[IN] An N*OxO matrix: A vertical stack of N covariance matrix, one for each of the N prediction.

results

[OUT] The output data association hypothesis, and other useful information.

method

[IN, optional] The selected method to make the associations.

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

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).

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]”.

See also:

data_association_full_covariance, data_association_independent_2d_points, data_association_independent_3d_points