struct mola::ASLAM_gtsam::SLAM_state

Overview

struct SLAM_state
{
    // structs

    struct StereoSmartFactorState;

    // fields

    std::unique_ptr<gtsam::ISAM2> isam2;
    gtsam::NonlinearFactorGraph newfactors;
    gtsam::Values newvalues;
    gtsam::FastMap<gtsam::FactorIndex, gtsam::KeySet> changedSmartFactors;
    std::map<std::size_t, mola::fid_t> newFactor2molaid;
    std::set<mola::id_t> kf_has_value;
    gtsam::Values last_values;
    std::map<mrpt::Clock::time_point, AdvertiseUpdatedLocalization_Input> trajectory {};
    mrpt::graphs::CNetworkOfPoses3D vizmap;
    std::map<mola::id_t, mrpt::math::TTwist3D> vizmap_dyn;
    id_t root_kf_id {mola::INVALID_ID};
    id_t last_created_kf_id {mola::INVALID_ID};
    id_t former_last_created_kf_id {mola::INVALID_ID};
    mrpt::Clock::time_point last_created_kf_id_tim {INVALID_TIMESTAMP};
    std::map<mola::id_t, KF_gtsam_keys> mola2gtsam;
    std::array<std::map<gtsam::Key, mola::id_t>, KF_KEY_COUNT> gtsam2mola;
    StereoSmartFactorState stereo_factors;
    std::map<mrpt::Clock::time_point, mola::id_t> time2kf;
    std::vector<mola::SmartFactorIMU*> active_imu_factors;

    // methods

    template <class T>
    T at_new_or_last_values(const gtsam::Key& k) const;

    void updateLastCreatedKF(id_t id);
};

Detailed Documentation

Fields

std::unique_ptr<gtsam::ISAM2> isam2

Incremental estimator

gtsam::NonlinearFactorGraph newfactors

Pending new elements to add to the map

std::map<std::size_t, mola::fid_t> newFactor2molaid

Map: new factor index in newfactors ==> MOLA factor ID

std::map<mrpt::Clock::time_point, AdvertiseUpdatedLocalization_Input> trajectory {}

History of vehicle poses over time (stored if params_.save_trajectory_file_prefix!=”“). Note that this stores relative poses for all frames, keyframes and non-keyframes. We keep them relative so we can reconstruct the optimal poses at any moment, composing the poses of the base, optimized, KF of reference for each entry.

id_t root_kf_id {mola::INVALID_ID}

Absolute coordinates single reference frame (WorldModel index)

std::map<mola::id_t, KF_gtsam_keys> mola2gtsam

Map between mola WorldModel KF indices and the corresponding gtsam Key(s) value(s). When in SE2/SE3 mode, only the pose Key is used. When in SE2Vel/SE3Vel mode, the extra key for the velocity variable is stored a well

std::array<std::map<gtsam::Key, mola::id_t>, KF_KEY_COUNT> gtsam2mola

Inverse map for mola2gtsam (indexed by gtsam pose ID)