struct mola::state_estimation_smoother::StateEstimationSmoother::GtsamImpl

Overview

struct GtsamImpl
{
    // typedefs

    typedef std::pair<frame_index_t, frame_index_t> frame_pair_t;
    typedef mola::factors::ImuReading RawImu;

    // structs

    struct GaugeAnchor;

    // fields

    std::optional<NewestLastFixedLagSmoother> smoother;
    std::map<odometry_frameid_t, gtsam::Pose3> odomFrameCovPending;
    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;
    gtsam::Pose3 mapAnchorValue;
    bool mapObserved = false;
    bool mapObservedSolved = false;
    bool mapPositionObserved = false;
    bool mapPositionObservedSolved = false;
    std::optional<GaugeAnchor> enuYawAnchor;
    std::optional<gtsam::Point2> firstGnssEnuXY;
    std::optional<gtsam::Point2> firstGnssMapXY;
    std::optional<GaugeAnchor> orientationAnchor;
    std::optional<gtsam::Key> orientationAnchorKey;
    gtsam::Pose3 orientationAnchorValue;
    bool rollPitchObserved = false;
    bool yawObserved = false;
    std::optional<gtsam::Point2> firstGnssXYForYaw;
    std::optional<frame_index_t> lastYawBiasKf;
    double lastYawBiasStamp = 0;
    std::optional<std::pair<double, double>> yawBiasEstimate;
    double yawBiasSigmaStamp = 0;
    std::deque<RawImu> rawImu;
    std::string rawImuLabel;
    std::optional<std::regex> rawImuLabelRe;
    gtsam_shared_ptr<gtsam::PreintegrationParams> pimParams;
    gtsam::Rot3 rawImuRotation;
    std::set<frame_pair_t> imuLinksAwaiting;
    gtsam::imuBias::ConstantBias lastBias;
    double lastBiasStamp = 0;
    std::optional<std::pair<double, gtsam::Rot3>> enuLevelRef;
    bool enuLevelDone = false;
    gtsam::Rot3 enuLeveledRotation;
    gtsam_shared_ptr<gtsam::PreintegrationParams> pimParamsMapGravity;

    // 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<odometry_frameid_t, gtsam::Pose3> odomFrameCovPending

Odometry frames whose covariance in State::last_estimated_frames is not computed yet for the latest solve, with the value of their graph variable in that solve (the covariance is expressed at it). Few solves have a consumer for it, and each one is a Bayes-tree marginal query. Filled in by complete_odom_frame_covariance_locked().

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}.

gtsam::Pose3 mapAnchorValue

Where mapAnchor pins that frame (identity, or its value before a reset)

bool mapObservedSolved = false

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

bool mapPositionObserved = false

Whether some source observes the position of {map} (GNSS through a fixed geo-reference, or a pose in {map}), not just its orientation (absolute attitude), and its value as of the last solve:

std::optional<GaugeAnchor> enuYawAnchor

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

std::optional<gtsam::Point2> firstGnssMapXY

Where the vehicle had the GNSS antenna in {map} at that first fix.

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.

gtsam::Pose3 orientationAnchorValue

Where that frame starts (identity, or its value before a reset)

std::optional<frame_index_t> lastYawBiasKf

Tail of the chain of IMU attitude yaw bias variables B(k), and its key stamp.

std::optional<std::pair<double, double>> yawBiasEstimate

Latest solved IMU attitude yaw bias: mean and sigma [rad], and the key stamp of its last sigma update

std::deque<RawImu> rawImu

oldest first

std::string rawImuLabel

the IMU in use

gtsam::Rot3 rawImuRotation

sensor -> vehicle

std::optional<std::pair<double, gtsam::Rot3>> enuLevelRef

An instant [s] and the vehicle attitude in {map} known for it, used to level T_enu_to_map from the accelerometer before its first solve.

gtsam::Rot3 enuLeveledRotation

of T_enu_to_map

gtsam_shared_ptr<gtsam::PreintegrationParams> pimParamsMapGravity

pimParams with gravity held fixed in {map}, at the leveled tilt

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