namespace mola::factors

Overview

namespace factors {

// namespaces

namespace mola::factors::detail;

// structs

struct ImuReading;

// classes

class FactorAngularVelocityIntegration;
class FactorAngularVelocityIntegrationPose;
class FactorConstLocalVelocity;
class FactorConstLocalVelocityPose;
class FactorGnssEnu;
class FactorGnssMapEnu;
class FactorGnssMapEnuConstVel;
class FactorImuPreintegrationBodyVel;
class FactorImuPreintegrationBodyVelEnu;
class FactorTrapezoidalIntegrator;
class FactorTrapezoidalIntegratorPose;
class FactorTricycleKinematic;
class MapGravityFactor;
class MeasuredGravityFactor;
class Pose3RotationFactor;
class Pose3RotationYawBiasFactor;

// global functions

bool imu_accel_looks_like_gravity(const gtsam::Vector3& measuredAcc);
bool imu_quaternion_looks_valid(double qw, double qx, double qy, double qz);
gtsam::Rot3 imu_apply_enu_azimuth_correction(const gtsam::Rot3& rawAttitude, double azimuthOffsetDeg);

bool integrate_imu_readings(
    const std::deque<ImuReading>& readings,
    double ti,
    double tj,
    double accScale,
    double maxReadingGap,
    gtsam::PreintegratedImuMeasurements& pim
    );

} // namespace factors

Global Functions

bool imu_accel_looks_like_gravity(const gtsam::Vector3& measuredAcc)

Returns true if the given accelerometer reading looks like a plausible gravity vector, accepting either raw m/s^2 units (norm ~9.8) or an already-normalized (“g”) unit vector (norm ~1.0), as reported by different IMU drivers.

bool imu_quaternion_looks_valid(double qw, double qx, double qy, double qz)

Returns true if the given IMU absolute-orientation quaternion (w,x,y,z) is finite and normalized, i.e. safe to convert into a gtsam::Rot3.

gtsam::Rot3 imu_apply_enu_azimuth_correction(
    const gtsam::Rot3& rawAttitude,
    double azimuthOffsetDeg
    )

Applies the fixed ENU-convention correction (yaw=0 => East) plus a user-calibrated azimuth offset (e.g. sensor mounting or magnetic declination) to a raw IMU attitude reading, so the result can be compared against poses expressed in the ENU frame.

bool integrate_imu_readings(
    const std::deque<ImuReading>& readings,
    double ti,
    double tj,
    double accScale,
    double maxReadingGap,
    gtsam::PreintegratedImuMeasurements& pim
    )

Integrates readings (oldest first, strictly increasing stamps) over [ti, tj] into pim, whose parameters (gravity, body_P_sensor) and bias linearization point are kept. Accelerations are multiplied by accScale.

Readings are taken as instantaneous samples, linearly interpolated in between. With an IMU away from the body origin (body_P_sensor), the tangential acceleration of the lever arm, which gtsam does not model, is compensated from the change of the gyro readings.

Integration starts at the last reading at or before ti (or at the first one, if it comes at most maxReadingGap after ti), and fails if two consecutive readings are farther apart than maxReadingGap or the readings do not reach tj.

Returns:

true if [ti, tj] was fully integrated.