diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index acaad08d7..8ce2e49bf 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -147,6 +147,11 @@ namespace gtsam { return Point2(uL_, v_); } + /** convenient function to get a Point2 from the right image */ + inline Point2 right(){ + return Point2(uR_, v_); + } + private: /// @} diff --git a/gtsam/geometry/tests/testStereoPoint2.cpp b/gtsam/geometry/tests/testStereoPoint2.cpp index cdd0ce742..4faeaec42 100644 --- a/gtsam/geometry/tests/testStereoPoint2.cpp +++ b/gtsam/geometry/tests/testStereoPoint2.cpp @@ -11,26 +11,65 @@ /** * @file testStereoPoint2.cpp - * * @brief Tests for the StereoPoint2 class * * @date Nov 4, 2011 * @author Alex Cunningham */ +#include +#include +#include #include -#include -#include - +using namespace std; using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(StereoPoint2) GTSAM_CONCEPT_LIE_INST(StereoPoint2) -/* ************************************************************************* */ -TEST(testStereoPoint2, test) { +/* ************************************************************************* */ +TEST(StereoPoint2, constructor) { + StereoPoint2 p1(1, 2, 3), p2 = p1; + EXPECT(assert_equal(p1, p2)); +} + +/* ************************************************************************* */ +TEST(StereoPoint2, Lie) { + StereoPoint2 p1(1, 2, 3), p2(4, 5, 6); + Matrix H1, H2; + + EXPECT(assert_equal(StereoPoint2(5,7,9), p1.compose(p2))); + + EXPECT(assert_equal(StereoPoint2(3,3,3), p1.between(p2))); + + EXPECT(assert_equal(StereoPoint2(5,7,9), p1.retract(Vector_(3, 4.,5.,6.)))); + EXPECT(assert_equal(Vector_(3, 3.,3.,3.), p1.localCoordinates(p2))); +} + +/* ************************************************************************* */ +TEST( StereoPoint2, expmap) { + Vector d(3); + d(0) = 1; + d(1) = -1; + d(2) = -3; + StereoPoint2 a(4, 5, 6), b = a.retract(d), c(5, 4, 3); + EXPECT(assert_equal(b,c)); +} + +/* ************************************************************************* */ +TEST( StereoPoint2, arithmetic) { + EXPECT(assert_equal( StereoPoint2(5,6,7), StereoPoint2(4,5,6)+StereoPoint2(1,1,1))); + EXPECT(assert_equal( StereoPoint2(3,4,5), StereoPoint2(4,5,6)-StereoPoint2(1,1,1))); +} + +/* ************************************************************************* */ +TEST(testStereoPoint2, left_right) { + StereoPoint2 p1(1,2,3); + + EXPECT(assert_equal( Point2(1,3), p1.point2())); + EXPECT(assert_equal( Point2(2,3), p1.right())); } /* ************************************************************************* */