class mp2p_icp::Matcher_Cov2Cov

Overview

Point-to-point with associated local covariance matcher.

Both maps (local and global layers) must implement the mp2p_icp::NearestPointWithCovCapable interface.

#include <Matcher_Cov2Cov.h>

class Matcher_Cov2Cov: public mp2p_icp::Matcher
{
public:
    // fields

    std::vector<std::pair<std::string, std::string>> layer_matches;
    float bounding_box_intersection_check_epsilon = 0.20f;
    float threshold = 0.40f;
    float thresholdFar = 0.0f;
    float thresholdKneeRange = 15.0f;
    float thresholdTransitionWidth = 5.0f;
    float pointWeightAlpha = 0.0f;
    float pointWeightRefRange = 20.0f;
    float pointWeightMin = 0.01f;
    float pointWeightMax = 1.0f;
    float incidenceWeightAlpha = 0.0f;
    float incidenceWeightRefCos = 0.5f;
    float incidenceWeightMin = 0.05f;
    float incidenceWeightMax = 1.0f;

    // methods

    virtual void initialize(const mrpt::containers::yaml& params);
    MatchingDistanceProfile matchingDistanceProfile() const;
    PointWeightByRange pointWeightByRange() const;
    PointWeightByIncidence pointWeightByIncidence() const;
};

Inherited Members

public:
    // fields

    uint32_t runUpToIteration = 0;

    // methods

    Parameterizable& operator = (const Parameterizable&);
    Parameterizable& operator = (Parameterizable&&);
    virtual void initialize(const mrpt::containers::yaml& params);

    virtual bool match(
        const metric_map_t& pcGlobal,
        const metric_map_t& pcLocal,
        const mrpt::poses::CPose3D& localPose,
        const MatchContext& mc,
        MatchState& ms,
        Pairings& out
        ) const;

Fields

std::vector<std::pair<std::string, std::string>> layer_matches

Pairs of Local->Global layers to match. Note: this field can be loaded from a configuration file via initialize().

Map is: { {“globalLayer”, “localLayer”} [,…] }

float bounding_box_intersection_check_epsilon = 0.20f

The additional “margin” in all axes (x,y,z) that bounding box is enlarged for checking the feasibility of pairings to exist.

float threshold = 0.40f

Inliers distance threshold [meters]. Also the near-range value of the matching distance when thresholdFar is set (see below).

float thresholdFar = 0.0f

Optional: matching distance at long range [meters]. Leave at 0 (the default) for a flat threshold, i.e. today’s behavior. When positive and different from threshold, the matching distance becomes a logistic function of the query point’s range, transitioning from threshold to thresholdFar around thresholdKneeRange. Map point density falls off with range, so a flat threshold is loose near the sensor and tight far away; see ~/plans/icp-bench-range-adaptive-matching.md for the measurement behind this option.

float thresholdKneeRange = 15.0f

Range [meters] at which the near/far transition is centered. Only used when thresholdFar is set.

float thresholdTransitionWidth = 5.0f

Width [meters] of the near/far logistic transition. Only used when thresholdFar is set.

float pointWeightAlpha = 0.0f

Optional: how much a correspondence counts as a function of the range at which its point was measured. Disabled by default (pointWeightAlpha = 0), which weighs every point the same.

This is distinct from thresholdFar above: that one decides which correspondences are ACCEPTED, this one decides what they WEIGH once accepted. See mp2p_icp::PointWeightByRange for the curve and for the reading of the two physically meaningful exponents.

Motivation: a fixed-size decimation voxel cannot thin points past the range at which the beam spacing exceeds the voxel, so the far field is over-represented in the correspondence set exactly where each point is least certain and its lever arm on attitude is longest.

float pointWeightRefRange = 20.0f

Range [meters] below which the point weight saturates. Only used when pointWeightAlpha is nonzero.

float pointWeightMin = 0.01f

Floor of the point weight, so a far point is never dropped outright.

float pointWeightMax = 1.0f

Ceiling of the point weight. Keep at 1 for a plain knee.

float incidenceWeightAlpha = 0.0f

Optional: how much a correspondence counts as a function of how obliquely its beam struck the surface. Disabled by default (incidenceWeightAlpha = 0), which weighs every point the same.

Independent of the range weighting above, and composable with it: that one asks how far the point is, this one how squarely it was seen. See mp2p_icp::PointWeightByIncidence.

The surface normal is taken as the dominant eigenvector of the pairing’s own information matrix, which for a locally planar neighborhood points along the normal, so nothing extra has to be stored or recomputed.

float incidenceWeightRefCos = 0.5f

|cos| of the incidence angle at which the weight starts to fall. Only used when incidenceWeightAlpha is nonzero.

float incidenceWeightMin = 0.05f

Floor, so a grazing point is never dropped outright.

float incidenceWeightMax = 1.0f

Ceiling. Keep at 1 for a plain knee.

Methods

virtual void initialize(const mrpt::containers::yaml& params)

Common parameters to all derived classes:

  • threshold : Inliers distance threshold [meters][mandatory]

  • thresholdFar, thresholdKneeRange, thresholdTransitionWidth : Optional range-adaptive matching distance, see the field docs above. Like threshold, they accept dynamic formulas, e.g. “3.0*ADAPTIVE_THRESHOLD_SIGMA”.

  • layerMatches : Optional map of layer names to match. Refer to example YAML files.

  • pointWeightAlpha, pointWeightRefRange, pointWeightMin, pointWeightMax : Optional per-point weighting by range, see the field docs above. Also accept dynamic formulas.

  • incidenceWeightAlpha, incidenceWeightRefCos, incidenceWeightMin, incidenceWeightMax : Optional per-point weighting by incidence angle, see the field docs above. Also accept dynamic formulas.

  • bounding_box_intersection_check_epsilon : Optional (Default=0.20). The additional “margin” in all axes (x,y,z) that bounding box is enlarged for checking the feasibility of pairings to exist.

MatchingDistanceProfile matchingDistanceProfile() const

The effective matching-distance profile built from threshold and, if set, thresholdFar / thresholdKneeRange / thresholdTransitionWidth.

PointWeightByRange pointWeightByRange() const

The effective per-point range weighting built from the pointWeight* fields.

PointWeightByIncidence pointWeightByIncidence() const

The effective per-point incidence weighting built from the incidenceWeight* fields.