Planes and degeneracy

Lesson 6 took each plane's centroid $\mathbf{c}$ and normal $\mathbf{n}$ as given. This lesson is where they come from, and when the whole scan should be refused. Both answers are the same move: build a 3×3 scatter matrix and look at its smallest eigenvalue. Once for a voxel's points, to find a plane; once for the scan's normals, to find a direction no plane holds. The second one is the story behind glass-lio's Outdoor01 run: a healthy RMSE, and a trajectory 111 times too long.

0 · Warm-up, from memory

Lesson 6's build: the source of the second 3×3 matrix in this lesson.

From memory: write the point-to-plane Jacobian $\mathbf{J}$ and what one correspondence adds to $\mathbf{H}$.

1 · From points to a plane

Each map voxel keeps up to 20 points. To fit a plane, take their centroid and covariance:local_map.cpp, fitPlane(), lines 100–136.

$$ \mathbf{c} = \frac{1}{n}\sum_{k}\mathbf{p}_k, \qquad \mathbf{C} = \frac{1}{n}\sum_{k}(\mathbf{p}_k - \mathbf{c})(\mathbf{p}_k - \mathbf{c})^{\top} $$

$\mathbf{C}$ is symmetric, so it has three orthogonal eigenvectors. Each eigenvalue is the variance of the points along its eigenvector. Sort them $\lambda_0 \le \lambda_1 \le \lambda_2$: for points on a surface, $\lambda_1$ and $\lambda_2$ measure the spread along it and $\lambda_0$ its thickness. So the normal is the eigenvector of the smallest eigenvalue. This is Hoppe et al.'s classic recipe: the centroid of the neighbourhood, and a normal "determined using principal component analysis", taken as the eigenvector of the smallest eigenvalue of the covariance.Hoppe, DeRose, Duchamp, McDonald & Stuetzle, Surface Reconstruction from Unorganized Points, SIGGRAPH 1992, §3.2. They number the eigenvalues largest first, so their normal is $\hat{\mathbf{v}}_3$.

const Eigen::Vector3d ev = es.eigenvalues();
if (ev[1] < 1e-12 || ev[0] > planarity_ratio_ * ev[1]) {
  return;
}

v.plane.centroid = centroid;
v.plane.normal = es.eigenvectors().col(0).normalized();

2 · The planarity gate

PCA hands every voxel a "normal", plane or not. So glass-lio only accepts one if $\lambda_0 \le 0.1\,\lambda_1$: the thickness must be small compared with the middle spread.livox_mid_360.yaml, lines 186–204: tighten it in vegetation, where blobs sneak through; loosen it in sparse indoor scenes, where too few planes form. The rejecting line in fitPlane() is commented "a blob or an edge, not a plane -- a normal here would be noise". Read the three eigenvalues and each case falls out:

