class mp2p_icp::Matcher_NDT_Blend

Overview

Pointcloud matcher: point to a likelihood-weighted blend of nearby planes.

Like Matcher_Point2Plane, this matcher pairs each local point against a plane taken from a global layer implementing mp2p_icp::NearestPlaneCapable, and emits one point-to-plane pairing per local point, so solvers consume it unchanged.

It differs in how the target plane is chosen. Matcher_Point2Plane keeps the single closest candidate, which makes the residual a discontinuous function of the sensor pose: an arbitrarily small pose change can swap the winning candidate and move the residual by the full disagreement between the two. This matcher instead combines all the candidates in the window with weights

w_i = taper(|q - c_i|) * exp(-0.5 * d_i^2 / temperature^2)

where d_i is the point-to-plane distance to candidate i, c_i its centroid and q the query point. The resulting plane is the weighted mean of the candidate centroids together with the dominant direction of the weighted normal structure tensor.

Two properties are deliberate:

• temperature = 0 reproduces Matcher_Point2Plane exactly, by taking the very same code path, so it is an exact control rather than an approximate one.

• taper() reaches zero with zero derivative at searchRadius, so a candidate entering or leaving the window does so with vanishing weight. A hard cutoff would reintroduce precisely the discontinuity the blend exists to remove.

Normals are accumulated as outer products rather than as vectors. Plane normals recovered by PCA carry an arbitrary sign, and deciding that sign per candidate against the query would itself be a discrete flip, triggered exactly where the candidate matters most. The outer product is invariant to it.

#include <Matcher_NDT_Blend.h>

class Matcher_NDT_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 :

• distanceThreshold : Max. inliers pt-plane distance [meters][mandatory]

• temperature : Blending scale for the point-plane distance [meters]. 0 (the default) means “keep the closest candidate”, i.e. exactly Matcher_Point2Plane.

• searchRadius : Radius [meters] around the query point within which candidates are blended, measured to the candidate centroid. Defaults to distanceThreshold, which is only sensible for maps whose plane models sit at the query’s own scale; for cell-based maps set it to the cell size or above, or every candidate falls outside the taper.

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

Plus: the parameters of Matcher_Points_Base::initialize()