From 64cb67ba3c8cde8ad695fd9a0d99b449b50d8b7a Mon Sep 17 00:00:00 2001
From: Dan McGann <dmcgann@andrew.cmu.edu>
Date: Mon, 27 Mar 2023 17:28:45 -0400
Subject: [PATCH] Adds jacobians for Pose2 component extraction

Adds jacobians to translation() and rotation() for Pose2 to bring it
into spec with Pose3's equilivent functions. Also adds tests.
---
 gtsam/geometry/Pose2.cpp           | 15 +++++++++++++++
 gtsam/geometry/Pose2.h             |  8 ++++----
 gtsam/geometry/tests/testPose2.cpp | 27 +++++++++++++++++++++++++++
 3 files changed, 46 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp
index b37674b92..9903a529f 100644
--- a/gtsam/geometry/Pose2.cpp
+++ b/gtsam/geometry/Pose2.cpp
@@ -32,6 +32,21 @@ GTSAM_CONCEPT_POSE_INST(Pose2)
 
 static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 
+/* ************************************************************************* */
+const Point2 &Pose2::translation(OptionalJacobian<2, 3> Hself) const {
+  if (Hself) {
+    *Hself = Matrix::Zero(2, 3);
+    (*Hself).block<2, 2>(0, 0) = rotation().matrix();
+  }
+  return t_;
+}
+
+/* ************************************************************************* */
+const Rot2& Pose2::rotation(OptionalJacobian<1, 3> Hself) const {
+  if (Hself) *Hself << 0, 0, 1;
+  return r_;
+}
+
 /* ************************************************************************* */
 Matrix3 Pose2::matrix() const {
   Matrix2 R = r_.matrix();
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 0ecd95e1c..bf3081339 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -249,16 +249,16 @@ public:
   inline double theta() const { return r_.theta(); }
 
   /// translation
-  inline const Point2& t() const { return t_; }
+  inline const Point2& t() const { return translation(); }
 
   /// rotation
-  inline const Rot2&   r() const { return r_; }
+  inline const Rot2&   r() const { return rotation(); }
 
   /// translation
-  inline const Point2& translation() const { return t_; }
+  const Point2& translation(OptionalJacobian<2, 3> Hself={}) const;
 
   /// rotation
-  inline const Rot2&   rotation() const { return r_; }
+  const Rot2&   rotation(OptionalJacobian<1, 3> Hself={}) const;
 
   //// return transformation matrix
   GTSAM_EXPORT Matrix3 matrix() const;
diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp
index 44dc55a81..0a27f127e 100644
--- a/gtsam/geometry/tests/testPose2.cpp
+++ b/gtsam/geometry/tests/testPose2.cpp
@@ -474,6 +474,33 @@ TEST( Pose2, compose_matrix )
   EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
 }
 
+
+
+/* ************************************************************************* */
+TEST( Pose2, translation )  {
+  Pose2 pose(3.5, -8.2, 4.2);
+
+  Matrix actualH;
+  EXPECT(assert_equal((Vector2() << 3.5, -8.2).finished(), pose.translation(actualH), 1e-8));
+
+  std::function<Point2(const Pose2&)> f = [](const Pose2& T) { return T.translation(); };
+  Matrix numericalH = numericalDerivative11<Point2, Pose2>(f, pose);
+  EXPECT(assert_equal(numericalH, actualH, 1e-6));
+}
+
+/* ************************************************************************* */
+TEST( Pose2, rotation ) {
+  Pose2 pose(3.5, -8.2, 4.2);
+
+  Matrix actualH(4, 3);
+  EXPECT(assert_equal(Rot2(4.2), pose.rotation(actualH), 1e-8));
+
+  std::function<Rot2(const Pose2&)> f = [](const Pose2& T) { return T.rotation(); };
+  Matrix numericalH = numericalDerivative11<Rot2, Pose2>(f, pose);
+  EXPECT(assert_equal(numericalH, actualH, 1e-6));
+}
+
+
 /* ************************************************************************* */
 TEST( Pose2, between )
 {