struct mola::state_estimation_smoother::StateEstimationSmoother::GtsamImpl

Overview

struct GtsamImpl
{
    // typedefs

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

    // structs

    struct GaugeAnchor;

    // 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;
    std::optional<GaugeAnchor> mapAnchor;
    odometry_frameid_t mapAnchorFrame = 0;
    bool mapObserved = false;
    bool mapObservedSolved = false;
    std::optional<GaugeAnchor> enuYawAnchor;
    std::optional<gtsam::Point2> firstGnssEnuXY;
    std::optional<GaugeAnchor> orientationAnchor;
    std::optional<gtsam::Key> orientationAnchorKey;
    bool rollPitchObserved = false;
    bool yawObserved = false;
    std::optional<gtsam::Point2> firstGnssXYForYaw;

    // methods

    bool rollPitchPinned() const;
    bool yawPinned() const;
    void update_orientation_anchor();

    template <class FACTOR, class... Args>
    void add_anchor(
        std::optional<GaugeAnchor>& anchor,
        Args&&... args
        );

    void withdraw_anchor(std::optional<GaugeAnchor>& anchor);
    void on_update_done(const gtsam::FactorIndices& newFactorsIndices);

    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().

std::optional<GaugeAnchor> mapAnchor

Defines {map} as odometry frame mapAnchorFrame while no source observes {map}.

bool mapObservedSolved = false

mapObserved, as of the last solve (the observation is in the estimate)

std::optional<GaugeAnchor> enuYawAnchor

Pins the azimuth of T_enu_to_map while no IMU attitude or GNSS observes it.

std::optional<GaugeAnchor> orientationAnchor

Once {map} is observed only in position (e.g. GNSS through a fixed geo-reference, with no attitude source), pins the orientation of the odometry frame orientationAnchorKey, which nothing observes yet. Roll/pitch and yaw are released independently, as sources observe them.

Methods

bool rollPitchPinned() const

Whether the reported orientation is pinned by a gauge anchor, not observed.

void update_orientation_anchor()

Re-creates the orientation anchor for what is still unobserved.

template <class FACTOR, class... Args>
void add_anchor(
    std::optional<GaugeAnchor>& anchor,
    Args&&... args
    )

Queues a new anchor factor, built in place from args

void withdraw_anchor(std::optional<GaugeAnchor>& anchor)

Drops the anchor, whether still pending or already inside the smoother.

void on_update_done(const gtsam::FactorIndices& newFactorsIndices)

Records the iSAM2 index of anchors passed in the last update()