diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 23af69529..8ec39a6ba 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -106,7 +106,13 @@ namespace gtsam { inline size_t dim() const { return dimension; } /** Compose - make a new rotation by adding angles */ - inline Rot2 compose(const Rot2& R1) const { return *this * R1;} + inline Rot2 compose(const Rot2& R1, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = eye(1); + if(H2) *H2 = eye(1); + return *this * R1; + } /** Expmap around identity - create a rotation from an angle */ static Rot2 Expmap(const Vector& v) { @@ -126,7 +132,13 @@ namespace gtsam { inline Vector logmap(const Rot2& p2) const { return Logmap(between(p2));} /** Between using the default implementation */ - inline Rot2 between(const Rot2& p2) const { return between_default(*this, p2); } + inline Rot2 between(const Rot2& p2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -eye(1); + if(H2) *H2 = eye(1); + return between_default(*this, p2); + } /** return 2*2 rotation matrix */ Matrix matrix() const; diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 6df2939d1..6ecccac2c 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -44,6 +44,23 @@ TEST( Rot2, compose) { CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.2)*Rot2::fromAngle(0.25))); CHECK(assert_equal(Rot2::fromAngle(0.45), Rot2::fromAngle(0.25)*Rot2::fromAngle(0.2))); + + Matrix H1, H2; + (void) Rot2::fromAngle(1.0).compose(Rot2::fromAngle(2.0), H1, H2); + EXPECT(assert_equal(eye(1), H1)); + EXPECT(assert_equal(eye(1), H2)); +} + +/* ************************************************************************* */ +TEST( Rot2, between) +{ + CHECK(assert_equal(Rot2::fromAngle(0.05), Rot2::fromAngle(0.2).between(Rot2::fromAngle(0.25)))); + CHECK(assert_equal(Rot2::fromAngle(-0.05), Rot2::fromAngle(0.25).between(Rot2::fromAngle(0.2)))); + + Matrix H1, H2; + (void) Rot2::fromAngle(1.0).between(Rot2::fromAngle(2.0), H1, H2); + EXPECT(assert_equal(-eye(1), H1)); + EXPECT(assert_equal(eye(1), H2)); } /* ************************************************************************* */