struct mola::LidarOdometry::MethodState

Overview

All variables that hold the algorithm state

struct MethodState
{
    // structs

    struct GravityEstimator;

    // 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;
    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;
    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<SearchablePoseList> distance_checker_local_map;
    std::optional<SearchablePoseList> distance_checker_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;
    int mapUpdateCnt = std::numeric_limits<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.

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.

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:

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:

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.

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.