Why preintegrate

Lesson 2 integrated a single IMU sample. Here we combine an interval into three deltas that can be reused when an optimizer changes the starting pose. Derive the deltas from repeated state updates, then decide when preintegration saves work.

0 · Warm-up, from memory

One retrieval from lesson 2, the line this lesson iterates. If you're doing this in the same sitting as lesson 2, do this box again tomorrow too: it only tests memory after a gap.

From memory: write the velocity line of eq. (31), with noise dropped.

1 · Iterate one step, many times

Between two keyframes $i$ and $j$ there are many IMU samples $k$. Apply eq. (31) to each in turn and the steps stack up:Forster et al., On-Manifold Preintegration…, §VI, eq. (32), plus a matching sum for position. Noise is dropped here, and the bias is taken as constant between keyframes, as Forster assumes in eq. (34).

$$ \begin{aligned} \mathbf{R}_j &= \mathbf{R}_i \prod_{k=i}^{j-1} \mathrm{Exp}\big((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}^{g})\,\Delta t\big)\\ \mathbf{v}_j &= \mathbf{v}_i + \mathbf{g}\,\Delta t_{ij} + \sum_{k=i}^{j-1} \mathbf{R}_k\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t \end{aligned} \tag{32} $$

Here $\Delta t_{ij}$ is the total time from $i$ to $j$. Gravity sums to a single $\mathbf{g}\,\Delta t_{ij}$, because it's the same constant at every step.

2 · The problem

Look at $\mathbf{R}_k$ inside the sum. That's the world orientation at sample $k$, and it's built on top of $\mathbf{R}_i$. So every term in eq. (32) depends on the state at keyframe $i$. Forster says it directly: "a change in the rotation $\mathbf{R}_i$ implies a change in all future rotations $\mathbf{R}_k$", so the whole integration has to be redone whenever the linearization point at $t_i$ moves.Forster §VI, the paragraph after eq. (32).

Inside an optimizer, the estimate of $\mathbf{R}_i$ can move every iteration. Without preintegration, each move repeats the sample integration. That cost grows with interval length and the number of solver iterations; section 10 compares a short scan interval with a longer-lived sliding-window factor.

3 · The move

Split each world orientation into "keyframe $i$" times "the rotation since $i$": $\mathbf{R}_k = \mathbf{R}_i\,\Delta\mathbf{R}_{ik}$. The rotation since $i$, $\Delta\mathbf{R}_{ik}$, is just the product of gyro steps, so it depends only on measurements. Now pull $\mathbf{R}_i$ out of the sum and multiply both sides by $\mathbf{R}_i^{\top}$:

$$ \begin{aligned} \mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij} &= \mathbf{R}_i \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t\\ \mathbf{R}_i^{\top}(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij}) &= \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t \end{aligned} $$

Now look at what sits on each side:

Left: only states ($\mathbf{R}_i, \mathbf{v}_i, \mathbf{v}_j$) and gravity. It's cheap to re-evaluate every iteration.
Right: only IMU measurements and the bias. No $\mathbf{R}_i$ anywhere, so moving $\mathbf{R}_i$ can't change it. Integrate it once per interval.

That right-hand side is what gets called $\Delta\mathbf{v}_{ij}$. The same move gives $\Delta\mathbf{R}_{ij}$ and $\Delta\mathbf{p}_{ij}$.

4 · The three deltas

