struct mola::LidarOdometry::MethodState

Overview

All variables that hold the algorithm state

struct MethodState
{
    // structs

    struct GravityEstimator;
    struct OdometryAttitudeState;

    // fields

    bool initialized = false;
    bool fatal_error = false;
    bool active = true;
    int worker_tasks_lidar = 0;
    int worker_tasks_others = 0;
    std::array<bool, DROP_STATS_WINDOW_LENGTH> drop_frames_stats_good =       create_array<DROP_STATS_WINDOW_LENGTH>(true);
    std::array<bool, DROP_STATS_WINDOW_LENGTH> drop_frames_stats_dropped =       create_array<DROP_STATS_WINDOW_LENGTH>(false);
    std::size_t drop_frames_stats_next_index = 0;
    bool initial_localization_done = false;
    std::optional<mola::imu::ImuInitialCalibrator> imu_initializer;
    GravityEstimator gravity_estimator;
    std::optional<std::pair<double, double>> gravity_calib_pitch_roll;
    std::optional<double> gravity_calib_first_available_time;
    OdometryAttitudeState odom_attitude;
    bool gravity_calib_from_odometry = false;
    std::optional<double> odom_attitude_wait_since;
    std::optional<mrpt::poses::CPose3D> gravity_calib_pose;
    mrpt::poses::CPose3DPDFGaussian last_lidar_pose;
    std::map<std::string, mrpt::Clock::time_point> last_obs_tim_by_label;
    std::map<std::string, mrpt::poses::CPose3D> last_lidar_sensor_poses;
    bool last_icp_was_good = true;
    double last_icp_quality = .0;
    std::size_t last_icp_iterations = 0;
    std::optional<mrpt::Clock::time_point> first_ever_timestamp;
    std::optional<mrpt::Clock::time_point> last_obs_timestamp;
    std::optional<mrpt::Clock::time_point> last_icp_timestamp;
    std::optional<mrpt::Clock::time_point> last_obs_reception_time;
    double last_observed_scan_period_sec = 0.0;
    std::optional<mrpt::Clock::time_point> waiting_for_state_estimator_since;
    std::optional<mola::Georeferencing> external_georef;
    std::map<std::string, mrpt::obs::CObservation::ConstPtr> sync_obs;
    std::shared_ptr<mola::NavStateFilter> navstate_fuse;
    std::optional<NavState> last_motion_model_output;
    mp2p_icp::ParameterSource parameter_source;
    double adapt_thres_sigma = 0;
    int consecutive_bad_icps = 0;
    size_t registrations_attempted = 0;
    size_t registration_no_motion_model = 0;
    size_t registration_icp_rejected = 0;
    std::optional<double> estimated_observation_radius;
    std::optional<double> instantaneous_observation_radius;
    mp2p_icp_filters::GeneratorSet obs_generators;
    mp2p_icp_filters::FilterPipeline pc_filterAdjustTimes;
    mp2p_icp_filters::FilterPipeline pc_prefilter;
    mp2p_icp_filters::FilterPipeline pc_deskew;
    mp2p_icp_filters::FilterPipeline pc_filter1;
    mp2p_icp_filters::FilterPipeline pc_filter2;
    mp2p_icp_filters::FilterPipeline pc_filter3;
    mp2p_icp_filters::GeneratorSet local_map_generators;
    mp2p_icp::metric_map_t::Ptr local_map = mp2p_icp::metric_map_t::Create();
    mp2p_icp_filters::FilterPipeline obs2map_merge;
    bool map_has_been_loaded = false;
    mp2p_icp_filters::FilterPipeline obsDeskewForViz;
    std::optional<bool> isPipelinesUsingIMU;
    mrpt::poses::CPose3DInterpolator estimated_trajectory;
    mrpt::maps::CSimpleMap reconstructed_simplemap;
    std::optional<KeyframeDecider> kf_decider_local_map;
    std::optional<KeyframeDecider> kf_decider_simplemap;
    uint32_t localmap_check_removal_counter = 0;
    uint32_t localmap_advertise_updates_counter = std::numeric_limits<uint32_t>::max();
    bool local_map_needs_viz_update = true;
    bool local_map_needs_publish = true;
    bool local_map_georef_needs_publish = true;
    std::optional<double> last_yaw_for_viz_camera;
    mrpt::maps::CPointsMap::ConstPtr last_deskewed_scan_for_publishing;
    uint32_t step_counter_post_relocalization = 0;
    std::map<mrpt::Clock::time_point, std::shared_ptr<const mrpt::obs::CObservationGPS>> last_gnss_;
    std::vector<mrpt::opengl::CRenderizable::Ptr> glVehicleModels;
    bool glVehicleModelsLoaded = false;
    mrpt::opengl::CSetOfLines::Ptr glEstimatedPath;
    unsigned int mapUpdateCnt = std::numeric_limits<unsigned int>::max();
    std::map<mrpt::Clock::time_point, mrpt::obs::CSensoryFrame::Ptr> past_simplemaps_observations;
    std::map<std::string, mrpt::containers::circular_buffer<double>> recent_lidar_stamps;
    mrpt::containers::circular_buffer<double> recent_imu_stamps {1500};
    mrpt::containers::circular_buffer<double> recent_gnss_stamps {100};
    static constexpr std::size_t DROP_STATS_WINDOW_LENGTH = 128;

    // methods

    void mark_local_map_as_updated(bool force_republish = false);
    void mark_local_map_georef_as_updated();
    std::tuple<double, double, double> get_sensor_rates();

    void append_lidar_stamp(
        const std::string& sensorLabel,
        const mrpt::Clock::time_point& stamp,
        const mrpt::system::COutputLogger& logger
        );

