struct mola::state_estimation_smoother::StateEstimationSmoother::GtsamImpl

Overview

struct GtsamImpl
{
    // typedefs

    typedef std::pair<frame_index_t, frame_index_t> frame_pair_t;

    // fields

    std::optional<gtsam::IncrementalFixedLagSmoother> smoother;
    gtsam::NonlinearFactorGraph newFactors;
    gtsam::Values newValues;
    gtsam::FixedLagSmoother::KeyTimestampMap newKeyStamps;
    std::map<frame_pair_t, gtsam::NonlinearFactorGraph> pendingKinematic;
    std::map<frame_pair_t, gtsam::FactorIndices> flushedKinematic;
    gtsam::FactorIndices factorsToRemove;

    // methods

    static frame_pair_t link_key(
        const frame_index_t a,
        const frame_index_t b
        );
};

Typedefs

typedef std::pair<frame_index_t, frame_index_t> frame_pair_t

Identifies the kinematic link between two keyframes. Links are undirected, so the pair is normalized and (a,b) and (b,a) name the same link.

Fields

std::map<frame_pair_t, gtsam::NonlinearFactorGraph> pendingKinematic

Kinematic links built but not yet handed to iSAM2, keyed by the keyframe pair they connect. Keeping them keyed, instead of appending straight into newFactors, is what allows a link to be withdrawn again before it ever reaches the solver, when a keyframe gets spliced between its endpoints.

std::map<frame_pair_t, gtsam::FactorIndices> flushedKinematic

iSAM2 factor indices of the kinematic links already inside the smoother, so a link can be removed on a later splice.

gtsam::FactorIndices factorsToRemove

Queued for removal at the next update().