namespace mrpt::slam

namespace slam {

// namespaces

namespace mrpt::slam::detail;

// typedefs

typedef size_t observation_index_t;
typedef size_t prediction_index_t;
typedef std::function<double(const map_keyframe_t&kf1, const map_keyframe_t&kf2, const mrpt::poses::CPose3D&relPose2wrt1)> similarity_func_t;

// enums

enum TDataAssociationMethod;
enum TDataAssociationMetric;
enum TICPAlgorithm;
enum TICPCovarianceMethod;
enum similarity_method_t;

// structs

struct TAuxDataRecursiveJCBB;
struct TDataAssociationResults;
struct TMetricMapAlignmentResult;
struct TMonteCarloLocalizationParams;
struct map_keyframe_t;

// classes

class CGridMapAligner;
class CICP;
class CIncrementalMapPartitioner;
class CMetricMapBuilder;
class CMetricMapBuilderICP;
class CMetricMapBuilderRBPF;
class CMetricMapsAlignmentAlgorithm;
class CMonteCarloLocalization2D;
class CMonteCarloLocalization3D;
class COccupancyGridMapFeatureExtractor;
class CRangeBearingKFSLAM;
class CRangeBearingKFSLAM2D;
class CRejectionSamplingRangeOnlyLocalization;

template <
    class PARTICLE_TYPE,
    class MYSELF,
    mrpt::bayes::particle_storage_mode STORAGE
    >
class PF_implementation;

class TKLDParams;

// global functions

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

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

double observationsOverlap(const mrpt::obs::CObservation* o1, const mrpt::obs::CObservation* o2, const mrpt::poses::CPose3D* pose_o2_wrt_o1 = nullptr);
double observationsOverlap(const mrpt::obs::CObservation::Ptr& o1, const mrpt::obs::CObservation::Ptr& o2, const mrpt::poses::CPose3D* pose_o2_wrt_o1 = nullptr);
double observationsOverlap(const mrpt::obs::CSensoryFrame& sf1, const mrpt::obs::CSensoryFrame& sf2, const mrpt::poses::CPose3D* pose_sf2_wrt_sf1 = nullptr);

template <class PARTICLETYPE, class BINTYPE>
void KLF_loadBinFromParticle(
    BINTYPE& outBin,
    const TKLDParams& opts,
    const PARTICLETYPE* currentParticleValue = nullptr,
    const mrpt::math::TPose3D* newPoseToBeInserted = nullptr
    );

void KLF_loadBinFromParticle(
    mrpt::slam::detail::TPoseBin2D& outBin,
    const TKLDParams& opts,
    const CMonteCarloLocalization2D::CParticleDataContent* currentParticleValue,
    const TPose3D* newPoseToBeInserted
    );

void KLF_loadBinFromParticle(
    mrpt::slam::detail::TPoseBin3D& outBin,
    const TKLDParams& opts,
    const CMonteCarloLocalization3D::CParticleDataContent* currentParticleValue,
    const TPose3D* newPoseToBeInserted
    );

void KLF_loadBinFromParticle(
    detail::TPoseBin2D& outBin,
    const TKLDParams& opts,
    const mrpt::maps::CRBPFParticleData* currentParticleValue,
    const TPose3D* newPoseToBeInserted
    );

void KLF_loadBinFromParticle(
    detail::TPathBin2D& outBin,
    const TKLDParams& opts,
    const mrpt::maps::CRBPFParticleData* currentParticleValue,
    const TPose3D* newPoseToBeInserted
    );

template <typename T, TDataAssociationMetric METRIC>
double joint_pdf_metric(
    const CMatrixDynamic<T>& Z_observations_mean,
    const CMatrixDynamic<T>& Y_predictions_mean,
    const CMatrixDynamic<T>& Y_predictions_cov,
    const TAuxDataRecursiveJCBB& info,
    ] const TDataAssociationResults& aux_data
    );

template <TDataAssociationMetric METRIC>
bool isCloser(
    const double v1,
    const double v2
    );

bool isCloser< metricMaha >(
    const double v1,
    const double v2
    );

bool isCloser< metricML >(
    const double v1,
    const double v2
    );

template <typename T, TDataAssociationMetric METRIC>
void JCBB_recursive(
    const mrpt::math::CMatrixDynamic<T>& Z_observations_mean,
    const mrpt::math::CMatrixDynamic<T>& Y_predictions_mean,
    const mrpt::math::CMatrixDynamic<T>& Y_predictions_cov,
    TDataAssociationResults& results,
    const TAuxDataRecursiveJCBB& info,
    const observation_index_t curObsIdx
    );

} // namespace slam

Global Functions

void KLF_loadBinFromParticle(
    mrpt::slam::detail::TPoseBin2D& outBin,
    const TKLDParams& opts,
    const CMonteCarloLocalization2D::CParticleDataContent* currentParticleValue,
    const TPose3D* newPoseToBeInserted
    )

Fills out a “TPoseBin2D” variable, given a path hypotesis and (if not set to nullptr) a new pose appended at the end, using the KLD params in “options”.

void KLF_loadBinFromParticle(
    mrpt::slam::detail::TPoseBin3D& outBin,
    const TKLDParams& opts,
    const CMonteCarloLocalization3D::CParticleDataContent* currentParticleValue,
    const TPose3D* newPoseToBeInserted
    )

Fills out a “TPoseBin3D” variable, given a path hypotesis and (if not set to nullptr) a new pose appended at the end, using the KLD params in “options”.

void KLF_loadBinFromParticle(
    detail::TPoseBin2D& outBin,
    const TKLDParams& opts,
    const mrpt::maps::CRBPFParticleData* currentParticleValue,
    const TPose3D* newPoseToBeInserted
    )

Fills out a “TPoseBin2D” variable, given a path hypotesis and (if not set to nullptr) a new pose appended at the end, using the KLD params in “options”.

void KLF_loadBinFromParticle(
    detail::TPathBin2D& outBin,
    const TKLDParams& opts,
    const mrpt::maps::CRBPFParticleData* currentParticleValue,
    const TPose3D* newPoseToBeInserted
    )

Fills out a “TPathBin2D” variable, given a path hypotesis and (if not set to nullptr) a new pose appended at the end, using the KLD params in “options”.

template <typename T, TDataAssociationMetric METRIC>
double joint_pdf_metric(
    const CMatrixDynamic<T>& Z_observations_mean,
    const CMatrixDynamic<T>& Y_predictions_mean,
    const CMatrixDynamic<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 associations.

On “currentAssociation”: maps “ID_obs” -> “ID_pred” For each landmark ID in the observations (ID_obs), its association in the predictions, that is: ID_pred = associations[ID_obs]