The point-to-plane factor

Lessons 3–5 built an IMU factor. Now build a LiDAR factor: a point’s signed distance to a plane in a map. Derive its pose Jacobian and the contribution of one correspondence to the normal equations.

0 · Warm-up, from memory

From lesson 2, and it matters here: this lesson turns on which side a small rotation is applied on.

From memory: write the rotation line of eq. (31). Which side does the gyro increment multiply on, and why?

1 · The residual

Take a laser return $\mathbf{p}$ in the sensor frame, move it into the world with the current pose, $\mathbf{q} = \mathbf{T}\mathbf{p}$, and match it to a plane in the map with centroid $\mathbf{c}$ and unit normal $\mathbf{n}$. The residual is the point's signed distance to that plane:registration.hpp, lines 62–74. Where $\mathbf{c}$ and $\mathbf{n}$ come from, a plane fitted per map voxel, is lesson 7.

$$ r(\mathbf{T}) = \mathbf{n}^{\top}(\mathbf{T}\mathbf{p} - \mathbf{c}) = \mathbf{n}^{\top}(\mathbf{q} - \mathbf{c}) $$

Why a plane, not the nearest point? glass-lio's comment says it best: "a LiDAR never re-samples the same physical points, it hits the same surface in different spots. Penalising only the distance ALONG the normal lets points slide freely ACROSS the surface." Low's classic note adds that the point-to-plane metric "has been shown to converge much faster than one that uses the point-to-point error metric".Low, Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration, UNC TR04-004, 2004. FAST-LIO2 uses the same geometry: "a point is registered to a small local plane in the map".Xu et al., FAST-LIO2: Fast Direct LiDAR-inertial Odometry, arXiv:2107.06829. It cites Low's note for this residual.

2 · Perturb on the left

glass-lio's loose path solves for the pose $\mathbf{T}\in SE(3)$ with a left perturbation, $\mathbf{T} \leftarrow \mathrm{Exp}(\boldsymbol{\xi})\,\mathbf{T}$, where $\boldsymbol{\xi} = [\boldsymbol{\rho};\ \boldsymbol{\phi}]$ is translation then rotation. The warm-up's rule decides the side: this correction is expressed in the world frame, so it composes on the left, while the gyro's increment lives in the body frame and composes on the right. glass-core's solver header says "Same manifold, same Exp, opposite side. Swapping them silently produces an estimator that still runs, still converges, and is wrong."gauss_newton.hpp, lines 17–29. glass-lio's own derivation is in docs/implementation/5-registration.md, §3.3–3.4.

3 · Derive the Jacobian

To first order a small rotation acts like a cross product, so $\mathrm{Exp}(\boldsymbol{\xi})\,\mathbf{q} \approx \mathbf{q} + \boldsymbol{\rho} + \boldsymbol{\phi}\times\mathbf{q}$. Low does the same thing in Euler angles by "replacing sin θ by θ and cos θ by 1". Substitute into the residual:

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

The last step is the scalar triple product, $\mathbf{n}\cdot(\boldsymbol{\phi}\times\mathbf{q}) = \boldsymbol{\phi}\cdot(\mathbf{q}\times\mathbf{n})$. Read off the coefficients of $\boldsymbol{\rho}$ and $\boldsymbol{\phi}$:

$$ \mathbf{J} = \frac{\partial r}{\partial\boldsymbol{\xi}} = \begin{bmatrix}\mathbf{n}^{\top} & (\mathbf{q}\times\mathbf{n})^{\top}\end{bmatrix} $$

In code, two lines:registration.hpp, lines 76–96. Its warning: "(q x n), NOT (n x q): swapping them flips the sign of every rotational update, and the solver will then walk away from the solution while still reporting a plausible residual." test_jacobian.cpp checks it against finite differences, and proves the swapped version would be caught.

J.head<3>() = normal.transpose();
J.tail<3>() = q.cross(normal).transpose();

4 · The build