    void append_imu_stamp(
        const mrpt::Clock::time_point& stamp,
        const mrpt::system::COutputLogger& logger
        );

    void append_gnss_stamp(
        const mrpt::Clock::time_point& stamp,
        const mrpt::system::COutputLogger& logger
        );
};

Fields

bool active = true

whether to process or ignore incoming sensors

std::optional<mola::imu::ImuInitialCalibrator> imu_initializer

Used for pitch & roll initialization. Protected by imu_state_mtx_.

GravityEstimator gravity_estimator

Protected by imu_state_mtx_.

std::optional<std::pair<double, double>> gravity_calib_pitch_roll

Gravity-derived (pitch, roll), in radians, captured at the time the first keyframe (map origin) was created. The IMU gravity estimator reports absolute tilt with respect to true vertical, while the map/global frame may itself not be exactly level (e.g. fixed_initial_pose has nonzero pitch/roll, or the vehicle was on a slope at start-up). This calibration offset is required to correctly re-express later absolute IMU tilt readings relative to the (possibly non-level) map frame.

std::optional<double> gravity_calib_first_available_time

Sensor timestamp of the first scan at which an accelerometer average was available for the map-origin verticality capture, so the dispersion gate’s timeout can be measured.

OdometryAttitudeState odom_attitude

Protected by imu_state_mtx_.

bool gravity_calib_from_odometry = false

True when the map-origin verticality reference was captured from the odometry attitude source. The per-scan reading is then taken from that same source and from nowhere else: a reading referenced against a vertical defined by a different sensor carries the constant offset between the two as a permanent map tilt, which is the error this reference exists to prevent. Protected by imu_state_mtx_.

std::optional<double> odom_attitude_wait_since

When the map-origin capture first had to wait for the odometry attitude source, on the sensor-time scale. Bounds that wait. Protected by imu_state_mtx_.

std::optional<mrpt::poses::CPose3D> gravity_calib_pose

Vehicle pose at the instant gravity_calib_pitch_roll was captured. The capture is attempted at the first keyframe, but the accelerometer average is often not available yet there: at the very first scan the IMU buffer holds only the few milliseconds that arrived before it, and the quasi-static gate rejects most of those on a moving platform. It is therefore retried on later scans, and the reading has to be transported through the pose it was taken at to still refer to the map frame. Equals fixed_initial_pose when the first attempt succeeds, which is what makes the retry a strict extension of the original behavior.

mrpt::poses::CPose3DPDFGaussian last_lidar_pose

in local map

std::map<std::string, mrpt::poses::CPose3D> last_lidar_sensor_poses

sensor pose per label

std::optional<mrpt::Clock::time_point> last_obs_reception_time

Wall-clock time when the last observation was received (used for staleness checks, avoids relying on sensor hardware clocks).

double last_observed_scan_period_sec = 0.0

seconds between last two processed scans

std::optional<mrpt::Clock::time_point> waiting_for_state_estimator_since

Timestamp when we started waiting for state estimator convergence.

std::optional<mola::Georeferencing> external_georef

Received georeferencing from external state estimator.

std::map<std::string, mrpt::obs::CObservation::ConstPtr> sync_obs

Cache for multiple LIDAR synchronization:

mp2p_icp::ParameterSource parameter_source

The source of “dynamic variables” in ICP pipelines. Protected by imu_state_mtx_: besides the variable map and the realize() flags, it owns the localVelocityBuffer that IMU samples write and the LiDAR deskew stage reads.

size_t registrations_attempted = 0

Run totals, reported once at shutdown. registration_no_motion_model is the one that had no aggregate before: the front end already logs a throttled warning when the state estimator returns nothing (or returns a prediction too uncertain to use) and falls back to a zero-motion initial guess, but a throttled line cannot be counted, so an estimator that was silently failing on 1 scan in 20 looked identical to one that never failed. Both counters exclude the very first scan, which has no motion model by definition.

mp2p_icp_filters::GeneratorSet obs_generators

Invocations are protected by imu_state_mtx_: apply_generators() on an IMU observation appends to parameter_source.localVelocityBuffer.

bool map_has_been_loaded = false

Set to true whenever a preexisting map is loaded into local_map / reconstructed_simplemap, either at start-up (doPreloadLocalMap()) or via a runtime map_load() service call. Used to avoid discarding an inherited map (multisession/multi-robot mapping) on later events that would otherwise assume a brand new, empty map (e.g. IMU-based initial re-localization).

std::optional<bool> isPipelinesUsingIMU

See isPipelineUsingIMU()

uint32_t localmap_check_removal_counter = 0

See check_for_removal_every_n.

bool local_map_needs_viz_update = true

To update the map in the viz only if really needed.

mrpt::maps::CPointsMap::ConstPtr last_deskewed_scan_for_publishing

For visualization on ROS: a decaying “cloud” of the last deskewed scans.

uint32_t step_counter_post_relocalization = 0

To handle post-re-localization. >0 means we are “recovering” from a request to re-localize:

unsigned int mapUpdateCnt = std::numeric_limits<unsigned int>::max()

Decimation counter for the local map visualization. Saturating (never wraps around), so its maximum value means “refresh at the next chance”.

std::map<std::string, mrpt::containers::circular_buffer<double>> recent_lidar_stamps

Used to estimate sensor rate, mapped by sensorLabel.

mrpt::containers::circular_buffer<double> recent_imu_stamps {1500}

Used to estimate sensor rate Protected by imu_state_mtx_.

mrpt::containers::circular_buffer<double> recent_gnss_stamps {100}

Used to estimate GNSS sensor rate.

Methods

std::tuple<double, double, double> get_sensor_rates()

Returns the rates (Hz) of incoming LiDAR, IMU, and GNSS sensors for the past few seconds. A rate of 0.0 means no data or stale data.