From 06af482d617d1aa819cb7e7bf91d1504c784c275 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Tue, 21 Oct 2014 23:07:38 +0200
Subject: [PATCH] Added test for Rot3 - all is good

---
 .cproject              |  8 +++++
 tests/testManifold.cpp | 72 ++++++++++++++++++++++++++----------------
 2 files changed, 53 insertions(+), 27 deletions(-)

diff --git a/.cproject b/.cproject
index 700b82ce6..97cdc3bcb 100644
--- a/.cproject
+++ b/.cproject
@@ -2553,6 +2553,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testManifold.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testManifold.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
diff --git a/tests/testManifold.cpp b/tests/testManifold.cpp
index e43cde102..2c3b20434 100644
--- a/tests/testManifold.cpp
+++ b/tests/testManifold.cpp
@@ -69,29 +69,42 @@ TEST(Manifold, DefaultChart) {
   EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0));
   EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
 
+  Vector v2(2);
+  v2 << 1, 0;
   DefaultChart<Vector2> chart2(Vector2(0, 0));
-  EXPECT(chart2.apply(Vector2(1,0))==Vector2(1,0));
-  EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0));
+  EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0))));
+  EXPECT(chart2.retract(v2)==Vector2(1,0));
 
   DefaultChart<double> chart3(0);
-  Eigen::Matrix<double, 1, 1> v1;
+  Vector v1(1);
   v1 << 1;
-  EXPECT(chart3.apply(1)==v1);
-  EXPECT(chart3.retract(v1)==1);
+  EXPECT(assert_equal(v1,chart3.apply(1)));
+  EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9);
 
   // Dynamic does not work yet !
-//  Vector z = zero(2), v(2);
-//  v << 1, 0;
-//  DefaultChart<Vector> chart4(z);
-//  EXPECT(chart4.apply(v)==v);
-//  EXPECT(chart4.retract(v)==v);
+  Vector z = zero(2), v(2);
+  v << 1, 0;
+  DefaultChart<Vector> chart4(z);
+  EXPECT(assert_equal(chart4.apply(v),v));
+  EXPECT(assert_equal(chart4.retract(v),v));
+
+  Vector v3(3);
+  v3 << 1, 1, 1;
+  Rot3 I = Rot3::identity();
+  Rot3 R = I.retractCayley(v3);
+  DefaultChart<Rot3> chart5(I);
+  EXPECT(assert_equal(v3,chart5.apply(R)));
+  EXPECT(assert_equal(chart5.retract(v3),R));
+  // Check zero vector
+  DefaultChart<Rot3> chart6(R);
+  EXPECT(assert_equal(zero(3),chart6.apply(R)));
 }
 
 /* ************************************************************************* */
 // zero
 TEST(Manifold, _zero) {
   EXPECT(assert_equal(Pose3(),traits::zero<Pose3>::value()));
-  Cal3Bundler cal(0,0,0);
+  Cal3Bundler cal(0, 0, 0);
   EXPECT(assert_equal(cal,traits::zero<Cal3Bundler>::value()));
   EXPECT(assert_equal(Camera(Pose3(),cal),traits::zero<Camera>::value()));
 }
@@ -104,39 +117,44 @@ TEST(Manifold, Canonical) {
   EXPECT(chart1.apply(Point2(1,0))==Vector2(1,0));
   EXPECT(chart1.retract(Vector2(1,0))==Point2(1,0));
 
+  Vector v2(2);
+  v2 << 1, 0;
   Canonical<Vector2> chart2;
-  EXPECT(assert_equal((Vector)chart2.apply(Vector2(1,0)),Vector2(1,0)));
-  EXPECT(chart2.retract(Vector2(1,0))==Vector2(1,0));
+  EXPECT(assert_equal(v2,chart2.apply(Vector2(1,0))));
+  EXPECT(chart2.retract(v2)==Vector2(1,0));
 
   Canonical<double> chart3;
   Eigen::Matrix<double, 1, 1> v1;
   v1 << 1;
   EXPECT(chart3.apply(1)==v1);
-  EXPECT(chart3.retract(v1)==1);
+  EXPECT_DOUBLES_EQUAL(chart3.retract(v1), 1, 1e-9);
 
   Canonical<Point3> chart4;
-  Point3 point(1,2,3);
-  Vector3 v3(1,2,3);
-  EXPECT(assert_equal((Vector)chart4.apply(point),v3));
+  Point3 point(1, 2, 3);
+  Vector v3(3);
+  v3 << 1, 2, 3;
+  EXPECT(assert_equal(v3,chart4.apply(point)));
   EXPECT(assert_equal(chart4.retract(v3),point));
 
   Canonical<Pose3> chart5;
-  Pose3 pose(Rot3::identity(),point);
-  Vector6 v6; v6 << 0,0,0,1,2,3;
-  EXPECT(assert_equal((Vector)chart5.apply(pose),v6));
+  Pose3 pose(Rot3::identity(), point);
+  Vector v6(6);
+  v6 << 0, 0, 0, 1, 2, 3;
+  EXPECT(assert_equal(v6,chart5.apply(pose)));
   EXPECT(assert_equal(chart5.retract(v6),pose));
 
   Canonical<Camera> chart6;
-  Cal3Bundler cal0(0,0,0);
-  Camera camera(Pose3(),cal0);
-  Vector9 z9 = Vector9::Zero();
-  EXPECT(assert_equal((Vector)chart6.apply(camera),z9));
+  Cal3Bundler cal0(0, 0, 0);
+  Camera camera(Pose3(), cal0);
+  Vector z9 = Vector9::Zero();
+  EXPECT(assert_equal(z9,chart6.apply(camera)));
   EXPECT(assert_equal(chart6.retract(z9),camera));
 
   Cal3Bundler cal; // Note !! Cal3Bundler() != zero<Cal3Bundler>::value()
-  Camera camera2(pose,cal);
-  Vector9 v9; v9 << 0,0,0,1,2,3,1,0,0;
-  EXPECT(assert_equal((Vector)chart6.apply(camera2),v9));
+  Camera camera2(pose, cal);
+  Vector v9(9);
+  v9 << 0, 0, 0, 1, 2, 3, 1, 0, 0;
+  EXPECT(assert_equal(v9,chart6.apply(camera2)));
   EXPECT(assert_equal(chart6.retract(v9),camera2));
 }