Gauss-Newton minimises $\sum_i w_i\,(r_i + \mathbf{J}_i\boldsymbol{\xi})^2$. Setting the derivative to zero gives the normal equations $\mathbf{H}\boldsymbol{\xi} = \mathbf{b}$, accumulated one correspondence at a time. Each one adds a rank-1, 6×6 term to $\mathbf{H}$:

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

Here $w$ is the Huber weight: 1 for small residuals, shrinking for big ones, so "a handful of gross residuals (a moving car, a wall seen for the first time)" can't dominate.gauss_newton.hpp, lines 31–78, and its comment on lines 59–61: "BOTH FOLD INTO THE SAME H AND b. That sum IS the sensor fusion." The IMU factor enters the same $\mathbf{H}$ through addBlock.

const double w = huberWeight(r, huber_delta);
H_.noalias() += w * J.transpose() * J;
b_.noalias() -= w * J.transpose() * r;

Then solve and retract, on the left, with $\mathrm{Exp}$, never by adding $\boldsymbol{\xi}$ to the pose:gauss_newton.hpp, lines 172–181: "Never add xi to the state -- SE(3) is not a vector space."

const Eigen::Matrix<double, 6, 1> xi = eq.solve();
T = Sophus::SE3d::exp(xi) * T;

Re-associate every point at the new pose and repeat: "that re-association IS the ICP loop".registration.cpp, lines 38–62. One thing to notice for lesson 7: $\mathbf{H}$'s translation block is $\sum w\,\mathbf{n}\mathbf{n}^{\top}$. If every normal points the same way, that block is singular.

5 · Same residual, different derivative

The tight solve uses the same residual, but perturbs rotation on the right, like the IMU state ($\mathbf{R}\leftarrow\mathbf{R}\,\mathrm{Exp}(\delta\boldsymbol{\phi})$). With $\mathbf{R}\,\mathrm{Exp}(\delta\boldsymbol{\phi})\,\mathbf{p} \approx \mathbf{R}\mathbf{p} - \mathbf{R}\,\mathbf{p}^{\wedge}\delta\boldsymbol{\phi}$:

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

glass-core flags it in capitals, "THIS IS NOT THE SAME JACOBIAN AS pointToPlaneJacobian()", and notes that copying the left one in "still runs, still converges, and is wrong."nav_residual.hpp, lines 275–312. Two more differences in the tight loop: each residual and its Jacobian are divided by lidar_sigma, giving each point an information of $1/\sigma^{2}$, on the same footing as the IMU's $\Sigma^{-1}$ from lesson 5; and the zero columns mean LiDAR has no direct sensitivity to velocity or bias. Those variables are constrained through the IMU coupling and the bias prior.tight_registration.cpp, lines 64–91. The gravity columns are zero too: "a laser return sees the pose, not gravity".

J.block<1, 3>(0, kIdxPhi) =
  -normal.transpose() * x.R.matrix() * Sophus::SO3d::hat(p_sensor);
J.block<1, 3>(0, kIdxPos) = normal.transpose();

Explain the idea

"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."

6 · What one wall can and cannot see

Read the Jacobian's columns for returns that all land on a single wall, and the geometry of point-to-plane appears directly.

Translation along the normal is seen by every point. Each return reports how far it sits from the wall, so together they fix the sensor's distance to it.
Rotation about the wall's own normal has a zero column. $(\mathbf{q}\times\mathbf{n})$ is always perpendicular to $\mathbf{n}$, so spinning about the normal slides points across the wall, never towards it. A single wall cannot see that rotation at all.
The other two rotations are seen through lever arms. The rotation column is the point's offset from the sensor, crossed with the normal. Points on opposite sides of the sensor get lever arms of opposite sign, so a rotation pushes one towards the wall and the other away, and together they pin that rotation far better than either alone. The farther a point sits from the sensor, the harder it pins.

