ImuFactorWithGravity Family

Overview¶

The standard ImuFactor treats the nav-frame gravity vector as a known constant from PreintegrationParams. The ImuFactorWithGravity family instead makes gravity an optimized variable, for workflows where the navigation frame is not gravity-aligned (eg. LiDAR-odometry map frames), for initialization-free operation, or for online gravity refinement.

Gravity knowledge falls on a spectrum, handled by three factor choices:

Known exactly: use plain ImuFactor (gravity from params). Zero cost.
Known magnitude, unknown direction: ImuFactorWithGravityDirection optimizes a Unit3 direction (2 DOF on $S^2$ ) scaled by a fixed magnitude given at construction. On Earth, standard gravity is accurate to ~0.3% everywhere, so this is usually the right choice; it removes the magnitude degree of freedom by construction.
Unknown magnitude and direction: ImuFactorWithGravityVector optimizes the free vector $g \in \mathbb{R}^3$ (as a Point3), following Lupton and Sukkarieh (2012). When the magnitude is approximately known, pair it with a single VectorNormFactor on the gravity variable.

CombinedImuFactorWithGravityDirection / ...Vector are the corresponding variants of CombinedImuFactor, adding gravity as a 7th variable while keeping the bias random walk rows (which have a zero gravity Jacobian). ImuFactor2WithGravityDirection / ...Vector are the NavState-based variants, with one combined pose+velocity key per state, as ImuFactor2 is to ImuFactor.

Observability: only the combination $R_i^T g$ is observed by the accelerometer, so nav-frame gravity and initial attitude are entangled: anchor exactly one of them (a roll/pitch prior or a gravity prior, not both tightly). Gravity magnitude and the accelerometer bias along gravity are only jointly observable under rotation excitation whose axis changes over time (Nemiroff, Chen and Lopez, 2023).

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

# Install gtsam-develop if not installed
try:
    import gtsam
except ImportError:
    %pip install --quiet gtsam-develop

Mathematical Formulation¶

Both tangent- and manifold-preintegration accumulate the IMU measurements without gravity; gravity enters only in the PIM’s predict step (the key insight of Lupton and Sukkarieh: gravity can be changed, or estimated, at correction time without re-integrating). With $R_i$ the attitude of state $i$ and $\Delta t$ the preintegration interval, the corrected tangent vector is

\begin{aligned} \xi_{\Delta R} &= \tilde\xi_{\Delta R} \\ \xi_{\Delta p} &= \tilde\xi_{\Delta p} + \Delta t\, R_i^T v_i + \tfrac{1}{2}\Delta t^2\, R_i^T g \\ \xi_{\Delta v} &= \tilde\xi_{\Delta v} + \Delta t\, R_i^T g \end{aligned}

(1)

so the Jacobian with respect to the gravity vector is simply

\frac{\partial \xi}{\partial g} = \begin{bmatrix} 0_{3\times3} \\ \tfrac{1}{2}\Delta t^2 R_i^T \\ \Delta t\, R_i^T \end{bmatrix} \in \mathbb{R}^{9\times3}.

(2)

The two parametrizations chain onto this block:

Direction: $g = m\,d$ with $d \in S^2$ a Unit3 and $m$ fixed, so $\partial g / \partial \delta d = m B$ where $B \in \mathbb{R}^{3\times2}$ is the tangent basis of $d$ (Unit3::scaled) — the same scale-times-direction decomposition as MagFactor3.
Vector: $g$ free, $\partial g / \partial g = I_3$ ; the optional norm pseudo-observation is $e = \lVert g \rVert - 9.81$ with $\partial e/\partial g = g^T/\lVert g \rVert$ , added once per gravity variable (never per IMU factor, or the same prior information is counted multiple times).

See the “Gravity as an Optimized Variable” section of doc/ImuFactor.pdf for the full derivation.

After prediction, every factor in this family uses the default $SE_2(3)$ Logmap residual state_j.logmap(predictedState_j).

Usage Example¶

A stationary IMU in a nav frame whose true gravity is tilted away from the params’ $-z$ direction: the accelerometer measures $-R^T g_{true}$ , so optimizing the gravity variable must recover the true tilted gravity.

import numpy as np

import gtsam
from gtsam.symbol_shorthand import B, G, V, X

# True gravity: tilted ~3.3 degrees away from straight down
true_gravity = gtsam.Rot3.Rodrigues(0.05, -0.03, 0.0).rotate(
    gtsam.Point3(0, 0, -9.81))
print(f"true gravity: {np.round(true_gravity, 4)}")

# Nominal params believe gravity is straight down (MakeSharedU: z-up nav frame)
params = gtsam.PreintegrationParams.MakeSharedU(9.81)
params.setAccelerometerCovariance(1e-4 * np.eye(3))
params.setGyroscopeCovariance(1e-6 * np.eye(3))
params.setIntegrationCovariance(1e-8 * np.eye(3))

# Stationary body at identity: the accelerometer measures -g_true
pim = gtsam.PreintegratedImuMeasurements(params)
for _ in range(10):
    pim.integrateMeasurement(-true_gravity, np.zeros(3), 0.1)

true gravity: [ 0.2941  0.4902 -9.7933]

