Merge pull request #2317 from borglab/fix/sl4

Fix SL4 to determinant issue

Author: dellaert

.github/workflows/build-macos.yml

@@ -95,6 +95,7 @@ jobs:
       - name: Install Boost
         if: contains(matrix.name, 'boost') && matrix.build_type == 'Release'
         run: |
+          brew update
           brew upgrade boost --quiet || brew install boost --quiet
           echo "GTSAM_USE_BOOST_FEATURES=ON" >> $GITHUB_ENV
           echo "GTSAM_ENABLE_BOOST_SERIALIZATION=ON" >> $GITHUB_ENV

gtsam/geometry/SL4.cpp

@@ -7,7 +7,10 @@
 #include <gtsam/geometry/SL4.h>
 
 // To use exp(), log()
+#include <cmath>
+#include <limits>
 #include <unsupported/Eigen/MatrixFunctions>
+#include <Eigen/SVD>
 
 using namespace std;
 
@@ -65,19 +68,39 @@ Eigen::Matrix<double, 15, 16> setAlgtoVecMatrix() {
 // ALG_TO_VEC * VEC_TO_ALG is equals to I_15x15
 const Eigen::Matrix<double, 16, 15> VEC_TO_ALG = setVecToAlgMatrix();
 const Eigen::Matrix<double, 15, 16> ALG_TO_VEC = setAlgtoVecMatrix();
+
 }  // namespace
 namespace gtsam {
 
 SL4::SL4(const Matrix44& pose) {
-  double det = pose.determinant();
-  if (det <= 0.0) {
+  // Compute SVD: pose = U * S * V^T
+  const Eigen::JacobiSVD<Matrix44> svd(pose, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+  Matrix44 U = svd.matrixU();
+  const Matrix44 V = svd.matrixV();
+  const Vector4 S = svd.singularValues();
+
+  // Handle Orientation (Negative Determinant / Reflection)
+  const double detUV = (U * V.transpose()).determinant();
+  
+  if (detUV < 0.0) {
+    U.col(3) = -U.col(3);
+  }
+
+  // Reconstruct the matrix with corrected orientation
+  const Matrix44 M_corrected = U * S.asDiagonal() * V.transpose();
+  const double current_det_mag = S.prod();
+  
+   // Check for Singularity
+  if (current_det_mag <= std::numeric_limits<double>::epsilon() || !std::isfinite(current_det_mag)) {
     throw std::runtime_error(
-        "Matrix determinant must be positive for SL(4) normalization. Got det "
-        "= " +
-        std::to_string(det));
+        "SL4 Constructor: Input matrix is singular or invalid. " 
+        "SVD singular values product = " + std::to_string(current_det_mag));
   }
 
-  T_ = pose / std::pow(det, 1.0 / 4.0);
+  // Normalize: T = M / det^(1/4)
+  const double scale = std::pow(current_det_mag, 0.25);
+  T_ = M_corrected / scale;
 }
 
 /* ************************************************************************* */
@@ -89,9 +112,18 @@ bool SL4::equals(const SL4& sl4, double tol) const {
 }
 /* ************************************************************************* */
 SL4 SL4::ChartAtOrigin::Retract(const Vector15& v, ChartJacobian H) {
-  SL4 retracted(I_4x4 + Hat(v));
   if (H) throw std::runtime_error("SL4::Retract: Jacobian not implemented.");
-  return retracted;
+
+  const Matrix44 candidate = I_4x4 + Hat(v);
+  const double det = candidate.determinant();
+
+  // Use fast first-order retraction when it stays inside SL(4); fall back to
+  // the true exponential map otherwise to avoid invalid determinants.
+  if (det > 0.0 && std::isfinite(det)) {
+    return SL4(candidate);
+  }
+
+  return Expmap(v);
 }
 
 /* ************************************************************************* */
@@ -114,11 +146,17 @@ SL4 SL4::Expmap(const Vector& xi, SL4Jacobian H) {
 
   // NOTE(hlim):
   // The cost of the computation is approximately 20n^3 for matrices of size n.
-  // The number 20 depends weakly on the norm of the matrix.
-  // See
+  // The number 20 depends weakly on the norm of the matrix. See
   // https://eigen.tuxfamily.org/dox/unsupported/group__MatrixFunctions__Module.html
 
-  return SL4(A.exp());
+  // For SL(4), the Lie algebra consists of trace-zero 4x4 matrices.
+  // The exponential of a trace-zero matrix should have determinant 1 by the property:
+  // det(exp(A)) = exp(trace(A)) = exp(0) = 1.
+  // However, for large tangent vectors, numerical errors in the matrix exponential
+  // can cause the determinant to drift from 1. The constructor handles normalization.
+  
+  Matrix44 expA = A.exp();
+  return SL4(expA);
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testSL4.cpp

@@ -35,6 +35,12 @@ static const Vector15 xi2 =
      0.12, 0.11, 0.15, 0.13, 0.14)
         .finished();
 
+// define xi_large - moderately large values to test numerical stability
+// while remaining within the feasible range for matrix exponential
+static const Vector15 xi_large =
+    (Vector15() << 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4,
+     1.5, 1.6, 1.7, 1.8, 1.9)
+        .finished();
 // Create a SL4
 const Matrix4 T_matrix =
     (Matrix4() << 1, 0, 0, 1, 0, 1, 0, 2, 0, 0, 1, 3, 0, 0, 0, 1).finished();
@@ -60,6 +66,21 @@ TEST(SL4, Identity) {
 TEST(SL4, Expmap) {
   SL4 expected(SL4::Expmap(xi0));
   EXPECT(assert_equal(expected, SL4::Expmap(xi0), 1e-8));
+  
+  // Verify that the result has determinant 1
+  Matrix4 T = expected.matrix();
+  double det = T.determinant();
+  EXPECT_DOUBLES_EQUAL(1.0, det, 1e-8);
+}
+
+/* ************************************************************************* */
+TEST(SL4, ExpmapLargeTangent) {
+  // Test with large tangent vector - the numerical stability fix should handle this
+  SL4 result = SL4::Expmap(xi_large);
+  // Verify determinant is 1
+  Matrix4 T = result.matrix();
+  double det = T.determinant();
+  EXPECT_DOUBLES_EQUAL(1.0, det, 1e-6);  // Looser tolerance for large inputs
 }
 
 /* ************************************************************************* */
@@ -74,6 +95,22 @@ TEST(SL4, Retract) {
   SL4 actual = SL4::Retract(xi);
   SL4 expected(I_4x4 + SL4::Hat(xi));
   EXPECT(assert_equal(expected, actual, 1e-8));
+  
+  // Verify that the result has determinant 1
+  Matrix4 T = actual.matrix();
+  double det = T.determinant();
+  EXPECT_DOUBLES_EQUAL(1.0, det, 1e-8);
+}
+
+/* ************************************************************************* */
+TEST(SL4, RetractLargeTangent) {
+  // Test with large tangent vector - the retraction will fall back to Expmap
+  // for large inputs that produce invalid first-order approximations.
+  SL4 result = SL4::Retract(xi_large);
+  // Verify determinant is 1
+  Matrix4 T = result.matrix();
+  double det = T.determinant();
+  EXPECT_DOUBLES_EQUAL(1.0, det, 1e-6);  // Looser tolerance for large inputs
 }
 
 /* ************************************************************************* */