$$ \begin{aligned} \Delta\mathbf{R}_{ij} &\doteq \mathbf{R}_i^{\top}\mathbf{R}_j = \prod_{k=i}^{j-1} \mathrm{Exp}\big((\tilde{\boldsymbol{\omega}}_k - \mathbf{b}^{g})\,\Delta t\big)\\ \Delta\mathbf{v}_{ij} &\doteq \mathbf{R}_i^{\top}(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij})\\ &= \sum_{k=i}^{j-1} \Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t\\ \Delta\mathbf{p}_{ij} &\doteq \mathbf{R}_i^{\top}\big(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i\,\Delta t_{ij} - \tfrac{1}{2}\mathbf{g}\,\Delta t_{ij}^{2}\big)\\ &= \sum_{k=i}^{j-1} \Big[\Delta\mathbf{v}_{ik}\,\Delta t + \tfrac{1}{2}\,\Delta\mathbf{R}_{ik}\,(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t^{2}\Big] \end{aligned} \tag{33} $$

Each line reads as "state side $\doteq$ measurement side".Forster §VI, eq. (33). He also warns that, unlike $\Delta\mathbf{R}_{ij}$, "neither $\Delta\mathbf{v}_{ij}$ nor $\Delta\mathbf{p}_{ij}$ correspond to the true physical change in velocity and position". They're defined to make the right-hand side independent of the state at $i$ and of gravity. Gravity always ends up on the state side, where $\mathbf{R}_i$ is already in hand. Inside the sums it would have to be expressed in frame $i$, which would bring $\mathbf{R}_i$ straight back.

5 · Where this lives in glass-core

The preintegration header states exactly these three sums:preintegration.hpp, lines 25–31, which go on: "Gravity is deliberately NOT integrated here… Folding it in would re-introduce the dependence on R_i we just worked to remove."

///     dR_ij = prod Exp( (w_k - b_g) dt )
///     dv_ij = sum  dR_ik (a_k - b_a) dt
///     dp_ij = sum  [ dv_ik dt + 1/2 dR_ik (a_k - b_a) dt^2 ]

The loop you saw in lesson 2 is these sums, accumulated one sample at a time. The residual then compares the two sides of eq. (33):nav_residual.hpp, lines 27–43: "Read each as 'what the STATE says happened' minus 'what the IMU says happened'. Zero when they agree."

///     r_dR = Log( dR_hat^T * R_i^T R_j )
///     r_dv = R_i^T (v_j - v_i - g dt)                 - dv_hat
///     r_dp = R_i^T (p_j - p_i - v_i dt - 1/2 g dt^2)  - dp_hat

Notice the _hat. The deltas still depend on the bias, and the bias is a state the solver moves too. How glass-core avoids re-integrating when that changes is lesson 4.

Explain the idea

"Iterating the IMU from $i$ to $j$ puts $\mathbf{R}_i$ inside every term, so each time the optimizer moves $\mathbf{R}_i$ you'd re-integrate every sample in the interval. Preintegration writes $\mathbf{R}_k = \mathbf{R}_i\,\Delta\mathbf{R}_{ik}$, pulls $\mathbf{R}_i$ out of the sums, and moves the states and gravity to the other side. What's left, $\Delta\mathbf{R}_{ij}$, $\Delta\mathbf{v}_{ij}$ and $\Delta\mathbf{p}_{ij}$, depends only on the IMU measurements and the bias. So it's integrated once, and the residual just compares it with the states."

6 · Where $\mathbf{v}_i\,\Delta t_{ij}$ and $\tfrac12\mathbf{g}\,\Delta t_{ij}^2$ come from

The velocity line was one substitution. The position line takes two more steps, and it is the line where the starting velocity is easy to overlook, so derive it once. Iterate the position line of eq. (31) from $i$ to $j$, over $n$ steps of length $\Delta t$:

$$ \mathbf{p}_j = \mathbf{p}_i + \sum_{k=i}^{j-1}\Big[\mathbf{v}_k\,\Delta t + \tfrac12\mathbf{g}\,\Delta t^2 + \tfrac12\mathbf{R}_k(\tilde{\mathbf{a}}_k-\mathbf{b}^a)\,\Delta t^2\Big]. $$

The velocity $\mathbf{v}_k$ inside the sum is itself built from $\mathbf{v}_i$. By the velocity line you already derived, with $\Delta t_{ik} = (k-i)\,\Delta t$ the time elapsed since $i$,

$$ \mathbf{v}_k = \mathbf{v}_i + \mathbf{g}\,\Delta t_{ik} + \mathbf{R}_i\,\Delta\mathbf{v}_{ik}. $$

Substitute it, and sort every term by what it contains:

$\mathbf{v}_i$ terms: $\sum_k \mathbf{v}_i\,\Delta t = \mathbf{v}_i\,\Delta t_{ij}$. That is the first mystery term: the distance you would cover coasting at the start velocity.
Gravity terms: $\mathbf{g}\sum_k\big(\Delta t_{ik}\,\Delta t + \tfrac12\Delta t^2\big)$. With $\Delta t_{ik} = m\,\Delta t$ for $m = 0,\dots,n-1$, the bracket sums to $\Delta t^2\big(\tfrac{n(n-1)}{2} + \tfrac{n}{2}\big) = \tfrac12 n^2\Delta t^2 = \tfrac12\Delta t_{ij}^2$. The per-step gravity terms add up to exactly the free-fall distance over the whole interval, which is the second mystery term.
Everything else contains $\mathbf{R}_i$ times a measurement-only quantity: $\mathbf{R}_i\sum_k\big[\Delta\mathbf{v}_{ik}\,\Delta t + \tfrac12\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a)\,\Delta t^2\big]$.