# Common graph: tight priors anchor states and bias, gravity is free
def make_graph_and_values():
    graph = gtsam.NonlinearFactorGraph()
    tight_pose = gtsam.noiseModel.Isotropic.Sigma(6, 1e-6)
    tight_vec = gtsam.noiseModel.Isotropic.Sigma(3, 1e-6)
    tight_bias = gtsam.noiseModel.Isotropic.Sigma(6, 1e-6)
    graph.addPriorPose3(X(1), gtsam.Pose3(), tight_pose)
    graph.addPriorPose3(X(2), gtsam.Pose3(), tight_pose)
    graph.addPriorVector(V(1), np.zeros(3), tight_vec)
    graph.addPriorVector(V(2), np.zeros(3), tight_vec)
    graph.addPriorConstantBias(B(1), gtsam.imuBias.ConstantBias(), tight_bias)
    values = gtsam.Values()
    values.insert(X(1), gtsam.Pose3())
    values.insert(X(2), gtsam.Pose3())
    values.insert(V(1), np.zeros(3))
    values.insert(V(2), np.zeros(3))
    values.insert(B(1), gtsam.imuBias.ConstantBias())
    return graph, values

# Mode 2: direction on the sphere, magnitude fixed to 9.81 (from params)
graph, values = make_graph_and_values()
graph.add(gtsam.ImuFactorWithGravityDirection(
    X(1), V(1), X(2), V(2), B(1), G(0), pim))
values.insert(G(0), gtsam.Unit3(np.array([0.0, 0.0, -1.0])))  # initial guess: down

result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize()
recovered = result.atUnit3(G(0)).unitVector() * 9.81
print(f"recovered gravity (direction mode): {np.round(recovered, 4)}")
print(f"direction error: {np.linalg.norm(recovered - true_gravity):.2e}")

recovered gravity (direction mode): [ 0.2941  0.4902 -9.7933]
direction error: 1.78e-15

# Mode 3: free vector, with Lupton's magnitude pseudo-observation
graph, values = make_graph_and_values()
graph.add(gtsam.ImuFactorWithGravityVector(
    X(1), V(1), X(2), V(2), B(1), G(0), pim))
graph.add(gtsam.VectorNormFactor3(
    G(0), 9.81, gtsam.noiseModel.Isotropic.Sigma(1, 0.03)))
values.insert(G(0), gtsam.Point3(0, 0, -9.0))  # never initialize at zero!

result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize()
recovered = result.atPoint3(G(0))
print(f"recovered gravity (vector mode): {np.round(recovered, 4)}")
print(f"vector error: {np.linalg.norm(recovered - true_gravity):.2e}")

recovered gravity (vector mode): [ 0.2941  0.4902 -9.7933]
vector error: 7.85e-17

# NavState variant: ImuFactor2WithGravityDirection, one key per state
graph = gtsam.NonlinearFactorGraph()
tight_state = gtsam.noiseModel.Isotropic.Sigma(9, 1e-6)
tight_bias = gtsam.noiseModel.Isotropic.Sigma(6, 1e-6)
graph.add(gtsam.PriorFactorNavState(X(1), gtsam.NavState(), tight_state))
graph.add(gtsam.PriorFactorNavState(X(2), gtsam.NavState(), tight_state))
graph.addPriorConstantBias(B(1), gtsam.imuBias.ConstantBias(), tight_bias)
graph.add(gtsam.ImuFactor2WithGravityDirection(X(1), X(2), B(1), G(0), pim))

values = gtsam.Values()
values.insert(X(1), gtsam.NavState())
values.insert(X(2), gtsam.NavState())
values.insert(B(1), gtsam.imuBias.ConstantBias())
values.insert(G(0), gtsam.Unit3(np.array([0.0, 0.0, -1.0])))

result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize()
recovered = result.atUnit3(G(0)).unitVector() * 9.81
print(f"recovered gravity (NavState mode): {np.round(recovered, 4)}")
print(f"direction error: {np.linalg.norm(recovered - true_gravity):.2e}")

recovered gravity (NavState mode): [ 0.2941  0.4902 -9.7933]
direction error: 1.78e-15

Key Functionality / API¶

Constructors: same keys as ImuFactor plus a gravity key; the Direction variants accept an optional gravityMagnitude (defaulting to the norm of the params’ gravity vector). gravityMagnitude() returns it.
evaluateError(pose_i, vel_i, pose_j, vel_j, bias, gravity) with gravity a Unit3 (Direction) or Point3 (Vector); optional Jacobians follow the usual convention (9×2 or 9×3 for the gravity argument).
Merge (tangent preintegration only) merges consecutive factors sharing bias and gravity keys, and rejects mismatched gravity keys or magnitudes.
The Combined variants add the second bias key: CombinedImuFactorWithGravity* with keys (pose_i, vel_i, pose_j, vel_j, bias_i, bias_j, gravity) and a 15-dimensional error whose bias rows have a zero gravity Jacobian.
ImuFactor2WithGravityDirection / ...Vector are the NavState-based variants (as ImuFactor2 is to ImuFactor), with keys (state_i, state_j, bias, gravity) and the same 9-dimensional error; like ImuFactor2, they provide no Merge.

Source¶

ImuFactorWithGravity.h, CombinedImuFactorWithGravity.h
Derivation: doc/ImuFactor.pdf, section “Gravity as an Optimized Variable”
T. Lupton and S. Sukkarieh, “Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions”, IEEE T-RO 28(1), 2012.
R. Nemiroff, K. Chen and B. T. Lopez, “Joint On-Manifold Gravity and Accelerometer Intrinsics Estimation for Inertially Aligned Mapping”, arXiv:2303.03505, 2023.