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

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