From 89460fe93169be9a6a40cc2b32391a93b2327fa1 Mon Sep 17 00:00:00 2001
From: Paul Drews <drews.paul@gmail.com>
Date: Fri, 2 Jan 2015 09:02:43 -0500
Subject: [PATCH] Unit tests still fail, but believed to be correct.

---
 .../geometry/tests/testSimilarity3.cpp         | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
index 2f130b67f..7d3f3f553 100644
--- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp
+++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp
@@ -97,7 +97,8 @@ public:
     // Also, how do we actually get s out?  Seems like we need to store it somewhere.
     return Similarity3( //
         rotation().retract(v.head<3>()), // retract rotation using v[0,1,2]
-        translation().retract(v.segment<3>(3)), scale() + v[6]); // finally, update scale using v[6]
+        translation().retract(R() * v.segment<3>(3)), // Retract the translation
+        scale() + v[6]); //finally, update scale using v[6]
   }
 
   /// 7-dimensional vector v in tangent space that makes other = this->retract(v)
@@ -105,7 +106,8 @@ public:
 
     Vector7 v;
     v.head<3>() = rotation().localCoordinates(other.rotation());
-    v.segment<3>(3) = translation().localCoordinates(other.translation());
+    v.segment<3>(3) = rotation().unrotate(other.translation() - translation()).vector();
+    //v.segment<3>(3) = translation().localCoordinates(other.translation());
     v[6] = other.scale() - scale();
     return v;
   }
@@ -166,14 +168,20 @@ TEST(Similarity3, Manifold) {
 
   EXPECT(assert_equal(z, sim2.localCoordinates(sim)));
 
-  Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 1, 1), 1);
+  Similarity3 sim3 = Similarity3(Rot3(), Point3(1, 2, 3), 1);
   Vector v3(7);
-  v3 << 0, 0, 0, 1, 1, 1, 0;
+  v3 << 0, 0, 0, 1, 2, 3, 0;
   EXPECT(assert_equal(v3, sim2.localCoordinates(sim3)));
 
-  Similarity3 other = Similarity3(Rot3::ypr(1, 2, 3), Point3(4, 5, 6), 7);
+  Similarity3 other = Similarity3(Rot3::ypr(0.01, 0.02, 0.03), Point3(0.4, 0.5, 0.6), 1);
 //  Similarity3 other = Similarity3(Rot3(),Point3(4,5,6),1);
 
+  std::cout << "Local Coords: " << sim.localCoordinates(other) << std::endl;
+  std::cout << "Retracted: \n"
+      << sim.retract(sim.localCoordinates(other)).rotation().matrix() << std::endl
+      << sim.retract(sim.localCoordinates(other)).translation().vector() << std::endl
+      << sim.retract(sim.localCoordinates(other)).scale() << std::endl;
+
   EXPECT(sim.retract(sim.localCoordinates(other)) == other);
 
   // TODO add unit tests for retract and localCoordinates