class mp2p_icp::LearnedCovarianceEstimator

Overview

Learned covariance (LearnedCovariance + CovFeatureEvaluator) of every registration of a scan-to-map odometry, computed by a worker thread so the caller only pays for queueing a request.

The evaluator’s reference is a sliding window with the last window_scans accepted scans, each at its estimated pose. Requests and window updates are served strictly in call order, so each registration is evaluated against exactly the window that preceded it: results do not depend on when, or from which thread, they are collected.

Typical use, per registration of scan k:

auto cov = est.submit({scan_k, solution_k, ctx_k}); // cheap
// ... hand `cov` to its consumer (e.g. a state estimator) ...
est.pushToWindow(scan_k, pose_k); // cheap, once scan k is accepted

If the worker falls behind, the oldest evaluations still waiting are answered without a learned covariance (Result::ok false), and queued scans that would leave the window before any evaluation sees them are dropped, which bounds the latency and the queue. If a window update fails, the window restarts empty.

#include <LearnedCovarianceEstimator.h>

class LearnedCovarianceEstimator
{
public:
    // structs

    struct Parameters;
    struct Request;
    struct Result;
    struct Task;

    // construction

    LearnedCovarianceEstimator(const Parameters& p);
    LearnedCovarianceEstimator(const LearnedCovarianceEstimator&);

    // methods

    LearnedCovarianceEstimator& operator = (const LearnedCovarianceEstimator&);
    std::shared_future<Result> submit(Request r);
    void pushToWindow(mrpt::maps::CPointsMap::Ptr scan, const mrpt::poses::CPose3D& pose);
    void clearWindow();
    const Parameters& parameters() const;
    const LearnedCovariance& model() const;
};

Construction

LearnedCovarianceEstimator(const Parameters& p)

Loads the model and starts the worker. Throws on a model error.

Methods

std::shared_future<Result> submit(Request r)

Queues the evaluation of one registration against the current window.

void pushToWindow(
    mrpt::maps::CPointsMap::Ptr scan,
    const mrpt::poses::CPose3D& pose
    )

Appends an accepted scan (vehicle frame) at its estimated pose. The scan is shared, not copied, until it leaves the window: do not modify it after this call.

void clearWindow()

Empties the window, e.g. after the odometry restarts.