struct mp2p_icp::MatchingDistanceProfile

Overview

The correspondence-acceptance criteria used by NearestPointWithCovCapable::nn_search_cov2cov().

Implicitly constructible from a plain float, so every existing caller passing a flat threshold keeps compiling and behaving exactly as before.

Opt-in refinement over a single flat distance: when far != near, the acceptance distance is a logistic function of the query point’s range from the sensor, transitioning from near to far around kneeRange, over width meters. Motivation: map point density falls off with range, so a single flat threshold is loose near the sensor (dense map, many close alternatives) and tight far away (sparse map).

#include <MatchingDistanceProfile.h>

struct MatchingDistanceProfile
{
    // fields

    float near = 0.40f;
    float far = 0.40f;
    float kneeRange = 15.0f;
    float width = 5.0f;

    // construction

    MatchingDistanceProfile();
    MatchingDistanceProfile(float flatThreshold);

    MatchingDistanceProfile(
        float nearDist,
        float farDist,
        float kneeRangeM,
        float widthM
        );

    // methods

    bool isFlat() const;
    bool needsRange() const;
    float operator () (float range) const;
};

Fields

float near = 0.40f

Distance at range=0. Also the flat value when far==near.

float far = 0.40f

Distance as range -> infinity. Equal to near means “flat” (the common case, and the fast path below).

float kneeRange = 15.0f

Range [m] at which the logistic transition is centered.

float width = 5.0f

Logistic transition width [m]. Smaller = closer to a hard knee.

Construction

MatchingDistanceProfile(float flatThreshold)

Implicit on purpose: nn_search_cov2cov(map, pose, 0.4f, out) must keep compiling and behave as a flat threshold.

Methods

bool needsRange() const

Whether a query point’s range must be computed at all: only if the acceptance distance depends on it.

float operator () (float range) const

Matching distance at the given range [m] from the sensor.