class mp2p_icp::CovFeatureEvaluator

Overview

Evaluates a registration result with a cost function of its own, so that results from different ICP implementations and configurations are described by the same numbers.

The evaluator owns its reference cloud (voxelized), its query scan (voxelized and subsampled) and a point-to-plane cost with a Cauchy kernel and lazily fitted map normals. At the given solution it computes the analytic covariance of that cost and a feature vector describing the problem’s structure, the residuals, the cost’s real shape at finite steps along each whitened axis (with correspondences re-searched), the scene geometry, and how the solution relates to its initial guess.

Typical use: setReference() once per local map, setScan() once per query scan, then evaluate() once per registration result.

#include <IcpCovFeatures.h>

class CovFeatureEvaluator
{
public:
    // structs

    struct Assoc;
    struct RefVoxel;

    // fields

    static constexpr int kVersion = 1;

    // construction

    CovFeatureEvaluator(const CovFeatureParameters& p = {});

    // methods

    void setReference(const std::vector<std::pair<const mrpt::maps::CPointsMap*, mrpt::poses::CPose3D>>& clouds);
    void addReferenceCloud(const mrpt::maps::CPointsMap& cloud, const mrpt::poses::CPose3D& pose);

    void removeReferenceCloud(
        const mrpt::maps::CPointsMap& cloud,
        const mrpt::poses::CPose3D& pose
        );

    void finalizeReference();
    void setScan(const mrpt::maps::CPointsMap& scan);
    CovFeatureResult evaluate(const mrpt::poses::CPose3D& solution, const CovFeatureContext& ctx);
    size_t referenceSize() const;
    size_t scanSize() const;
    const CovFeatureParameters& parameters() const;
    static const std::vector<std::string>& featureNames();
};

Fields

static constexpr int kVersion = 1

Increased whenever the meaning or order of the features changes.

Methods

void setReference(const std::vector<std::pair<const mrpt::maps::CPointsMap*, mrpt::poses::CPose3D>>& clouds)

Sets the reference cloud from point clouds and the poses that bring each of them into the map frame.

void addReferenceCloud(
    const mrpt::maps::CPointsMap& cloud,
    const mrpt::poses::CPose3D& pose
    )

Incremental alternative to setReference(), for a sliding window of clouds: adds one cloud’s points, or removes them (pass the same cloud and pose it was added with). Call finalizeReference() before the next evaluate(). Cheaper than setReference() when only the ends of the window change; the result is the same up to floating-point rounding and the order of the reference points.

void setScan(const mrpt::maps::CPointsMap& scan)

Sets the query scan, in the vehicle frame.

CovFeatureResult evaluate(const mrpt::poses::CPose3D& solution, const CovFeatureContext& ctx)

Evaluates the registration result solution (vehicle pose in the map frame). Requires setReference() and setScan().

static const std::vector<std::string>& featureNames()

Names of the entries of CovFeatureResult::features.