From 87797c687e0219b7e7ab66092b49821da9ee5109 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 10 May 2025 10:29:47 -0400
Subject: [PATCH] Fix bug in Debug

---
 gtsam/navigation/tests/testLieGroupEKF.cpp | 20 +++++++++-----------
 gtsam/navigation/tests/testManifoldEKF.cpp | 20 +++++++-------------
 2 files changed, 16 insertions(+), 24 deletions(-)

diff --git a/gtsam/navigation/tests/testLieGroupEKF.cpp b/gtsam/navigation/tests/testLieGroupEKF.cpp
index f70511293..031ab743c 100644
--- a/gtsam/navigation/tests/testLieGroupEKF.cpp
+++ b/gtsam/navigation/tests/testLieGroupEKF.cpp
@@ -126,19 +126,17 @@ namespace exampleLieGroupDynamicMatrix {
     return kFixedVelocityTangent;
   }
 
-  // Measurement function h(X, H_p)
-  // H_p is D(h)/D(X_local)
-  double h(const Matrix& p, OptionalJacobian<-1, -1> H_p = {}) {
-    if (p.size() == 0) {
-      if (H_p) H_p->resize(1, 0); // Jacobian dh/dX_local is 1x0
-      return 0.0; // Define trace of empty matrix as 0 for consistency
+  // Measurement function h(X, H)
+  double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
+    // Specialized for a 2x2 matrix!
+    if (p.rows() != 2 || p.cols() != 2) {
+      throw std::invalid_argument("Matrix must be 2x2.");
     }
-    // This test specifically uses 2x2 matrices (size 4)
-    if (H_p) {
-      H_p->resize(1, p.size()); // p.size() is 4
-      (*H_p) << 1.0, 0.0, 0.0, 1.0; // d(trace)/d(p_flat) for p_flat = [p00,p10,p01,p11]
+    if (H) {
+      H->resize(1, p.size());
+      *H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11
     }
-    return p(0, 0) + p(1, 1); // trace
+    return p(0, 0) + p(1, 1); // Trace of the matrix
   }
 } // namespace exampleLieGroupDynamicMatrix
 
diff --git a/gtsam/navigation/tests/testManifoldEKF.cpp b/gtsam/navigation/tests/testManifoldEKF.cpp
index ff7a9a68d..50edbe049 100644
--- a/gtsam/navigation/tests/testManifoldEKF.cpp
+++ b/gtsam/navigation/tests/testManifoldEKF.cpp
@@ -151,27 +151,21 @@ TEST(ManifoldEKF_Unit3, Update) {
 namespace exampleDynamicMatrix {
 
   // Predicts the next state given current state (Matrix), tangent "velocity" (Vector), and dt.
-  // For Matrix (a VectorSpace type), Retract(M, v) is typically M + v.
   Matrix f(const Matrix& p, const Vector& vTangent, double dt) {
-    return traits<Matrix>::Retract(p, vTangent * dt);
+    return traits<Matrix>::Retract(p, vTangent * dt); // +
   }
 
   // Define a measurement model: measure the trace of the Matrix (assumed 2x2 here)
-  // H is the Jacobian dh/d(flatten(p))
   double h(const Matrix& p, OptionalJacobian<-1, -1> H = {}) {
+    // Specialized for a 2x2 matrix!
+    if (p.rows() != 2 || p.cols() != 2) {
+      throw std::invalid_argument("Matrix must be 2x2.");
+    }
     if (H) {
-      // The Jacobian H = d(trace)/d(flatten(p)) will be 1xN, where N is p.size().
-      // For a 2x2 matrix, N=4, so H is 1x4: [1, 0, 0, 1].
       H->resize(1, p.size());
-      (*H)(0, 0) = 1.0; // d(trace)/dp00
-      (*H)(0, p.rows() * (p.cols() - 1) + (p.rows() - 1)) = 1.0; // d(trace)/dp11 (for col-major)
+      *H << 1.0, 0.0, 0.0, 1.0; // d(trace)/dp00, d(trace)/dp01, d(trace)/dp10, d(trace)/dp11
     }
-    if (p.rows() < 1 || p.cols() < 1) return 0.0; // Or throw error
-    double trace = 0.0;
-    for (DenseIndex i = 0; i < std::min(p.rows(), p.cols()); ++i) {
-      trace += p(i, i);
-    }
-    return trace;
+    return p(0, 0) + p(1, 1); // Trace of the matrix
   }
 
 } // namespace exampleDynamicMatrix