class mola::LidarOdometry

Overview

LIDAR-inertial odometry based on ICP against a local metric map model.

#include <LidarOdometry.h>

class LidarOdometry:
    public mola::FrontEndBase,
    public mola::LocalizationSourceBase,
    public mola::MapSourceBase,
    public mola::MapServer,
    public mola::Relocalization
{
public:
    // enums

    enum AlignKind;

    // structs

    struct ICP_Input;
    struct ICP_Output;
    struct MethodState;
    struct Parameters;
    struct StateUI;

    // fields

    Parameters params_;
    std::atomic_bool shutdown_cleanup_done_ {false};

    // construction

    LidarOdometry();
    LidarOdometry(const LidarOdometry&);
    LidarOdometry(LidarOdometry&&);

    // methods

    virtual void initialize_frontend(const Yaml& cfg);
    virtual void spinOnce();
    virtual void onNewObservation(const CObservation::ConstPtr& o);
    virtual void onQuit();
    void reset();
    bool isBusy() const;
    void shutdownCleanup();
    bool isActive() const;
    void setActive(bool active);
    mrpt::poses::CPose3DInterpolator estimatedTrajectory() const;
    std::optional<std::tuple<mrpt::poses::CPose3DPDFGaussian, mrpt::math::TTwist3D>> lastEstimatedState() const;
    double lastIcpQuality() const;
    mrpt::maps::CSimpleMap reconstructedMap() const;
    void saveEstimatedTrajectoryToFile() const;
    void saveReconstructedMapToFile() const;
    void saveLocalMapToFile() const;
    void enqueue_request(const std::function<void()>& userRequest);
    virtual void relocalize_near_pose_pdf(const mrpt::poses::CPose3DPDFGaussian& p);
    virtual void relocalize_from_gnss();
    MapServer::ReturnStatus map_load(const std::string& path);
    MapServer::ReturnStatus map_save(const std::string& path);
    virtual ReturnStatus map_load(] const std::string& path);
    virtual ReturnStatus map_save(] const std::string& path);
    LidarOdometry& operator = (const LidarOdometry&);
    LidarOdometry& operator = (LidarOdometry&&);
    void subscribeToMapUpdates(const map_updates_callback_t& callback);
};

Inherited Members

public:
    // structs

    struct DiagnosticsOutput;
    struct LocalizationUpdate;
    struct MapUpdate;
    struct ReturnStatus;

    // methods

    virtual void initialize(const Yaml& cfg) = 0;
    virtual void spinOnce() = 0;
    ExecutableBase& operator = (const ExecutableBase&);
    ExecutableBase& operator = (ExecutableBase&&);
    virtual void onNewObservation(const CObservation::ConstPtr& o) = 0;
    virtual void initialize(const Yaml& cfg);
    virtual void relocalize_near_pose_pdf(const mrpt::poses::CPose3DPDFGaussian& p) = 0;
    virtual void relocalize_from_gnss() = 0;

Fields

Parameters params_

Algorithm parameters

Methods

virtual void initialize_frontend(const Yaml& cfg)

Loads children specific parameters

virtual void spinOnce()

Runs any required action on a timely manner

virtual void onNewObservation(const CObservation::ConstPtr& o)

To be called whenever a new observation arrives. It should return as fast as possible, enqueuing the data for processing in another thread.

virtual void onQuit()

Called while destroying the SLAM system. A perfect placeholder for saving data to filesystem, clean up, etc. before any module destructor has been actually beeing invoked.

void reset()

Re-initializes the odometry system. It effectively calls initialize() once again with the same parameters that were used the first time.

void shutdownCleanup()

Drains the worker thread pools and saves the simplemap/trajectory/local map to disk, if so configured. Idempotent: safe to call from onQuit() and then again from the destructor.

Must run to completion (and thus stop calling back into other modules, e.g. via VizInterface or BridgeROS2 ‘s TF broadcaster) before any other module of the running MOLA system is destroyed. onQuit() is invoked by MolaLauncherApp for that exact purpose, before any module destructor runs.

mrpt::poses::CPose3DInterpolator estimatedTrajectory() const

Returns a copy of the estimated trajectory, with timestamps for each lidar observation. Multi-thread safe to call.

std::optional<std::tuple<mrpt::poses::CPose3DPDFGaussian, mrpt::math::TTwist3D>> lastEstimatedState() const

Returns the last estimated kinematic state: pose of the vehicle in the LiDAR odometry frame, and its estimated local (body-frame) twist vector. This method will block if LO is running in another thread, until it is safe to get the data. It will return std::nullopt if pose information is not available yet, e.g. still initializing.

double lastIcpQuality() const

Returns the ICP quality (range: [0,1]) of the last registered scan. Only valid if lastEstimatedState() returns non-empty.

mrpt::maps::CSimpleMap reconstructedMap() const

Returns a copy of the estimated simplemap. Multi-thread safe to call.

void enqueue_request(const std::function<void()>& userRequest)

Enqueue a custom user request to be executed on the main LidarOdometry thread on the next iteration.

So, this method is safe to be called from any other thread.

virtual void relocalize_near_pose_pdf(const mrpt::poses::CPose3DPDFGaussian& p)

Re-localize near this pose, including uncertainty. Thread-safe: may be called from any thread. The actual work is dispatched asynchronously to the internal lidar worker thread via enqueue_request(), so this call does not block on state_mtx_ and cannot be involved in any deadlock with the main processing loop.

Parameters:

pose	The pose, in the local map frame. There is no return value from this method.

virtual void relocalize_from_gnss()

Re-localize with the next incoming GNSS message. There is no return value from this method.

Thread-safe and non-blocking; see relocalize_near_pose_pdf() for details.

MapServer::ReturnStatus map_load(const std::string& path)

Loads a map from file(s) and sets it as active current map. Different implementations may use one or more files to store map as files.

Thread-safe: the load work is dispatched to the internal lidar worker thread via enqueue_request() and this call blocks on an std::future until the work completes. The caller holds no internal mutex while waiting, so it cannot contribute to deadlock with the processing loop.

Parameters:

path	File name(s) prefix for the map to load. Do not add file extension.

MapServer::ReturnStatus map_save(const std::string& path)

Saves a map from file(s) and sets it as active current map. Different implementations may use one or more files to store map as files.

Thread-safe; see map_load() for details on the async dispatch.

Parameters:

path	File name(s) prefix for the map to save. Do not add file extension.

virtual ReturnStatus map_load(] const std::string& path)

Loads a map from file(s) and sets it as active current map. Different implementations may use one or more files to store map as files.

Parameters:

path	File name(s) prefix for the map to load. Do not add file extension.

virtual ReturnStatus map_save(] const std::string& path)

Saves a map from file(s) and sets it as active current map. Different implementations may use one or more files to store map as files.

Parameters:

path	File name(s) prefix for the map to save. Do not add file extension.

void subscribeToMapUpdates(const map_updates_callback_t& callback)

Subscribe to map updates.

Behaves analogously to ROS’ transient_local durability: any prior advertiseUpdatedMap() call made with keep_last_one_only=true is cached (one entry per map_name) and replayed once into the new callback at subscription time. This avoids losing the initial state when the producer advertises before the consumer has had a chance to subscribe (e.g. on startup, when modules come up in arbitrary order). Subscribe to map updates.

Behaves analogously to ROS’ transient_local durability: any prior advertiseUpdatedMap() call made with keep_last_one_only=true is cached (one entry per map_name) and replayed once into the new callback at subscription time. This avoids losing the initial state when the producer advertises before the consumer has had a chance to subscribe (e.g. on startup, when modules come up in arbitrary order).