namespace mola::state_estimation_smoother

Overview

namespace state_estimation_smoother {

// enums

enum KinematicModel;

// structs

struct Snapshot;

// classes

class FastPredictor;
class Parameters;
class StateEstimationSmoother;

// global variables

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

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

} // namespace state_estimation_smoother

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
    )

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.

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.