namespace IMPLEMENTS_MRPT_OBJECT

Overview

namespace IMPLEMENTS_MRPT_OBJECT {

// global variables

constexpr double ENU2MAP_WEAK_SIGMA = 1e4;
constexpr double INIT_ODOM_FRAME_POSE_SIGMA = 1e3;
constexpr double FIRST_POSE_WEAK_PRIOR_SIGMA = 1e6;
constexpr double PLANAR_XY_SIGMA = 1e10;
constexpr double PLANAR_Z_SIGMA = 1e-4;
constexpr double TRICYCLE_LARGE_SIGMAS = 1e6;
constexpr double GYRO_HUBER_K = 1.345;

// global functions

void enforce_planar_pose(mrpt::poses::CPose3D& p);
void enforce_planar_twist(mrpt::math::TTwist3D& tw);
double key_stamp_seconds(const mrpt::Clock::time_point& t);
std::ofstream* navstate_dump_stream();

void navstate_dump_row(
    const mrpt::Clock::time_point& timestamp,
    double dt,
    const mola::NavState& ns
    );

gtsam::SharedNoiseModel with_huber(const gtsam::SharedNoiseModel& base, double threshold);
void replace_cov_block(mrpt::math::CMatrixDouble66& cov, int first, double variance);

mrpt::poses::CPose3DPDFGaussian odometry_increment_pdf(
    const mola::state_estimation_smoother::Parameters& p,
    const mrpt::poses::CPose2D& odometryIncrement
    );

} // namespace IMPLEMENTS_MRPT_OBJECT

Global Variables

constexpr double GYRO_HUBER_K = 1.345

Huber threshold [whitened units] for raw gyro observations; the standard value giving ~95% efficiency under Gaussian noise.

Global Functions

gtsam::SharedNoiseModel with_huber(
    const gtsam::SharedNoiseModel& base,
    double threshold
    )

Wraps a pose factor’s Gaussian noise model in a Huber m-estimator when threshold (in whitened units) is > 0. Huber is convex, so unlike a redescending kernel it cannot switch off a correct measurement whose variable starts far from it, as a new keyframe seeded at its neighbor’s pose does.

void replace_cov_block(
    mrpt::math::CMatrixDouble66& cov,
    int first,
    double variance
    )

Replaces the 3x3 diagonal block of a 6x6 pose covariance starting at first (0: translation, 3: rotation) with variance * I, and clears its correlations with the other block. Keeping cross terms that belong to a different (and possibly much larger) covariance would, in general, make the result indefinite.

mrpt::poses::CPose3DPDFGaussian odometry_increment_pdf(
    const mola::state_estimation_smoother::Parameters& p,
    const mrpt::poses::CPose2D& odometryIncrement
    )

Wheel-odometry increment with the uncertainty of the configured probabilistic motion model.