From 6b1c411f9e485716e7abe0b0204c5e7852f8dce1 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Sun, 12 Jul 2015 14:37:40 -0400 Subject: [PATCH] feature: add two small unit tests for local coordinate --- gtsam/geometry/tests/testUnit3.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a4227acdf..e55caaa3c 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -192,6 +192,16 @@ TEST(Unit3, localCoordinates) { EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(q, p.retract(expected), 1e-8)); } + { + Unit3 p(0,1,0), q(0,-1,0); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + } + { + Unit3 p(0,0,1), q(0,0,-1); + Vector2 actual = p.localCoordinates(q); + EXPECT(assert_equal(q, p.retract(actual), 1e-8)); + } double twist = 1e-4; {