From 387dfe52298b012c7f4af49f11d8dc218bb21d8d Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 24 May 2015 15:33:15 -0700
Subject: [PATCH] Some more tests on retract/localCoordinates

---
 gtsam/geometry/tests/testEssentialMatrix.cpp | 23 +++++++++++++++++---
 1 file changed, 20 insertions(+), 3 deletions(-)

diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp
index d2990a747..fe27b2911 100644
--- a/gtsam/geometry/tests/testEssentialMatrix.cpp
+++ b/gtsam/geometry/tests/testEssentialMatrix.cpp
@@ -30,6 +30,14 @@ TEST (EssentialMatrix, equality) {
   EXPECT(assert_equal(expected, actual));
 }
 
+//*************************************************************************
+TEST (EssentialMatrix, FromPose3) {
+  EssentialMatrix expected(c1Rc2, Unit3(c1Tc2));
+  Pose3 pose(c1Rc2, c1Tc2);
+  EssentialMatrix actual = EssentialMatrix::FromPose3(pose);
+  EXPECT(assert_equal(expected, actual));
+}
+
 //*******************************************************************************
 TEST(EssentialMatrix, localCoordinates0) {
   EssentialMatrix E;
@@ -47,11 +55,11 @@ TEST (EssentialMatrix, localCoordinates) {
   Vector actual = hx.localCoordinates(EssentialMatrix::FromPose3(pose));
   EXPECT(assert_equal(zero(5), actual, 1e-8));
 
-  Vector d = zero(6);
-  d(4) += 1e-5;
+  Vector6 d;
+  d << 0.1, 0.2, 0.3, 0, 0, 0;
   Vector actual2 = hx.localCoordinates(
       EssentialMatrix::FromPose3(pose.retract(d)));
-  EXPECT(assert_equal(zero(5), actual2, 1e-8));
+  EXPECT(assert_equal(d.head(5), actual2, 1e-8));
 }
 
 //*************************************************************************
@@ -75,6 +83,15 @@ TEST (EssentialMatrix, retract2) {
   EXPECT(assert_equal(expected, actual));
 }
 
+//*************************************************************************
+TEST (EssentialMatrix, RoundTrip) {
+  Vector5 d;
+  d << 0.1, 0.2, 0.3, 0.4, 0.5;
+  EssentialMatrix e, hx = e.retract(d);
+  Vector actual = e.localCoordinates(hx);
+  EXPECT(assert_equal(d, actual, 1e-8));
+}
+
 //*************************************************************************
 Point3 transform_to_(const EssentialMatrix& E, const Point3& point) {
   return E.transform_to(point);