The point-to-plane factor

Lessons 3–5 built the IMU's half of glass-lio's solve. This is the other half: the LiDAR factor. Each laser return becomes one scalar residual, its distance to a plane in the map. By the end you should be able to derive that residual's 1×6 Jacobian at a whiteboard, and say exactly what one correspondence adds to the normal equations. This supplies the LiDAR terms needed to explain the complete tight solve.

0 · Warm-up, from memory

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

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.

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 doc/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:

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}\):

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 to 1e-7 over 500 trials, 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}\):

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}\):

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();

6 · Practice

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

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