class mp2p_icp::Matcher_Points_Blend

Overview

Pointcloud matcher: point to a distance-weighted blend of nearby map points.

This is the point-based counterpart of Matcher_NDT_Blend. Like Matcher_Points_DistanceThreshold it emits one point-to-point pairing per local point, so solvers consume it unchanged, but the target is not the single nearest map point: it is the weighted mean

m(q) = sum_i w_i p_i / sum_i w_i,
w_i  = taper(|q - p_i|) * exp(-0.5 * |q - p_i|^2 / temperature^2)

over the map points within searchRadius of the query q.

The motivation is that a nearest-neighbor target is a discontinuous function of the sensor pose: as the query crosses the perpendicular bisector between two map points the winner swaps and the residual moves by the full separation between them. The weighted mean crosses that bisector continuously.

Three properties are deliberate:

temperature = 0 runs the very same nn_single_search() query as Matcher_Points_DistanceThreshold with pairingsPerPoint: 1, so it reproduces it exactly and is an exact control rather than an approximate one.
The neighborhood is a radius query, never a fixed-k one. A map point entering or leaving a top-k list is itself a hard flip, and a harder one than the nearest-neighbor swap this matcher exists to remove, since k is a count rather than a geometric boundary.
taper() reaches zero with zero derivative at searchRadius, so a map point entering or leaving the neighborhood does so with vanishing weight.

A known cost, stated because it is intrinsic rather than incidental: a weighted mean of points sampled from a surface shrinks toward the interior of the neighborhood. At large temperature the target tends to the local centroid rather than to the surface, which biases the residual. The temperature should be chosen against the map’s point spacing.

#include <Matcher_Points_Blend.h>

class Matcher_Points_Blend: public mp2p_icp::Matcher_Points_Base
{
public:
    // methods

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

Inherited Members

public:
    // structs

    struct TransformedLocalPointCloud;

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

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

Methods

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

Parameters :

threshold : Max. distance between the local point and the blended target [meters][mandatory]
thresholdAngularDeg : Additional range-proportional term added to threshold, as in Matcher_Points_DistanceThreshold [degrees][mandatory]
temperature : Blending scale for the point-to-point distance [meters]. 0 (the default) means “keep the nearest map point”, i.e. exactly Matcher_Points_DistanceThreshold with pairingsPerPoint: 1.
searchRadius : Radius [meters] around the query point within which map points are blended. Defaults to threshold.
smoothCutoff : Optional (Default=true). Fade weights smoothly to zero at searchRadius instead of truncating there.
minWeightSum : Optional. Emit no pairing if the accumulated weight falls below this.
maxNeighbors : Optional (Default=0, meaning every point in the radius). Leave it at 0. A nonzero value makes the underlying nn_radius_search() rank its candidates and return only the best few, which is a top-k rule and reintroduces exactly the discreteness this matcher exists to remove. Some map classes cap that list at a handful of points regardless of the value asked for.

Plus: the parameters of Matcher_Points_Base::initialize()