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

@@ -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();

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;

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