diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index c3f588dcca..f1b38c5a6f 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -258,10 +258,19 @@ class Pose2: public LieGroup { 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 index 0710959bc9..32fb1ce4b2 100644 --- 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 Hself) const; gtsam::Rot2 rotation() const; + gtsam::Rot2 rotation(Eigen::Ref Hself) const; Matrix matrix() const; // enabling serialization functionality diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 8c1bdccc06..b52a6e590f 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 f = [](const Pose2& T) { return T.translation(); }; + Matrix numericalH = numericalDerivative11(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 f = [](const Pose2& T) { return T.rotation(); }; + Matrix numericalH = numericalDerivative11(f, pose); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + + /* ************************************************************************* */ TEST( Pose2, between ) {