From cd943d89ce7556a618863d3ee78494078b691ae2 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 11 Feb 2015 14:13:24 +0100
Subject: [PATCH] Narrowed issue to problem in Local

---
 gtsam/geometry/tests/testRot3Q.cpp | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp
index d7d43b33f..7c2f80bad 100644
--- a/gtsam/geometry/tests/testRot3Q.cpp
+++ b/gtsam/geometry/tests/testRot3Q.cpp
@@ -51,10 +51,15 @@ TEST(Rot3Q , Compare) {
   R R3 = TR::Compose(R1, R2, none, none);
   EXPECT(assert_equal(R(q3), R3));
 
-  // Check Retract/Local
+  // Check Retract
   Vector3 v(1e-5, 0, 0);
-  Vector3 vQ = TQ::Local(q3, TQ::Retract(q3, v));
-  Vector3 vR = TR::Local(R3, TR::Retract(R3, v));
+  Q q4 = TQ::Retract(q3, v);
+  R R4 = TR::Retract(R3, v);
+  EXPECT(assert_equal(R(q4), R4));
+
+  // Check Local
+  Vector3 vQ = TQ::Local(q3, q4);
+  Vector3 vR = TR::Local(R3, R4);
   EXPECT(assert_equal(vQ, vR));
 
   // Check Retract/Local of Compose
@@ -65,7 +70,6 @@ TEST(Rot3Q , Compare) {
   Vector3 vR2 = TR::Local(R3, TR::Compose(R1, TR::Retract(R2, -v)));
   EXPECT(assert_equal(vQ2, vR2));
   EXPECT(assert_equal<Vector3>((vQ1 - vQ2) / 0.2, (vR1 - vR2) / 0.2));
-  cout << (vR1 - vR2) / 0.2 << endl;
 
   // Check Compose Derivatives
   Matrix HQ, HR;