Reference card · Lesson 6
Point-to-plane factor
One residual per laser return, its Jacobian, and how thousands of them build one linear system.
Key equations
Warm-up, from memory
$$ \mathbf{R}(t+\Delta t) = \mathbf{R}(t)\,\mathrm{Exp}\big((\tilde{\boldsymbol{\omega}} - \mathbf{b}^{g})\,\Delta t\big) $$
The residual
$$ r(\mathbf{T}) = \mathbf{n}^{\top}(\mathbf{T}\mathbf{p} - \mathbf{c}) = \mathbf{n}^{\top}(\mathbf{q} - \mathbf{c}) $$
Derive the Jacobian
$$ \begin{aligned}
r(\boldsymbol{\xi}) &\approx \mathbf{n}^{\top}(\mathbf{q} + \boldsymbol{\rho} + \boldsymbol{\phi}\times\mathbf{q} - \mathbf{c})\\
&= r + \mathbf{n}^{\top}\boldsymbol{\rho} + \mathbf{n}^{\top}(\boldsymbol{\phi}\times\mathbf{q})\\
&= r + \mathbf{n}^{\top}\boldsymbol{\rho} + (\mathbf{q}\times\mathbf{n})^{\top}\boldsymbol{\phi}
\end{aligned} $$
$$ \mathbf{J} = \frac{\partial r}{\partial\boldsymbol{\xi}} = \begin{bmatrix}\mathbf{n}^{\top} & (\mathbf{q}\times\mathbf{n})^{\top}\end{bmatrix} $$
The build
$$ \begin{aligned}
\mathbf{H} &\leftarrow \mathbf{H} + w\,\mathbf{J}^{\top}\mathbf{J}\\
\mathbf{b} &\leftarrow \mathbf{b} - w\,\mathbf{J}^{\top} r
\end{aligned} $$
Same residual, different derivative
$$ \begin{aligned}
\partial r/\partial\delta\boldsymbol{\phi} &= -\mathbf{n}^{\top}\mathbf{R}\,\mathbf{p}^{\wedge}\\
\partial r/\partial\delta\mathbf{p} &= \mathbf{n}^{\top}\\
\partial r/\partial(\mathbf{v}, \mathbf{b}^{g}, \mathbf{b}^{a}) &= \mathbf{0}
\end{aligned} $$
Practice
$$ \begin{aligned}
r &= \mathbf{n}^{\top}(\mathbf{q} - \mathbf{c}), \quad \mathbf{q} = \mathbf{T}\mathbf{p}\\
\mathbf{J} &= \begin{bmatrix}\mathbf{n}^{\top} & (\mathbf{q}\times\mathbf{n})^{\top}\end{bmatrix}\\
\mathbf{H} &\leftarrow \mathbf{H} + w\,\mathbf{J}^{\top}\mathbf{J}, \qquad \mathbf{b} \leftarrow \mathbf{b} - w\,\mathbf{J}^{\top}r
\end{aligned} $$
Say it at a whiteboard
"Each LiDAR point, moved into the world by the current pose, \(\mathbf{q} = \mathbf{T}\mathbf{p}\), gives one
scalar residual: its signed distance to the matched plane, \(r = \mathbf{n}^{\top}(\mathbf{q} - \mathbf{c})\).
Point-to-plane lets points slide along the surface, which is right, because a LiDAR never re-hits the same
points. With a left perturbation \(\mathbf{T}\leftarrow\mathrm{Exp}(\boldsymbol{\xi})\mathbf{T}\), first order
gives \(r + \mathbf{n}^{\top}\boldsymbol{\rho} + (\mathbf{q}\times\mathbf{n})^{\top}\boldsymbol{\phi}\), so
\(\mathbf{J} = [\mathbf{n}^{\top},\ (\mathbf{q}\times\mathbf{n})^{\top}]\). Each correspondence adds
\(w\mathbf{J}^{\top}\mathbf{J}\) to \(\mathbf{H}\) and \(-w\mathbf{J}^{\top}r\) to \(\mathbf{b}\); solve, retract with
\(\mathrm{Exp}\) on the left, re-associate, repeat. The tight solve differentiates the same residual for a
right perturbation, whitens it by the LiDAR's noise, and its velocity and bias columns are zero."