namespace mola::state_estimation_smoother

Overview

namespace state_estimation_smoother {

// typedefs

typedef gtsam::IncrementalFixedLagSmoother NewestLastFixedLagSmoother;

// enums

enum KinematicModel;

// structs

struct Snapshot;

// classes

class FastPredictor;
class Parameters;
class StateEstimationSmoother;

// global variables

constexpr size_t MIN_PREDICTION_RESIDUALS = 10;
constexpr double MIN_PREDICTION_VAR_RATE = 1e-6;
const bool NAVSTATE_PRINT_FG = mrpt::get_env<bool>("NAVSTATE_PRINT_FG", false);
const bool NAVSTATE_PRINT_FG_ERRORS = mrpt::get_env<bool>("NAVSTATE_PRINT_FG_ERRORS", false);
const double NAVSTATE_PRINT_FG_ERRORS_THRESHOLD =     mrpt::get_env<double>("NAVSTATE_PRINT_FG_ERRORS_THRESHOLD", 0.1);
const auto symbol_T_enu_to_map = F(0);
const auto symbol_T_map_to_odom_i_base = F(0);
constexpr unsigned int REFERENCE_FRAME_ID = 0;

// global functions

std::string pose_pdf_to_string_with_sigmas(const mrpt::poses::CPose3DPDFGaussian& pdf);
std::string pose_pdf_to_string_with_sigmas(const mrpt::poses::CPose3DPDFGaussianInf& pdf);

mrpt::poses::CPose3D body_twist_delta(
    const Parameters& params,
    const mrpt::math::TTwist3D& twist,
    double dt
    );

mrpt::poses::CPose3DPDFGaussian extrapolate_pose_pdf(
    const Parameters& params,
    const mrpt::poses::CPose3DPDFGaussian& anchorPose,
    const mrpt::math::TTwist3D& twist,
    const mrpt::math::CMatrixDouble66& twistCov,
    double dt,
    bool addRandomWalk = true
    );

mrpt::math::CMatrixDouble66 twist_cov_from_residuals(const std::array<double, 6>& varRate);
void apply_pose_sigma_floor(const Parameters& params, mrpt::poses::CPose3DPDFGaussian& pdf);

} // namespace state_estimation_smoother

Global Variables

constexpr size_t MIN_PREDICTION_RESIDUALS = 10

Prediction errors an odometry source must have accumulated before they set the covariance of predictions in its frame.

constexpr double MIN_PREDICTION_VAR_RATE = 1e-6

Floor of the running prediction-error rate [(m/s)^2 or (rad/s)^2] under the cap on new errors, so a rate that starts at exactly zero can still grow.

Global Functions

std::string pose_pdf_to_string_with_sigmas(const mrpt::poses::CPose3DPDFGaussian& pdf)

Converts a 3D pose Gaussian PDF into a human-readable string.

This function extracts the mean pose and the 1-sigma standard deviations (square roots of the covariance diagonal) from an mrpt::poses::CPose3DPDFGaussian, and formats them into a multi-line string of the form:

x = <mean> ± <sigma> [m]
y = <mean> ± <sigma> [m]
z = <mean> ± <sigma> [m]
yaw = <mean> ± <sigma> [deg]
pitch = <mean> ± <sigma> [deg]
roll = <mean> ± <sigma> [deg]

Translation components are expressed in meters, and rotational components in degrees.

Parameters:

pdf

The Gaussian PDF representing the 3D pose mean and covariance.

Returns:

A std::string containing the formatted pose and uncertainties.

mrpt::poses::CPose3D body_twist_delta(
    const Parameters& params,
    const mrpt::math::TTwist3D& twist,
    double dt
    )

Integrates a constant body-frame twist over dt seconds, returning the relative pose increment T_i_to_j (right-composed onto an anchor: T_j = T_i (+) delta). Branches on the configured kinematic model so the short-term extrapolation matches the motion model used to build the inter-keyframe factors: full SE(3) exp of the 6D body twist for ConstantVelocity; the planar arc from forward velocity v=vx and yaw rate w=wz for Tricycle, mirroring mola::factors::FactorTricycleKinematic exactly.

mrpt::poses::CPose3DPDFGaussian extrapolate_pose_pdf(
    const Parameters& params,
    const mrpt::poses::CPose3DPDFGaussian& anchorPose,
    const mrpt::math::TTwist3D& twist,
    const mrpt::math::CMatrixDouble66& twistCov,
    double dt,
    bool addRandomWalk = true
    )

Covariance-aware short-term pose extrapolation: returns the pose PDF of anchorPose (+) Exp(twist*dt) with first-order uncertainty propagation.

Two sources of uncertainty are combined:

  • the anchor pose covariance, transported through the composition (handled exactly by MRPT’s CPose3DPDFGaussian::operator+, i.e. the Adjoint term);

  • the body-frame increment covariance: the current-velocity uncertainty twistCov carried over the interval (dt^2 * twistCov), plus an acceleration process-noise term (~ 0.5*a*dt^2 position/attitude drift).

The increment covariance is first built in the body tangent [vx vy vz wx wy wz] (the ordering of NavState::twist_inv_cov) and then mapped to the increment’s CPose3DPDFGaussian ordering [x y z yaw pitch roll] with J = d(pose)/d(tangent). To first order at a small increment J reduces to a coordinate relabeling: identity on translation, and the body angular rates map to the Euler rates as yaw<-wz, pitch<-wy, roll<-wx. The residual Jacobian curvature is the higher-order term neglected in this sub-second regime; swap in the exact SE(3) exp Jacobian if the rotation channel ever needs it.

mrpt::math::CMatrixDouble66 twist_cov_from_residuals(const std::array<double, 6>& varRate)

Twist covariance (diagonal, twist order) of a prediction in an odometry source’s own frame, from the running mean of its squared prediction errors per unit time squared. Already includes the effect of accelerations, so the random-walk term of extrapolate_pose_pdf() must not be added on top.

void apply_pose_sigma_floor(
    const Parameters& params,
    mrpt::poses::CPose3DPDFGaussian& pdf
    )

Adds the flat, dt-independent floor of sigma_relative_pose_{linear,angular} to a pose PDF’s covariance, in place. A no-op when both are 0, which is the shipped default.

Call this as the LAST step of every path that returns a pose to a caller, i.e. AFTER any frame conversion. The floor is a statement about the pose the caller asked for, and SE(3) composition mixes the angular block into the translation one through the lever arm so a floor applied before a frame change is not a floor in the frame that comes out of it.

Isotropic within each block, so it does not depend on whether the consumer reads the rotation entries in Euler or Lie-tangent order: adding the same variance to all three commutes with any permutation of them.