Move the first two groups to the left and multiply by $\mathbf{R}_i^\top$:

$$ \mathbf{R}_i^\top\big(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i\,\Delta t_{ij} - \tfrac12\mathbf{g}\,\Delta t_{ij}^2\big) = \sum_{k=i}^{j-1}\Big[\Delta\mathbf{v}_{ik}\,\Delta t + \tfrac12\Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k-\mathbf{b}^a)\,\Delta t^2\Big], $$

which is the third line of eq. (33). Nothing in the derivation was clever: the only idea is “expand everything built on $\mathbf{v}_i$ and $\mathbf{R}_i$ and push it to the state side”. If you can reproduce the gravity-sum identity, you can reproduce the whole line.

7 · Who preintegrates, and who does not need to

Preintegration is not the only way to use an IMU, and knowing the alternatives tells you when it is the right tool.

Lupton and Sukkarieh (2012) introduced the idea of summarising the IMU between two poses in the start pose's frame, with rotations parametrised by Euler angles.
Forster et al. (2015, 2017), the paper this course follows, moved it onto the rotation manifold, which removed the singularities of Euler angles and gave the clean bias correction and covariance of lessons 4 and 5.
Optimisation-based VIO systems, VINS-Mono among them, build their IMU factors this way. VINS-Mono discretises the deltas with a midpoint rule instead of eq. (31), the higher-order choice lesson 2 discussed.
Filter-based systems, such as FAST-LIO2's iterated Kalman filter, propagate the state and covariance forward sample by sample, once, and then correct at the LiDAR scan. They never re-linearise a past state, so there is nothing to re-integrate, and they have no need for preintegration at all.

The dividing line is simple. If your estimator revisits past states, as windows and smoothers do, preintegration saves you from re-integrating every time it does. If it never looks back, forward propagation is simpler and just as exact. glass-lio sits in between: it builds a preintegrated factor per scan, and lesson 8 explains why that hybrid is a deliberate stepping stone rather than an accident.

8 · The same deltas, any starting orientation

Here is the payoff made concrete. Suppose the solver is unsure which way the robot faced at keyframe $i$ and tries two very different answers for $\mathbf{R}_i$. Each trial predicts $\mathbf{v}_j$ from the state side of eq. (33), $\mathbf{v}_j = \mathbf{v}_i + \mathbf{g}\,\Delta t_{ij} + \mathbf{R}_i\,\Delta\mathbf{v}_{ij}$.

The same three deltas serve both trials. Whatever forward acceleration the IMU felt is sent along whichever world direction the trial rotation says the robot faced, exactly as it should be, and the IMU samples are never touched again. Changing $\mathbf{R}_i$ costs one matrix-vector product per line, not a re-integration. Notice also that $\Delta\mathbf{v}_{ij}$ carries a large vertical component, which the state side's $\mathbf{g}\,\Delta t_{ij}$ cancels precisely. That leads straight to the most common confusion about these deltas.

9 · A stationary sensor with a large “velocity change”

Forster warns that $\Delta\mathbf{v}_{ij}$ and $\Delta\mathbf{p}_{ij}$ are not physical changes. The cleanest way to believe him is to preintegrate a sensor that does not move at all. A level IMU at rest reads gravity's reaction, $-\mathbf{g}$, every sample. Over one interval:

$$ \Delta\mathbf{v}_{ij} = \sum_k \tilde{\mathbf{a}}_k\,\Delta t = -\mathbf{g}\,\Delta t_{ij}. $$

A substantial upward “velocity change” for a sensor that never moved. Now evaluate the state side with the true states, $\mathbf{v}_i = \mathbf{v}_j = 0$, $\mathbf{R}_i = \mathbf{I}$:

$$ \mathbf{R}_i^\top(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij}) = -\mathbf{g}\,\Delta t_{ij}. $$

The two sides agree exactly and the residual is zero, which is what it should be for a correct state. The deltas are best read as what a falling observer would have seen: the change in velocity the specific force alone would produce, with gravity left for the state side to supply. That is why a residual can be zero while a delta is large, and why you should never log $\Delta\mathbf{v}_{ij}$ and read it as how fast the robot went.

A test worth writing

