diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 8ec39a6ba..ff159364f 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -171,6 +171,11 @@ namespace gtsam { Point2 unrotate(const Point2& p, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const; + /** + * Creates a unit vector as a Point2 + */ + inline Point2 unit() const { return Point2(c_, s_); } + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp index 6ecccac2c..3edc53e3a 100644 --- a/gtsam/geometry/tests/testRot2.cpp +++ b/gtsam/geometry/tests/testRot2.cpp @@ -33,6 +33,13 @@ TEST( Rot2, constructors_and_angle) CHECK(assert_equal(R,Rot2::atan2(s*5,c*5))); } +/* ************************************************************************* */ +TEST( Rot2, unit) +{ + EXPECT(assert_equal(Point2(1.0, 0.0), Rot2::fromAngle(0).unit())); + EXPECT(assert_equal(Point2(0.0, 1.0), Rot2::fromAngle(M_PI_2).unit())); +} + /* ************************************************************************* */ TEST( Rot2, transpose) {