From 22d66df7668532522fa93ea13aebc0627d654ee8 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 5 Jan 2010 14:13:51 +0000 Subject: [PATCH] added manifold test --- cpp/testPose2.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/cpp/testPose2.cpp b/cpp/testPose2.cpp index e5cfd0025..83689fcde 100644 --- a/cpp/testPose2.cpp +++ b/cpp/testPose2.cpp @@ -47,6 +47,19 @@ TEST(Pose2, constructors) { assert_equal(pose,origin); } +/* ************************************************************************* */ +TEST(Pose2, manifold) { + Pose2 t1(M_PI_2, Point2(1, 2)); + Pose2 t2(M_PI_2+0.018, Point2(1.015, 2.01)); + Pose2 origin; + Vector d12 = t1.log(t2); + CHECK(assert_equal(t2, t1.exmap(d12))); + CHECK(assert_equal(t2, origin.exmap(d12)*t1)); + Vector d21 = t2.log(t1); + CHECK(assert_equal(t1, t2.exmap(d21))); + CHECK(assert_equal(t1, origin.exmap(d21)*t2)); +} + /* ************************************************************************* */ TEST(Pose2, exmap) { Pose2 pose(M_PI_2, Point2(1, 2));