Preintegrate a synthetic stationary sensor and assert that the residual against a stationary state is zero, not that the deltas are. A test that asserts $\Delta\mathbf{v}=0$ at rest encodes exactly the misreading this section is about, and it will fail on correct code.

10 · How much it saves

The saving depends on how the deltas are used.

In glass-lio's own tight solve, one IMU factor spans one scan, a modest number of samples. Without preintegration each solver iteration would re-integrate them. That is real work but not catastrophic; the bigger wins here are the two things the next lessons build on the deltas: a first-order bias correction (lesson 4) and a covariance computed once (lesson 5), neither of which would be possible if the deltas were rebuilt from scratch at every iteration.

In a sliding window, the saving can grow much larger. A factor that stays in the window is re-linearised at every iteration of every update it survives, and without preintegration each re-linearisation re-integrates all of its samples:

$$ \underbrace{(\text{updates} \times \text{iterations})}_{\text{re-linearisations}} \times \text{samples} \quad\text{versus}\quad \text{samples, once, with preintegration.} $$

And that is before counting the covariance and the bias Jacobians, which would otherwise be rebuilt just as often. The longer a factor lives inside an optimiser, the more preintegration pays. That is why it is the standard building block of sliding-window and smoothing estimators.

11 · What moving $\mathbf{R}_i$ still costs: a Jacobian, in closed form

Preintegration does not make $\mathbf{R}_i$ disappear from the problem; it moves it to the state side, where it appears exactly once per line, in $\mathbf{R}_i^\top(\cdot)$. That is what makes the residual's derivatives with respect to the starting state cheap closed forms instead of sums over samples. For the velocity line, perturbing $\mathbf{R}_i$ on the right and using the toolkit's hat rules gives

$$ \frac{\partial\,\mathbf{r}_{\Delta v}}{\partial\,\delta\boldsymbol{\phi}_i} = \big(\mathbf{R}_i^\top(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij})\big)^{\wedge}, \qquad \frac{\partial\,\mathbf{r}_{\Delta v}}{\partial\,\delta\mathbf{v}_i} = -\mathbf{R}_i^\top . $$

Three multiplications, independent of how many samples the interval held.

glass-core computes these as imuJacobianI, and its header records a lesson that is worth knowing before you ever write an estimator. For a long time only the Jacobian with respect to the end state existed, because both front-ends solved for $\mathbf{x}_j$ with $\mathbf{x}_i$ held fixed. Holding $\mathbf{x}_i$ fixed asserts that it is perfectly known. Over a short interval the IMU's information is enormous, so a perfectly known start let the IMU factor outvote every other measurement.nav_residual.hpp, lines 209–233 records the failure measured on EuRoC: with this Jacobian missing, reprojection error grew monotonically while well-observed landmarks sat in view, unrejected and simply outvoted. The cure is to treat $\mathbf{x}_i$ as uncertain, which needs exactly the cheap Jacobian above. Lesson 8 takes this idea to its conclusion: filters, windows and factors differ mainly in how they treat the state at $i$.

12 · Practice

From memory, no scrolling. The last two questions come from lessons 2 and 1.

Why does eq. (32) have to be re-integrated whenever the optimizer changes $\mathbf{R}_i$?

In $\mathbf{R}_i^{\top}(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\,\Delta t_{ij}) = \sum_k \Delta\mathbf{R}_{ik}(\tilde{\mathbf{a}}_k - \mathbf{b}^{a})\,\Delta t$, which side is computed once from the IMU and reused while the solver iterates?

Why does gravity sit on the state side of eq. (33), rather than inside the sums?

From lesson 2. In the velocity line of eq. (31), which of these is added without being rotated?

From lesson 1. An IMU at rest is rolled so that its body $y$ axis points straight up. Ignoring bias and noise, what does its accelerometer read, as $(x, y, z)$?

A level IMU sits still over one preintegration interval. What is its preintegrated $\Delta\mathbf{v}_{ij}$?

The solver tries a new $\mathbf{R}_i$ rotated far from the last one. What happens to $\Delta\mathbf{v}_{ij}$?

Why did an IMU factor with only a Jacobian for $\mathbf{x}_j$ let the IMU outvote many well-observed landmarks?

From memory: write $\Delta\mathbf{v}_{ij}$ both ways, state side $=$ measurement side, with noise dropped. Then say in one sentence why the right-hand side never needs recomputing when $\mathbf{R}_i$ moves. Stretch: do the same for $\Delta\mathbf{R}_{ij}$.