IMU factor#
ImuFactorBatch ties two keyframes together through the raw gyroscope and
accelerometer samples recorded between them. It is the inertial part of
visual-inertial bundle adjustment, inertial PnP and visual-inertial odometry
back ends. The keyframe poses it reads are the same pose states that
ReprojectionFactorBatch and PnPFactorBatch read.
Unlike the classical preintegrated IMU factor, it does not cache integrated deltas at a reference bias. Every evaluation integrates the samples at the current bias, treats the states between samples as variables of an inner least-squares problem, and eliminates them exactly (a Schur complement). The outer solver sees a dense factor on the two keyframes whose normal equations are exactly those of the full sample-by-sample chain at the current linearization point. There is no reference bias, no first-order bias correction and no re-integration policy to tune.
When to use it#
Visual-inertial bundle adjustment (keyframes, landmarks, IMU between consecutive keyframes): estimate poses, velocities and IMU biases together with the structure. See Example: visual-inertial bundle adjustment.
Inertial PnP / tracking: two frames joined by an IMU factor, one of them with prior information from the previous solve, landmarks fixed. See Example: visual-inertial odometry with RANSAC.
Inertial pose graphs and smoothing: keyframes with IMU factors and other pose factors (priors, between factors, position priors).
The factor needs a known gravity vector in the world frame (a parameter, not estimated) and at least one sample per keyframe pair.
Conventions#
Quantity |
Convention |
|---|---|
Pose state \(T\) |
|
Extrinsic |
|
Velocity \(v\) |
|
Bias \(b\) |
|
Gravity \(g\) |
|
Samples |
Gyroscope \(\tilde\omega\) [rad/s] and specific force \(\tilde a\) [m/s²] in the IMU frame, and the step duration \(\Delta t\) [s] over which they are applied. |
Specific force. An accelerometer at rest on a table reads \(+9.81\) m/s² along the up axis: \(\tilde a = R^\top (a - g) + b_a\), with \(a\) the true world acceleration. Feed the raw accelerometer output.
Theory#
Measurement model and kinematics#
With \((R, v, p)\) the IMU’s orientation, velocity and position in the world, the sensor measures
where \(\omega\) is the body angular velocity and \(\eta_g, \eta_a\) are white noise. The kinematics are
The discrete chain#
The \(N\) samples between keyframes \(a\) and \(b\) define \(N\) Euler steps between states \(x_0 = x_a, x_1, \dots, x_N = x_b\). The bias of keyframe \(a\) is held over the interval:
Sample \(k\) is applied over \([t_k, t_{k+1})\); the durations add up to the time between the keyframes. Write \(f_k(x_k)\) for the right-hand side. The intermediate states use the tangent \([\delta\varphi;\ \delta v;\ \delta p]\) (right rotation perturbation, world-frame velocity and position).
Noise model#
The noise densities are continuous-time, as on IMU data sheets and in Kalibr calibrations: a sample averaged over \(\Delta t\) has variance \(\sigma^2 / \Delta t\), and a random walk grows by \(\sigma^2 \Delta t\). The defect of step \(k\),
then has covariance
with \(J_r = J_r((\tilde\omega_k - b_g)\Delta t_k)\) the SO(3) right
Jacobian. The velocity and position blocks are isotropic, so \(R_k\) drops
out. The last term, integration_noise_density \(\sigma_i\), models the
Euler step’s own error (GTSAM’s integration covariance). It must be positive:
velocity and position noise both come from \(\eta_a\), so without it a
one-sample chain would have a singular covariance.
The biases follow a random walk across the interval, which gives six more rows
with \(\sigma_{bg}\) for the gyroscope bias and \(\sigma_{ba}\) for the accelerometer bias.
Eliminating the chain#
Consider the chain as its own least-squares problem. The keyframe states \((T_a, v_a, b_a, T_b, v_b, b_b)\) and the \(N-1\) intermediate states are variables, and every step contributes \(\|d_k\|^2_{Q_k^{-1}}\). It is linearized at the states integrated forward from keyframe \(a\), where every interior defect is zero. In the Gauss-Newton system
(\(B\): keyframe variables, \(I\): intermediate states), eliminating \(\delta_I\) leaves the Schur complement
Solving the outer problem with \((\bar H, \bar g)\) gives the same keyframe step as keeping every intermediate state in the problem. This is variable elimination (Dellaert and Kaess), known as condensing in multiple shooting (Bock and Plitt). The factor returns a residual \(r\) and Jacobian \(J\) with
so the outer minimizer also sees the chain’s marginal cost, which line search and the Levenberg-Marquardt gain ratio use. The marginal has rank 15: given the start state and the bias, the chain predicts the 9-dimensional end state up to noise, and the bias random walk adds 6. Hence 15 residual rows, not 30.
Computing the marginal: covariance form#
The elimination is a Riccati recursion along the chain. Its information form (a block-tridiagonal Cholesky factorization of \(H_{II}\)) loses about five digits in float32 over 200 samples. Each step subtracts the large per-step information to leave the much smaller accumulated information. The factor uses the equivalent covariance form, which only accumulates positive terms. It propagates the covariance of the predicted end state,
with \(E_k = \mathrm{Exp}((\tilde\omega_k - b_g)\Delta t_k)\). For the Jacobian it also propagates the sensitivities of the prediction to the start rotation and to the bias. Measured against a float64 elimination of the explicit chain at 200 samples, the float32 marginal Hessian is accurate to about \(2 \cdot 10^{-6}\) (relative). The information form is off by \(10^{-1}\).
Let \(\hat x_b = (\hat R, \hat v, \hat p)\) be the predicted IMU state at keyframe \(b\), \(x_b = (R_b, v_b, p_b)\) the IMU state given by keyframe \(b\)’s states, and \(\Sigma = L L^\top\) (Cholesky). The chain rows are
\(D\) accounts for the rotation residual’s tangent at the prediction. Since \(J_l(e_R)\,e_R = e_R\), it leaves \(r\) unchanged, and with it the identities above hold exactly at any state. The test suite checks them against the explicit chain. As usual in Gauss-Newton, the whitening is held constant when differentiating.
Jacobians#
A state perturbation \(T\,\mathrm{Exp}([\varphi;\ \rho])\) acts in the rig frame. With \(T = (R, t)\) and \(T_{bi} = (R_{bi}, t_{bi})\) it moves the IMU by
Before whitening, \(\partial e/\partial(\cdot)\) has these blocks (rows: rotation; velocity; position):
State |
Block |
|---|---|
\(T_a\) |
\(\big[-\Psi_\varphi R_{bi}^\top + [0;\ 0;\ R_a[t_{bi}]_\times] \ \big|\ [0;\ 0;\ -R_a]\big]\), where \(\Psi_\varphi = [\Delta R^\top;\ -R_0[\Delta v]_\times;\ -R_0[\Delta p]_\times]\) |
\(v_a\) |
\(-[0;\ I;\ T\,I]\) |
\(b_a\) |
\(-[G_g \mid G_a]\), the bias sensitivities of the prediction |
\(T_b\) |
\(\big[[\hat R^\top R_b;\ 0;\ -R_b[t_{bi}]_\times]\ \big|\ [0;\ 0;\ R_b]\big]\) |
\(v_b\) |
\([0;\ I;\ 0]\) |
\(b_b\) |
0 (in the bias rows only) |
Here \(R_a, R_b\) are the rotations of \(T_a, T_b\), \(R_0 = R_a R_{bi}\) is the IMU orientation at keyframe \(a\), and \((\Delta R, \Delta v, \Delta p)\) are the integrated deltas in the frame of \(R_0\) (without gravity). Because \(\varphi\) rotates a keyframe about its own origin, the rotation columns contain only the extrinsic lever arm \(|t_{bi}|\), not the distance from the world origin.
Comparison with preintegration#
Preintegrated factor (Forster et al.) |
|
|
|---|---|---|
Deltas at the current bias |
First-order correction from a reference bias; re-integration past a threshold |
Exact: integrated at every evaluation |
Covariance |
Computed once, at the reference bias |
Recomputed at every evaluation |
Residual |
\(9 + 6\) (combined factor) |
\(9 + 6\), the exact marginal of the sample chain |
Cost per evaluation |
\(O(1)\), plus \(O(N)\) re-integrations |
\(O(N)\), on the GPU, parallel over factors and within chains |
At the reference bias both give the same linearization. They differ as the bias estimate moves away from it.
Parallel evaluation#
The sweep along a chain is sequential, but it is associative. A run of samples reduces to a summary in the frame of its first state: the deltas \((\Delta R, \Delta v, \Delta p)\) without gravity, the duration \(T\), the accumulated noise \(Q\) and the bias sensitivities \(G\). The run’s transition is
Two consecutive runs \(a, b\) compose like preintegrated deltas:
Each factor’s chain therefore runs on 1 to 32 GPU threads, which integrate
consecutive segments and combine the summaries in a binary tree. Batches
with fewer factors than the GPU has threads split chains over a warp; large
batches give each thread a whole chain. The split depends on the batch size,
the number of SMs and the typical chain length (the num_samples
argument). Each factor uses at most one thread per 4 of its samples.
API#
States and residual#
One factor per keyframe pair, SizedFactorBatch<15, 6, 3, 6, 6, 3, 6>:
Slot |
State |
Tangent |
Jacobian columns |
|---|---|---|---|
0 |
\(T_a\), |
6, \([\varphi; \rho]\) |
0–5 |
1 |
\(v_a\), |
3 |
6–8 |
2 |
\(b_a = [b_g; b_a]\), |
6 |
9–14 |
3 |
\(T_b\) |
6 |
15–20 |
4 |
\(v_b\) |
3 |
21–23 |
5 |
\(b_b\) |
6 |
24–29 |
Residual rows 0–8 are the whitened chain defect \(L^{-1} e\). Rows 9–14 are the bias random walk, gyroscope first.
Sample buffer#
All samples of all factors go into one device buffer, back to back, with CSR offsets (capacity + 1 of them):
imu_samples = [ω_x ω_y ω_z a_x a_y a_z Δt] x num_samples (float32)
sample_offsets = [0, n_0, n_0 + n_1, ..., num_samples] (int32)
Factor \(f\) uses samples [sample_offsets[f], sample_offsets[f + 1]).
The first starts at keyframe \(a\) and the last ends at keyframe \(b\).
A sensor that time-stamps each sample at the end of its interval (sample
\(m\) covering \((t_{m-1}, t_m]\)) maps directly: store
\((\tilde\omega_m, \tilde a_m, t_m - t_{m-1})\). The buffers are read at
every evaluation and must outlive the factor; they may be rewritten between
solves.
ImuParameters#
Field |
Unit |
Default |
Meaning |
|---|---|---|---|
|
m/s² |
(0, 0, −9.80665) |
World-frame gravity. |
|
rad/s/√Hz |
1.6968e-4 |
\(\sigma_g\), gyroscope white noise. |
|
m/s²/√Hz |
2.0e-3 |
\(\sigma_a\), accelerometer white noise. |
|
m/√s |
1e-4 |
\(\sigma_i\), Euler step error on the position. Must be positive. |
|
rad/s²/√Hz |
1.9393e-5 |
\(\sigma_{bg}\). |
|
m/s³/√Hz |
3.0e-3 |
\(\sigma_{ba}\). |
|
— |
identity |
Pose of the IMU in the rig frame (rig_from_imu). |
The noise defaults are those of the ADIS16448 in the EuRoC MAV dataset. All five densities must be positive. Read Float32 conditioning before using realistic values together with vision.
C++#
Header: cunls/factor/imu_factor_batch.h.
ImuFactorBatch(const float *imu_samples, const int *sample_offsets,
size_t num_samples, const ImuParameters ¶meters,
size_t capacity);
imu_samples,sample_offsets: device buffers as above (not null).num_samples: number of samples the buffer holds. Only sizes the work split:num_samples / capacityis taken as the typical chain length.parameters: copied at construction;Parameters()returns them.capacity: keyframe pairs the buffers hold. CallSetNumActiveFactors(n)before solving.
Throws std::invalid_argument for a null buffer or a non-positive noise
density. Evaluation follows the FactorBatch item contract (item and plain
evaluations are bitwise equal), so the factor also works with the RANSAC
minimizers.
Python#
p = pycunls.ImuParameters() # the fields above
p.gravity = [0.0, 9.81, 0.0] # lists; body_from_imu: 16 floats, row-major
imu = pycunls.ImuFactorBatch(imu_samples, sample_offsets, num_samples, p, capacity)
imu.set_num_active_factors(capacity)
imu_samples must be a float32 array and sample_offsets an int32 array
(TypeError otherwise). Raw integer device pointers are accepted unchecked.
The factor keeps references to both arrays.
Example: visual-inertial bundle adjustment#
python/examples/imu_bundle_adjustment.py simulates ten keyframes of a
smooth trajectory, 40 IMU samples between consecutive keyframes (200 Hz) and
300 landmarks observed by nearby keyframes. IMU and reprojection factors share
the pose states. Velocities and biases start at zero, poses and landmarks
start perturbed, and keyframe 0 is fixed (the gauge). Output on an RTX A6000:
10 keyframes, 40 IMU samples each, 300 landmarks, 1320 observations
LM: 8 iterations, cost 2.51e+05 -> 0.000287
max position error 1.23e-03 m
max velocity error 9.68e-04 m/s
gyro bias [ 0.00999385 -0.01996889 0.01499784] (true [ 0.01 -0.02 0.015])
accel bias [ 0.10012887 -0.04981745 0.07999814] (true [ 0.1 -0.05 0.08])
The problem setup:
# Noise densities a few times larger than a navigation-grade spec (the
# defaults are the EuRoC ADIS16448): with very stiff IMU rows next to
# vision, a float32 solve converges slowly (see the guide, "Float32
# conditioning").
params = pycunls.ImuParameters() # gravity (0, 0, -9.80665): world +Z up
params.gyro_noise_density = 1e-2 # rad/s/sqrt(Hz)
params.accel_noise_density = 1e-1 # m/s^2/sqrt(Hz)
params.integration_noise_density = 1e-2 # m/sqrt(s)
params.gyro_bias_random_walk = 1e-2 # rad/s^2/sqrt(Hz)
params.accel_bias_random_walk = 1e-1 # m/s^3/sqrt(Hz)
gravity = np.array(params.gravity, dtype=np.float64)
keyframes, samples = simulate(K, N, gravity, rng)
true_poses = [world_from_rig(R, p) for R, _, p in keyframes]
true_points, observations, pairs = landmarks_and_observations(true_poses, rng)
L = len(true_points)
# --- States: perturbed poses and points, zero velocities and biases ---
init_poses = []
for k, X in enumerate(true_poses):
Xi = X.copy()
if k > 0: # keyframe 0 is fixed (gauge)
Xi[:3, 3] += rng.normal(0, 0.02, 3)
init_poses.append(Xi)
pose_buf = cp.asarray(np.stack(init_poses), dtype=cp.float32)
vel_buf = cp.zeros((K, 3), dtype=cp.float32)
bias_buf = cp.zeros((K, 6), dtype=cp.float32)
point_buf = cp.asarray(true_points + rng.normal(0, 0.05, true_points.shape), dtype=cp.float32)
fixed = cp.asarray([0], dtype=cp.int32)
poses = pycunls.SE3StateBatch(pose_buf, K, fixed, 1)
poses.set_num_active_states(K, 1)
vels = pycunls.VectorStateBatch3(vel_buf, K)
vels.set_num_active_states(K)
biases = pycunls.VectorStateBatch6(bias_buf, K)
biases.set_num_active_states(K)
points = pycunls.VectorStateBatch3(point_buf, L)
points.set_num_active_states(L)
# --- IMU factors: samples of all keyframe pairs back to back, CSR offsets ---
imu_samples = cp.asarray(samples, dtype=cp.float32) # (num_samples, 7)
offsets = cp.asarray(np.arange(K) * N, dtype=cp.int32) # K - 1 pairs + 1
imu = pycunls.ImuFactorBatch(imu_samples, offsets, len(samples), params, K - 1)
imu.set_num_active_factors(K - 1)
imu_ptrs = []
for k in range(K - 1):
for j in (k, k + 1): # X_a, v_a, b_a, X_b, v_b, b_b
imu_ptrs += [poses.state_device_ptr(j), vels.state_device_ptr(j),
biases.state_device_ptr(j)]
# --- Reprojections (normalized image coordinates, sigma = 1e-3) on the same poses ---
obs_buf = cp.asarray(observations, dtype=cp.float32)
reproj = pycunls.WeightedFactorBatch(
pycunls.ReprojectionFactorBatch(obs_buf, len(pairs)), 1e3)
reproj.set_num_active_factors(len(pairs))
reproj_ptrs = []
for k, i in pairs:
reproj_ptrs += [poses.state_device_ptr(k), points.state_device_ptr(i)]
problem = pycunls.Problem()
for s in (poses, vels, biases, points):
problem.add_state_batch(s)
problem.add_factor_batch(imu, imu_ptrs)
problem.add_factor_batch(reproj, reproj_ptrs)
options = pycunls.LevenbergMarquardtMinimizerOptions()
options.base_options.max_num_iterations = 30
stream = pycunls.CudaStream()
summary = pycunls.LevenbergMarquardtMinimizer(options).minimize(stream, problem)
The IMU factor in C++, with the states and buffers allocated as for any problem:
#include "cunls/cunls.h"
ImuParameters params; // gravity (0, 0, -9.80665)
params.gyro_bias_random_walk = 1e-2f;
params.accel_bias_random_walk = 1e-1f;
// d_samples: num_samples x 7 floats; d_offsets: K ints (K - 1 pairs + 1).
ImuFactorBatch imu(d_samples, d_offsets, num_samples, params, K - 1);
imu.SetNumActiveFactors(K - 1);
std::vector<float *> imu_ptrs;
for (int k = 0; k + 1 < K; ++k) {
for (int j : {k, k + 1}) {
imu_ptrs.push_back(pose_states.StateDevicePtr(j)); // SE3StateBatch
imu_ptrs.push_back(vel_states.StateDevicePtr(j)); // VectorStateBatch<3>
imu_ptrs.push_back(bias_states.StateDevicePtr(j)); // VectorStateBatch<6>
}
}
problem.AddFactorBatch(&imu, imu_ptrs);
// The reprojection factors read the same pose_states.
Example: visual-inertial odometry with RANSAC#
python/examples/tartan_vio.py is a small RGB-D inertial odometry on the
TartanGround OldTownFall anymal sequence P2000 (82 m in 129 s, a camera
at 10 Hz with depth, an IMU at 100 Hz). It tracks KLT features (OpenCV) and
creates each track’s landmark from the depth image at its first frame. Every
frame solves one problem on a two-frame window: the pose, velocity and bias of
the previous and the current frame (30 free tangent dimensions), one
ImuFactorBatch between them, priors on the previous frame from the last
solve, and one PnPFactorBatch factor per tracked landmark.
RansacLevenbergMarquardtMinimizer samples the matches (two per hypothesis,
since the IMU predicts the motion) and keeps the IMU factor and the priors
always on. The buffers, the problem and the minimizers are created once;
every frame rewrites the buffers, the active count and the connectivity
(Problem.set_state_pointers).
The example adds noise and biases to the dataset’s ideal IMU and injects
swapped matches, coherently shifted matches (repetitive texture), a camera
blackout and 75% clutter into the 2D matches. Visual-only RANSAC PnP, the
same window solved by Levenberg-Marquardt with a Huber loss, and IMU dead
reckoning run on the same data. --rrd / --spawn log the run to Rerun.
Output on an RTX A6000:
inertial RANSAC : final position error 0.717 m (0.87% of the path)
visual RANSAC : final position error 2.360 m (2.87% of the path)
inertial LM + Huber : final position error 4364.330 m (diverges in the clutter)
inertial RANSAC: kept 116905 of 197972 genuine matches, accepted 5 of 58106 injected outliers
Practical notes#
Gauge and observability. With known gravity, IMU factors make scale, roll
and pitch observable, but not the global position or yaw. Fix a keyframe (a
constant state) or add a prior. Biases need some rotation and acceleration
over the window. On static or straight-line segments, add a bias prior (a
weighted PriorVectorFactorBatch6 on \(b\)).
Initialization. Velocities and biases can start at zero when the poses are reasonable (from vision or the previous solve). Gravity must be known: align the world frame with it first.
Weighting. Wrap the factor in WeightedFactorBatch (uniform or
per-factor weights) to down-weight IMU factors, for example at the boundary of
a sliding window or in frame-to-frame tracking.
Extrinsics and timing. body_from_imu must be calibrated (for example
with Kalibr). Time offsets between camera and IMU are not modeled: shift the
timestamps before building the sample buffer.
Float32 conditioning. cuNLS solves in float32. Realistic IMU noise makes the IMU rows very stiff compared with vision and with the weakly observed directions (absolute biases, velocity along the motion). The bias random-walk rows weigh \(1/(\sigma_b\sqrt T)\), about \(10^5\) for the default gyroscope value over 0.2 s. The linear solve then loses the weak directions, and Levenberg-Marquardt converges slowly or stalls. In the tests and the example:
A 6-keyframe trajectory with pose priors stalls with the default bias random walks and converges with \((\sigma_{bg}, \sigma_{ba}) = (10^{-2}, 10^{-1})\).
The example with the default (EuRoC) chain noise reaches cost 0.02 after 100 iterations but is still 2–3 cm off. With the noise in the example it converges in 8 iterations to about 1 mm.
When this happens, loosen the noise densities or down-weight the IMU factors. This trades some statistical efficiency for convergence.
Empty chains. A factor without samples (offsets[f] == offsets[f + 1]),
or whose samples have no positive duration, has zero covariance and carries no
information. It evaluates to all-zero residual and Jacobian rows, so it adds
nothing to the solve. States constrained only by such a factor (for example the
velocity of the next keyframe) are then unobserved; give each pair at least
one sample.
Limits#
Gravity is a parameter; it is not estimated.
The bias is constant between two keyframes (a random walk across keyframes only).
Euler integration: its error is well below the noise at typical IMU rates (100–1000 Hz), but grows at low rates or high angular velocities.
No measurements on the intermediate states (barometer, wheel odometry, zero-velocity updates), although the elimination would support them.
No camera-IMU time offset.
Performance#
Time per evaluation, with Jacobians / residuals only (RTX A6000):
Samples per factor |
16 factors |
1,024 factors |
65,536 factors |
|---|---|---|---|
20 |
29 / 11 µs |
31 / 12 µs |
418 / 135 µs |
200 |
33 / 15 µs |
44 / 29 µs |
2.0 / 1.9 ms |
1,000 |
58 / 37 µs |
159 / 131 µs |
— |
Large batches run at about 0.15 ns per sample. In a visual-inertial bundle adjustment with 100 keyframes (200 samples per pair), 10,000 landmarks and 41,600 reprojections, the IMU kernels take 1.9% of the GPU time. The preconditioned conjugate gradient solve takes most of the rest.
References#
C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, “On-Manifold Preintegration for Real-Time Visual-Inertial Odometry,” IEEE Transactions on Robotics 33(1), 2017. Measurement model, noise propagation and the preintegrated factor this one replaces.
T. Lupton, S. Sukkarieh, “Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions,” IEEE Transactions on Robotics 28(1), 2012. Origin of IMU preintegration.
L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, “Eliminating Conditionally Independent Sets in Factor Graphs: A Unifying Perspective based on Smart Factors,” IEEE ICRA, 2014. Marginalizing variables inside a factor.
F. Dellaert, M. Kaess, “Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing,” International Journal of Robotics Research 25(12), 2006. Variable elimination in factor graphs.
H. G. Bock, K. J. Plitt, “A Multiple Shooting Algorithm for Direct Solution of Optimal Control Problems,” IFAC Proceedings 17(2), 1984. Condensing the intermediate states of a shooting chain.
J. Solà, J. Deray, D. Atchuthan, “A micro Lie theory for state estimation in robotics,” arXiv:1812.01537, 2018. Right perturbations and the SO(3) / SE(3) Jacobians.
M. Burri et al., “The EuRoC micro aerial vehicle datasets,” International Journal of Robotics Research 35(10), 2016. Source of the default noise densities.
GTSAM:
PreintegrationParams(integrationCovariance) andCombinedImuFactor. The integration-noise term and the combined factor with bias rows.