From 2df2ff756777c034af5826aff39be4f413078d67 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 17 Apr 2025 00:15:22 -0400
Subject: [PATCH] Better tests

---
 gtsam/geometry/tests/testPose3.cpp | 53 ++++++++++++++++++++++++++++++
 gtsam/geometry/tests/testSO3.cpp   |  9 +++--
 2 files changed, 59 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index c33985aad..29e39cfa5 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -956,6 +956,59 @@ TEST(Pose3, LogmapDerivatives) {
   }
 }
 
+//******************************************************************************
+TEST(Pose3, LogmapDerivative) {
+  // Copied from testSO3.cpp
+  const Rot3 R2((Matrix3() <<            // Near pi
+    -0.750767, -0.0285082, -0.659952,
+    -0.0102558, -0.998445, 0.0547974,
+    -0.660487, 0.0479084, 0.749307).finished());
+  const Rot3 R3((Matrix3() <<            // Near pi
+    -0.747473, -0.00190019, -0.664289,
+    -0.0385114, -0.99819, 0.0461892,
+    -0.663175, 0.060108, 0.746047).finished());
+  const Rot3 R4((Matrix3() <<            // Final pose in a drone experiment
+    0.324237, 0.902975, 0.281968,
+    -0.674322, 0.429668, -0.600562,
+    -0.663445, 0.00458662, 0.748211).finished());
+
+  // Now creates poses
+  const Pose3 T0; // Identity
+  const Vector6 xi(0.1, -0.1, 0.1, 0.1, -0.1, 0.1);
+  const Pose3 T1 = Pose3::Expmap(xi);  // Small rotation
+  const Pose3 T2(R2, Point3(1, 2, 3));
+  const Pose3 T3(R3, Point3(1, 2, 3));
+  const Pose3 T4(R4, Point3(1, 2, 3));
+  size_t i = 0;
+  for (const Pose3& T : { T0, T1, T2, T3, T4 }) {
+    const bool nearPi = (i == 2 || i == 3); // Flag cases near pi
+
+    Matrix6 actualH; // H computed by Logmap(T, H) using LogmapDerivative(xi)
+    const Vector6 xi = Pose3::Logmap(T, actualH);
+
+    // 1. Check self-consistency of analytical derivative calculation:
+    //    Does the H returned by Logmap match an independent calculation
+    //    of J_r^{-1} using ExpmapDerivative with the computed xi?
+    Matrix6 J_r_inv = Pose3::ExpmapDerivative(xi).inverse(); // J_r^{-1}
+    EXPECT(assert_equal(J_r_inv, actualH)); // This test is crucial and should pass
+
+    // 2. Check analytical derivative against numerical derivative:
+    //    Only perform this check AWAY from the pi singularity, where
+    //    numerical differentiation of Logmap is expected to be reliable
+    //    and should match the analytical derivative.
+    if (!nearPi) {
+      const Matrix expectedH = numericalDerivative11<Vector6, Pose3>(
+        std::bind(&Pose3::Logmap, std::placeholders::_1, nullptr), T, 1e-7);
+      EXPECT(assert_equal(expectedH, actualH, 1e-5)); // 1e-5 needed to pass R4
+    }
+    else {
+      // We accept that the numerical derivative of this specific Logmap implementation
+      // near pi will not match the standard analytical derivative J_r^{-1}.
+    }
+    i++;
+  }
+}
+
 /* ************************************************************************* */
 Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
   return Pose3::adjointMap(xi) * v;
diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp
index 1a1300864..feac58093 100644
--- a/gtsam/geometry/tests/testSO3.cpp
+++ b/gtsam/geometry/tests/testSO3.cpp
@@ -295,9 +295,12 @@ TEST(SO3, LogmapDerivative) {
     -0.747473, -0.00190019, -0.664289,
     -0.0385114, -0.99819, 0.0461892,
     -0.663175, 0.060108, 0.746047).finished());
-
+  const SO3 R4((Matrix3() <<            // Final pose in a drone experiment
+      0.324237, 0.902975, 0.281968,
+      -0.674322, 0.429668, -0.600562,
+      -0.663445, 0.00458662, 0.748211).finished());
   size_t i = 0;
-  for (const SO3& R : { R0, R1, R2, R3 }) {
+  for (const SO3& R : { R0, R1, R2, R3, R4 }) {
     const bool nearPi = (i == 2 || i == 3); // Flag cases near pi
 
     Matrix3 actualH; // H computed by Logmap(R, H) using LogmapDerivative(omega)
@@ -317,7 +320,7 @@ TEST(SO3, LogmapDerivative) {
     if (!nearPi) {
       const Matrix expectedH = numericalDerivative11<Vector3, SO3>(
         std::bind(&SO3::Logmap, std::placeholders::_1, nullptr), R, 1e-7);
-      EXPECT(assert_equal(expectedH, actualH)); // Use default tolerance
+      EXPECT(assert_equal(expectedH, actualH, 1e-6)); // 1e-6 needed to pass R4
     }
     else {
       // We accept that the numerical derivative of this specific Logmap implementation