Merge pull request #1504 from DanMcGann/pose2_component_jacobians

borglab · Jun 9, 2023 · 107f541 · 107f541
2 parents 13cb9d8 + a4e4e1f
commit 107f541
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 2 deletions.
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
@@ -258,10 +258,19 @@ class Pose2: public LieGroup<Pose2, 3> {
   inline const Rot2&   r() const { return r_; }
 
   /// translation
-  inline const Point2& translation() const { return t_; }
+  inline const Point2& translation(OptionalJacobian<2, 3> Hself={}) const {
+    if (Hself) {
+      *Hself = Matrix::Zero(2, 3);
+      (*Hself).block<2, 2>(0, 0) = rotation().matrix();
+    }
+    return t_;
+  }
 
   /// rotation
-  inline const Rot2&   rotation() const { return r_; }
+  inline const Rot2&   rotation(OptionalJacobian<1, 3> Hself={}) const {
+    if (Hself) *Hself << 0, 0, 1;
+    return r_;
+  }
 
   //// return transformation matrix
   GTSAM_EXPORT Matrix3 matrix() const;

diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i
@@ -431,7 +431,9 @@ class Pose2 {
   gtsam::Rot2 bearing(const gtsam::Point2& point) const;
   double range(const gtsam::Point2& point) const;
   gtsam::Point2 translation() const;
+  gtsam::Point2 translation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
   gtsam::Rot2 rotation() const;
+  gtsam::Rot2 rotation(Eigen::Ref<Eigen::MatrixXd> Hself) const;
   Matrix matrix() const;
 
   // enabling serialization functionality

diff --git 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 )
 {