struct mola::LidarOdometry::MethodState

Overview

All variables that hold the algorithm state

struct MethodState
{
    // 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;
    mrpt::poses::CPose3DPDFGaussian last_lidar_pose;
    std::map<std::string, mrpt::Clock::time_point> last_obs_tim_by_label;
    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::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;
    std::optional<double> estimated_sensor_max_range;
    std::optional<double> instantaneous_sensor_max_range;
    mp2p_icp_filters::GeneratorSet obs_generators;
    mp2p_icp_filters::FilterPipeline pc_filterAdjustTimes;
    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;
    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;
    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_;
    mrpt::opengl::CSetOfObjects::Ptr glVehicleFrame;
    mrpt::opengl::CSetOfObjects::Ptr glPathGrp;
    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};
    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> get_lidar_imu_sensor_rates();

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

    void append_imu_stamp(const mrpt::Clock::time_point& stamp);
};

Fields

bool active = true

whether to process or ignore incoming sensors

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

Used for pitch & roll initialization.

mrpt::poses::CPose3DPDFGaussian last_lidar_pose

in local map

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:

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.

Methods

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

Returns the rates (Hz) of incoming LiDAR and IMU sensors for the past few seconds.