Why preintegrate · Lesson 3

Lesson 2 integrated a single IMU sample. This lesson explains why we combine a whole interval into reusable deltas, and gives the derivation behind that sentence: one substitution and one multiplication turn Forster's eq. (32) into eq. (33), the preintegrated deltas $\Delta\mathbf{R}_{ij}$, $\Delta\mathbf{v}_{ij}$, $\Delta\mathbf{p}_{ij}$. It's the core of this course's first goal: deriving them from memory.

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$ moves every iteration. glass-core's own header puts the cost bluntly: "every one of those hundreds of samples must be re-integrated. Inside an iterative solve that is ruinous."preintegration.hpp, lines 13–18.

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.

Say it at a whiteboard

"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 hundreds of samples. 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 · 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)$?

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}$.

Apply it: complete the practical worksheet exercise before moving on. Derive an answer and test it on the supplied inputs.