Fix Gram-Schmidt bug in SelfAdjointEigenSolver::computeDirect and add small matrix benchmarks

Fix a bug in the 3x3 direct eigensolver's Gram-Schmidt orthogonalization
for near-degenerate eigenvalues. The code was subtracting the projection
onto eivecs.col(l) (itself) instead of onto eivecs.col(k):

  // Before (bug): subtracts scalar multiple of self — does nothing useful
  eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l)) * eivecs.col(l);
  // After (fix): removes component along eivecs.col(k)
  eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l)) * eivecs.col(k);

This path is taken when two of three eigenvalues are nearly equal, which
is common for covariance matrices of near-planar point clouds.

Also add comprehensive small fixed-size matrix benchmarks covering the
operations that dominate robotics/CV inner loops: matmul, matvec,
inverse, determinant, LLT, LDLT, PartialPivLU, ColPivHouseholderQR,
JacobiSVD, SelfAdjointEigenSolver (iterative and direct) for sizes
2x2 through 8x9.

Note: the direct 3x3 eigensolver (computeDirect) is 3x faster than
the iterative solver but has 5-6 orders of magnitude worse residuals
for near-degenerate eigenvalues. This is inherent to the closed-form
algorithm, not a consequence of the Gram-Schmidt bug. Users should
prefer compute() when accuracy matters and computeDirect() only when
speed is critical and eigenvalues are well-separated.

Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
Rasmus Munk Larsen
2026-04-04 15:24:42 -07:00
parent 8ddbe44799
commit 3eed3b0ab9

View File

@@ -691,7 +691,7 @@ struct direct_selfadjoint_eigenvalues<SolverType, 3, false> {
if (d0 <= 2 * Eigen::NumTraits<Scalar>::epsilon() * d1) {
// If d0 is too small, then the two other eigenvalues are numerically the same,
// and thus we only have to ortho-normalize the near orthogonal vector we saved above.
eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l)) * eivecs.col(l);
eivecs.col(l) -= eivecs.col(k).dot(eivecs.col(l)) * eivecs.col(k);
eivecs.col(l).normalize();
} else {
tmp = scaledMat;