From ead1271f3306b6f253858b90fd82f6432698296f Mon Sep 17 00:00:00 2001
From: Dan McGann <dmcgann@andrew.cmu.edu>
Date: Tue, 4 Mar 2025 15:30:13 -0500
Subject: [PATCH] Bugfix Bearing/Range Pose3toPose3 Jacobians

Previously the jacobians for bearing between two Pose3 objects were
computed incorrectly as it neglected the jacobian of a pose's
translation wrt its tangent vector. The jacobian for range betwen two
Pose3 objects was computed correctly but in a non-canonical way. This
commit fixes the bearing jacobian, and provides a canonical
implementation for both Pose3-Pose3 bearing and range jacobians.
---
 gtsam/geometry/Pose3.cpp           | 19 +++++++++++--------
 gtsam/geometry/tests/testPose3.cpp | 10 ----------
 2 files changed, 11 insertions(+), 18 deletions(-)

diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
index 337037676..a92bb84ef 100644
--- a/gtsam/geometry/Pose3.cpp
+++ b/gtsam/geometry/Pose3.cpp
@@ -442,9 +442,11 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself,
 /* ************************************************************************* */
 double Pose3::range(const Pose3& pose, OptionalJacobian<1, 6> Hself,
                     OptionalJacobian<1, 6> Hpose) const {
+  Matrix36 D_point_pose;
   Matrix13 D_local_point;
-  double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0);
-  if (Hpose) *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix();
+  Point3 point = pose.translation(Hpose ? &D_point_pose : 0);
+  double r = range(point, Hself, Hpose ? &D_local_point : 0);
+  if (Hpose) *Hpose = D_local_point * D_point_pose;
   return r;
 }
 
@@ -467,12 +469,13 @@ Unit3 Pose3::bearing(const Point3& point, OptionalJacobian<2, 6> Hself,
 
 /* ************************************************************************* */
 Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
-                     OptionalJacobian<2, 6> Hpose) const {
-  if (Hpose) {
-    Hpose->setZero();
-    return bearing(pose.translation(), Hself, Hpose.cols<3>(3));
-  }
-  return bearing(pose.translation(), Hself, {});
+  OptionalJacobian<2, 6> Hpose) const {
+  Matrix36 D_point_pose;
+  Matrix23 D_local_point;
+  Point3 point = pose.translation(Hpose ? &D_point_pose : 0);
+  Unit3 b = bearing(point, Hself, Hpose ? &D_local_point : 0);
+  if (Hpose) *Hpose = D_local_point * D_point_pose;
+  return b;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp
index b1df45004..0e83dc2b7 100644
--- a/gtsam/geometry/tests/testPose3.cpp
+++ b/gtsam/geometry/tests/testPose3.cpp
@@ -770,17 +770,7 @@ TEST(Pose3, PoseToPoseBearing) {
 
   // Check numerical derivatives
   expectedH1 = numericalDerivative21(bearing_proxy, xl1, l2);
-
-  // Since the second pose is treated as a point, the value calculated by
-  // numericalDerivative22 only depends on the position of the pose. Here, we
-  // calculate the Jacobian w.r.t. the second pose's position, and then augment
-  // that with zeroes in the block that is w.r.t. the second pose's
-  // orientation.
   H2block = numericalDerivative22(bearing_proxy, xl1, l2);
-  expectedH2 = Matrix(2, 6);
-  expectedH2.setZero();
-  expectedH2.block<2, 3>(0, 3) = H2block;
-
   EXPECT(assert_equal(expectedH1, actualH1, 1e-5));
   EXPECT(assert_equal(expectedH2, actualH2, 1e-5));
 }