From fcbceaf746ff2a5d70e3683f6894afb5a3903958 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Sun, 12 Jul 2015 00:12:06 -0700
Subject: [PATCH] More tests

---
 gtsam/geometry/tests/testUnit3.cpp | 27 +++++++++++++++++++++++----
 1 file changed, 23 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp
index b2f387d67..3f5f32e1d 100644
--- a/gtsam/geometry/tests/testUnit3.cpp
+++ b/gtsam/geometry/tests/testUnit3.cpp
@@ -158,20 +158,39 @@ TEST(Unit3, distance) {
 
 TEST(Unit3, localCoordinates) {
   {
-    Unit3 p;
-    Vector2 actual = p.localCoordinates(p);
+    Unit3 p, q;
+    Vector2 expected = Vector2::Zero();
+    Vector2 actual = p.localCoordinates(q);
     EXPECT(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
   }
   {
     Unit3 p, q(1, 6.12385e-21, 0);
+    Vector2 expected = Vector2::Zero();
     Vector2 actual = p.localCoordinates(q);
-    CHECK(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal(zero(2), actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
   }
   {
     Unit3 p, q(-1, 0, 0);
     Vector2 expected(M_PI, 0);
     Vector2 actual = p.localCoordinates(q);
-    CHECK(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+  }
+  {
+    Unit3 p, q(0, 1, 0);
+    Vector2 expected(0,-M_PI_2);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
+  }
+  {
+    Unit3 p, q(0, -1, 0);
+    Vector2 expected(0, M_PI_2);
+    Vector2 actual = p.localCoordinates(q);
+    EXPECT(assert_equal(expected, actual, 1e-8));
+    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
   }
 
   double twist = 1e-4;