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 ScanPeriodEstimate;
    struct StateUI;
    struct StatusSnapshot;

    // classes

    class InvalidInputError;

    // 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();
    void flushPendingLidarScans();
    bool isBusy() const;
    std::size_t invalidInputCount() 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);
    void enqueue_locked_request(const std::function<void()>& userRequest);
    void setDeskewedColoring(mrpt::img::TColormap colormap, const std::string& colorByField);
    void setLocalMapColoring(mrpt::img::TColormap colormap, const std::string& colorByField);
    void setTrajectoryVisualization(bool show, const std::vector<float>& rgba);
    void setCurrentPoseCornerVisualization(bool show);
    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 flushPendingLidarScans()

Processes the LiDAR scans still held back waiting for IMU data, using whatever IMU samples did arrive, and blocks until the worker is idle.

Call it once the input is known to be over (end of a dataset, end of a rosbag replay): those scans wait for IMU that will never arrive, and without this they are silently discarded, losing the tail of the estimated trajectory. shutdownCleanup() calls it as well, so pipelines going through onQuit() need no explicit call. As during normal operation, only the freshest pending scan is actually processed; the older ones are accounted as dropped.

std::size_t invalidInputCount() const

Number of observations dropped as invalid input since construction. Multi-thread safe to call.

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. It does not wait for a scan being processed: it returns the state as of the last processed one. 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 by the next spinOnce() or by the LiDAR worker before its next scan, whichever comes first.

This method is safe to be called from any other thread. The request itself runs without any lock held, possibly while the other of those two threads is processing a scan.

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

Like enqueue_request(), but the request runs with the internal state mutex held. Any request that writes params_ must use this one. The request must not call reset(), relocalize_*() or any other method that takes the state mutex itself.

void setDeskewedColoring(
    mrpt::img::TColormap colormap,
    const std::string& colorByField
    )

Sets the colormap and color-by-field used to render the live deskewed scan streams (the incoming current observation and the decaying “dense local map” of past observations). Pass cmNONE as colormap to keep the point cloud’s own RGB colors (no recoloring).

void setLocalMapColoring(
    mrpt::img::TColormap colormap,
    const std::string& colorByField
    )

Sets the colormap and color-by-field used to render the local map cloud (e.g. cmGRAYSCALE for a grayscale global map).

void setTrajectoryVisualization(bool show, const std::vector<float>& rgba)

Sets estimated-trajectory line visibility and, if rgba has size 4, its RGBA color (each component in [0,1]). Lets a host own the trajectory appearance instead of toggling the scene object externally.

void setCurrentPoseCornerVisualization(bool show)

Shows or hides the current-pose XYZ corner opengl object. Useful for a host using a first-person camera placed at the vehicle pose, where the corner would otherwise fill the whole view.

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