Planes and degeneracy

Lesson 6 assumed each map plane had a centroid and normal. Here you will fit those planes and identify motions the observed geometry cannot constrain. Both tasks use eigenvalues: first to test planarity, then to find weak directions in a scan’s information matrix.

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 a capped number of 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 \rho\,\lambda_1$, for a small configurable planarity ratio $\rho$: 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 docs/implementation/6-local-map.md §6.3: keeping only the first points to arrive left thin slabs that passed the gate with wrong normals, adding 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 as many neighbouring voxels as the search radius needs, and keeps the plane whose centroid is nearest, within max_correspondence_distance.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 gate sits in the gap between the lowest ratios seen on a healthy indoor bag and the highest seen during a diverging outdoor stretch.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 many times its true length, while the RMSE sat at its normal level and correspondences stayed plentiful. 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.docs/implementation/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. This illustrates another response to a weakly constrained direction.

Explain the idea

A plane fit uses the smallest-eigenvalue eigenvector of a point covariance as its normal, provided the point spread is sufficiently planar. For a whole scan, the smallest eigenvalue of the translation information identifies a weak motion direction. A small residual can coexist with that weakness, and a translation test alone does not certify the full pose. An estimator may reject the scan, use independent motion information, or solve only well-constrained directions.

5 · The eigenvalues of real shapes

“Small compared with the middle one” becomes concrete once you picture the eigenvalues of common shapes in a voxel:

A flat wall spreads widely in two directions; sensor noise gives it only a thin thickness, so $\lambda_0$ is far below $\lambda_1$. Accepted, even when the surface is somewhat rough.
A pole spreads along one direction only: $\lambda_0$ and $\lambda_1$ are both small and similar. Rejected.
Foliage filling the voxel spreads in every direction: all three eigenvalues are similar. Rejected.

Two things follow. First, the gate is generous about thickness: a real sensor's noise leaves a flat surface far below the threshold, so the gate is not rejecting noisy surfaces at all. It is rejecting shapes: lines, blobs and balanced corners, whose ratio sits near one. Second, the pole shows why the comparison is with $\lambda_1$ and not $\lambda_2$. Against $\lambda_2$, a pole's ratio would be small and it would pass, handing ICP a “normal” pointing in an arbitrary direction around the pole.

How good is a fitted normal?

With $n$ points of noise $\sigma$ spread over a patch whose in-plane standard deviation is $\sqrt{\lambda_1}$, the normal's angular error is roughly $\sigma/(\sqrt{\lambda_1}\sqrt{n})$. More points buy a better normal, until the cost of storing and refitting them outweighs the gain, which is why the per-voxel cap is not arbitrary. It is also why the reservoir sampling of the map matters, since the formula assumes the points really are spread over the patch.

Try it: a small patch fills only part of a voxel. What happens to the normal's accuracy?

The in-plane spread $\sqrt{\lambda_1}$ shrinks with the patch, so the angular error grows in the same proportion. Small patches give poorly determined normals, one reason thin structures help more with translation constraints than with precise orientation.

6 · How little geometry rescues a degenerate scan

The quiz's floor-plus-one-wall scan has a ratio of exactly zero: nothing constrains sliding along the wall. Add $k$ correspondences on a second, perpendicular wall and the missing direction gains information $k$, while the other two keep theirs. If the floor and first wall each contribute $W$, the ratio is $k/(2W+k)$.

A small number of points on the new wall is enough to move the scan across the gate. That seems small until you remember what the ratio measures: those few points pin the direction along which all the others say nothing at all. It also shows why the threshold needs data behind it, as glass-lio's did. A lower gate would accept scans with barely any constraint in the weak direction; a higher one would reject scans that are in fact adequately constrained. Neither choice can be argued from first principles.

7 · Rotation has degeneracies too, and $\mathbf{H}_t$ cannot see them

Section 4's callout warned that $\mathbf{H}_t$ tests translation only. The rotation columns of lesson 6's Jacobian, $\mathbf{q}\times\mathbf{n}$, make the rotational version just as easy to read. Take points on a flat floor, $\mathbf{n} = [0,0,1]$:

$$ \mathbf{q}\times\mathbf{n} = \begin{bmatrix}q_y\\ -q_x\\ 0\end{bmatrix}. $$

The yaw component is zero for every floor point, wherever it lies. Spinning about the vertical axis slides every floor point across the floor and never changes a residual. So a scan of an open, flat car park, with nothing vertical in range, loses three degrees of freedom at once: both horizontal translations and yaw. $\mathbf{H}_t$ would report the two missing translations, but not the missing yaw.

A few shapes to recognise on sight:

scene	free directions
open flat ground	$x$, $y$, yaw
straight corridor (floor and two parallel walls)	translation along the corridor
smooth circular tunnel	translation along the axis, and roll about it
a single wall in front	sideways, vertical, and roll about the wall's normal

glass-lio does compute a rotational counterpart: the same smallest-eigenvalue-over-trace ratio, taken on the LiDAR-only rotation block of the tight solve's $\mathbf{H}$. It does not reject scans. It decides whether this scan's gyro-bias estimate is trustworthy enough to feed back into deskewing, because a bias “measured” while the LiDAR barely constrains rotation is mostly IMU dead reckoning, and feeding that into the next scan's geometry would corrupt it.livox_mid_360.yaml, lines 132–137: min_rotation_eigenvalue_ratio, built the same way as the translation ratio.

Three remedies, compared

Reject the scan (glass-lio's loose path): safe, but the pose coasts on a prediction with no floor under it, and a long degenerate stretch still drifts. Solve only the well-conditioned directions (Zhang et al.): keeps what the scan does know, leaves the rest at the prediction. Fill the null space with another sensor (glass-lio's tight path): the IMU's information enters the same $\mathbf{H}$, so the weak direction is held by physics rather than by a rule. The first two decide what to do without information; the third adds it.

8 · 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 evenly, so its points spread in three directions. What does the planarity gate do?

A scan's correspondences lie on the floor (normal $z$) and 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}$?

A pose drifts along a direction the observed planes cannot constrain, while point-to-plane RMSE stays small. Why does RMSE miss it?

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

A thin pole crosses a voxel. Why compare $\lambda_0$ with $\lambda_1$ rather than with $\lambda_2$?

A scan sees only the floor and a front wall, so it is degenerate. What does adding a few correspondences on a perpendicular side wall do?

A scan sees only flat ground. Which rotation does it fail to constrain?

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.