A flat patch: $\lambda_0 \ll \lambda_1 \le \lambda_2$. Accepted.
A corner (wall meets floor): a balanced sample usually spreads in three directions and is rejected. An uneven sample dominated by one surface can still pass; this ratio is a heuristic, not a surface classifier.
A blob (foliage): all three similar. Rejected. Hoppe: when "the eigenvalues are similar… the eigenvectors do not reveal the surface's true tangent plane."
A line (a pole's edge): $\lambda_0 \approx \lambda_1 \ll \lambda_2$. Comparing with $\lambda_1$, not $\lambda_2$, is what catches it: a line has no single normal.

The gate is only as good as the points it sees.glass-lio's doc/6-local-map.md §6.3: keeping only the first 20 points left thin slabs that passed the gate with wrong normals, "1.2e6 of spurious stiffness" in a corridor test. Reservoir sampling fixed it.

3 · Finding a plane for each point

For a world point $\mathbf{q}$, glass-lio hashes $\lfloor\mathbf{q}/\text{voxel size}\rfloor$, checks that voxel and its 26 neighbours, and keeps the plane whose centroid is nearest, within max_correspondence_distance. The 27 cells cover that radius only because the voxel size (1.0 m) is at least the correspondence distance (1.0 m).local_map.cpp, closestPlane(), lines 138–172; std::floor, not a cast, in keyOf() (lines 23–31). A published relative: VoxelMap keeps "one plane (or edge) feature" per voxel, "organized by a Hash table and octrees", inside an iterated EKF (Yuan et al., RA-L 2022). It's a hash lookup, not a nearest-neighbour tree search, which is why registration is cheap.

4 · The second eigen-test: degeneracy

From the warm-up: every correspondence adds $w\,\mathbf{n}\mathbf{n}^{\top}$ to $\mathbf{H}$'s translation block, so

$$ \mathbf{H}_t = \sum_i w_i\,\mathbf{n}_i\mathbf{n}_i^{\top} $$

Same reading as before. Its smallest eigenvalue's eigenvector is the translation direction that the planes constrain least. If every normal is perpendicular to some direction, sliding along it changes no residual at all. Try it: a scan of floor plus one wall, then add a second wall.

What this test can establish

$H_t$ tests pure translation with rotation held fixed. A zero eigenvalue gives a pure-translation null direction. A positive ratio does not prove that all six pose degrees of freedom are observable: rotation and coupled rotation–translation motions may still be unobservable.

To examine the full pose, inspect the full Jacobian's nullspace or the full 6×6 information matrix, with appropriate translation/rotation scaling. If $H_{rr}$ is invertible, the translation information after eliminating rotation is $H_{tt}-H_{tr}H_{rr}^{-1}H_{rt}$, which can be smaller than $H_{tt}$.

glass-lio divides by the trace, because the raw eigenvalue "scales with correspondence count and so cannot tell 'under-constrained' from 'just fewer points'". It calibrated the threshold from real data: the indoor bag never dropped below 0.117, and the diverging Outdoor01 stretch never rose above 0.034, so the gate sits at 0.05, in the gap.registration.hpp, lines 25–36; livox_mid_360.yaml, lines 49–61.

const Eigen::Matrix3d Ht = final_eq.H().topLeftCorner<3, 3>();
const Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(Ht);
const double trace = Ht.trace();
result.translation_eigenvalue_ratio =
  trace > 0.0 ? es.eigenvalues().minCoeff() / trace : 0.0;

In the loose path, a ratio below the gate marks the scan invalid. The tight path does not reject a scan using this translation ratio: it adds IMU information in the same solve. Its separate rotation ratio gates only the gyro-bias feedback into deskew.registration.cpp, lines 72–88. It re-associates once at the converged pose, to test the $\mathbf{H}$ the answer actually rests on. Why was it needed? On M3DGR's Outdoor01, glass-lio's path came out at 38,657 m against a true 346 m, while the RMSE sat at its normal 0.10–0.16 and correspondences never fell below 300. A pose sliding along a free direction leaves every residual small. The gate stops those scans being trusted; tight coupling, with the IMU in the same solve, is what carries the pose through (RPE 22.6 m → 0.29 m).doc/5-registration.md §3.6.1, "a healthy RMSE can still be a lie".

Zhang, Kaess & Singh read degeneracy the same way, from "eigenvalues and eigenvectors of the system" ($\mathbf{A}^{\top}\mathbf{A}$, which is Gauss-Newton's $\mathbf{H}$), but choose a different remedy: they "only partially solve the problem in well-conditioned directions" instead of rejecting the scan.Zhang, Kaess & Singh, On Degeneracy of Optimization-based State Estimation Problems, ICRA 2016. That contrast is worth having ready in an interview.

Say it at a whiteboard

"Both LiDAR tests are the same move: build a 3×3 scatter matrix and read its smallest eigenvalue. For a voxel's points, the covariance's smallest-eigenvalue eigenvector is the plane normal, and if that eigenvalue isn't much smaller than the middle one, it's a blob, a corner or a line, so there's no plane. For the whole scan, $\mathbf{H}$'s translation block is $\sum w\,\mathbf{n}\mathbf{n}^{\top}$; its smallest-eigenvalue eigenvector is the direction no plane constrains, and when that eigenvalue is a tiny fraction of the trace, like floor plus one wall, the pose can slide along it with a perfectly healthy RMSE. glass-lio's loose path refuses scans below 0.05; the tight path can use IMU information along the weak direction; Zhang et al. instead solve only the well-conditioned directions."

5 · Practice

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

Which eigenvector of a voxel's point covariance is the plane normal?

A voxel samples a wall–floor corner with eigenvalues $(0.03,0.04,0.08)$ m². What does the 0.1 planarity gate do?

A scan's correspondences: 100 on the floor (normal $z$), 100 on one wall (normal $x$), nothing else. What is $\lambda_{\min}(\mathbf{H}_t)/\mathrm{tr}(\mathbf{H}_t)$?

Why does the gate use $\lambda_{\min}/\mathrm{tr}(\mathbf{H}_t)$ rather than the raw $\lambda_{\min}$?

On Outdoor01 the pose drifted hundreds of metres while the RMSE stayed at 0.10–0.16. Why didn't the RMSE catch it?

From lesson 6. Which block of $\mathbf{J} = [\mathbf{n}^{\top},\ (\mathbf{q}\times\mathbf{n})^{\top}]$ builds $\mathbf{H}$'s translation block?

From memory: write how a voxel's plane is fitted (centroid, covariance, normal) and the planarity test. Then write $\mathbf{H}_t$ and the degeneracy test.

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