Add up the $\mathbf{J}^\top\mathbf{J}$ terms and $\mathbf{H}$ is nonzero only in those three directions. Sliding along the wall in either tangent direction, and rotating about its normal, get nothing from this wall, however many points land on it. A single plane constrains at most three of the six degrees of freedom. That is the whole reason lesson 7 exists: the solve needs planes facing in different directions, and it needs a way to detect when it does not have them.

Try it: add returns from a floor. Which new directions does it constrain, and what is still free?

The floor's normal is vertical, so it adds vertical translation, and its lever arms add the two rotations about horizontal axes. Floor and wall together cover everything except sliding horizontally along the wall. That remaining direction is exactly what a corridor fails to constrain.

7 · Finding the plane: association

The derivation assumed each point came with its plane. Finding that plane is association, and it is where most real-world failures of ICP start. glass-lio's map is a voxel hash with a plane cached per voxel (lesson 7 explains the planes). For each point it looks at the voxels around the point's own voxel, as many as the search radius needs, fits any plane that is stale, and keeps the plane whose centroid is closest, provided it is within max_correspondence_distance.local_map.cpp, lines 138–172; the centroid distance is line 162.

Two properties of that rule are worth understanding, because each explains a parameter.

The search radius is also a basin of attraction. A point whose true surface is farther than the search radius at the current guess finds the wrong plane, or none. ICP is a local method: it converges when the initial guess is already close, and confidently locks onto the wrong wall when it is not. This is why glass-lio feeds the solver an IMU prediction rather than the previous pose.
Closest centroid is not closest surface. A point near the edge of a big voxel can be closer to a neighbouring voxel's centroid than to its own surface. The residual is still the distance to whichever plane was chosen, so an occasional wrong pick produces a large residual rather than a silent error, and that is exactly the case robust weighting is for.

Association is also why the build is iterated. After each solve the pose moves, points fall into different voxels, and some pick different planes. Re-associating every iteration is not an optimisation detail; it is what makes this ICP rather than a single least-squares fit.

8 · Robust weights

Section 4 introduced the Huber weight. Its effect is easiest to see in one dimension: estimate a single value from many measurements that agree, plus one outlier, a moving car perhaps.

Plain least squares gives the mean, and the single bad point drags the answer noticeably towards itself.
Huber, solved by reweighting and re-solving, settles close to the value the good measurements agree on. The outlier's weight falls, and its pull on the answer is capped.

The mechanism is the influence, the weight times the residual. Without robust weighting a residual pulls in proportion to its size; with Huber, any residual larger than $\delta$ pulls with the same capped strength $\delta$, however large it is. Good correspondences, below $\delta$, are treated exactly as in ordinary least squares, so nothing is lost when there are no outliers.

Two limits keep this honest. Huber reduces an outlier's influence but does not remove it: a small shift towards the outlier remains. And it helps only when outliers are a minority: if half the correspondences come from a moving truck, no weighting scheme can tell which half is the world. That is why glass-lio also refuses a solve whose final RMSE is too large, rather than trusting any answer the weighting produces.

9 · Practice

From memory, no scrolling. The last question comes from lesson 5.

Why point-to-plane rather than point-to-point?

Under the left perturbation $\mathbf{T}\leftarrow\mathrm{Exp}(\boldsymbol{\xi})\mathbf{T}$, what is the rotation block of $\mathbf{J}$?

What does one correspondence add to $\mathbf{H}$?

Why does the tight solve use a different Jacobian for the very same residual?

In the tight solve, which columns of the LiDAR factor's Jacobian are always zero?

From lesson 5. What does the inverse of the preintegration covariance do in the solve?

Many returns land on one wall with normal $\mathbf{n} = [1,0,0]$. Which rotation can they never constrain?

Why give ICP a motion prediction rather than always starting from the last pose?

With Huber weighting, how hard does a correspondence whose residual is far larger than $\delta$ pull on the solution, compared with plain least squares?

From memory: write the point-to-plane residual and its 1×6 Jacobian under the left perturbation. Then write what one correspondence adds to $\mathbf{H}$ and to $\mathbf{b}$. Stretch: the tight path's rotation block.