From 7ff7482792b9586682269a486a0612c380ffa86e Mon Sep 17 00:00:00 2001
From: jing <jdong@gatech.edu>
Date: Mon, 10 Mar 2014 17:09:56 -0400
Subject: [PATCH] all unit test failure fixed

---
 gtsam/geometry/Cal3Unify.cpp           |  5 ++---
 gtsam/geometry/tests/testCal3Unify.cpp | 23 +++++++++++------------
 2 files changed, 13 insertions(+), 15 deletions(-)

diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp
index 5f2e330bd..8e200ff2a 100644
--- a/gtsam/geometry/Cal3Unify.cpp
+++ b/gtsam/geometry/Cal3Unify.cpp
@@ -152,12 +152,11 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const {
   Point2 pn = nPlaneToSpace(invKPi);
 
   // iterate until the uncalibrate is close to the actual pixel coordinate
-  const int maxIterations = 100;
+  const int maxIterations = 20;
   int iteration;
   for ( iteration = 0; iteration < maxIterations; ++iteration ) {
 
-    pn.print();
-    if ( pn.distance(pi) <= tol ) break;
+    if ( uncalibrate(pn).distance(pi) <= tol ) break;
 
     // part1: 3D space -> normalized plane
     Point2 pnpl = spaceToNPlane(pn);
diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp
index 3995e03b2..370877ef3 100644
--- a/gtsam/geometry/tests/testCal3Unify.cpp
+++ b/gtsam/geometry/tests/testCal3Unify.cpp
@@ -19,28 +19,26 @@
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/Cal3Unify.h>
 
-
-#include <iostream>
 using namespace gtsam;
-using namespace std;
+
 GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify)
 GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify)
 
 /*
 ground truth from matlab, code :
-X = [2 3 1]';
-V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
+X = [0.5 0.7 1]';
+V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240];
 [P, J] = spaceToImgPlane(X, V);
 matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox
 */
 
-static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
-static Point2 p(2, 3);
+static Cal3Unify K(0.1, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
+static Point2 p(0.5, 0.7);
 
 /* ************************************************************************* */
 TEST( Cal3Unify, uncalibrate)
 {
-  Point2 p_i(582.5228344366194, 649.6685266099726) ;
+  Point2 p_i(364.7791831734982, 305.6677211952602) ;
   Point2 q = K.uncalibrate(p);
   CHECK(assert_equal(q,p_i));
 }
@@ -49,6 +47,7 @@ TEST( Cal3Unify, uncalibrate)
 TEST( Cal3Unify, spaceNplane)
 {
   Point2 q = K.spaceToNPlane(p);
+  CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q));
   CHECK(assert_equal(p, K.nPlaneToSpace(q)));
 }
 
@@ -56,7 +55,7 @@ TEST( Cal3Unify, spaceNplane)
 TEST( Cal3Unify, calibrate)
 {
   Point2 pi = K.uncalibrate(p);
-  Point2 pn_hat = K.calibrate(pi, 1);
+  Point2 pn_hat = K.calibrate(pi);
   CHECK( p.equals(pn_hat, 1e-5));
 }
 
@@ -68,7 +67,7 @@ TEST( Cal3Unify, Duncalibrate1)
   Matrix computed;
   K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical,computed,1e-6));
+  CHECK(assert_equal(numerical,computed,1e-5));
 }
 
 /* ************************************************************************* */
@@ -77,7 +76,7 @@ TEST( Cal3Unify, Duncalibrate2)
   Matrix computed;
   K.uncalibrate(p, boost::none, computed);
   Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
-  CHECK(assert_equal(numerical,computed,1e-6));
+  CHECK(assert_equal(numerical,computed,1e-5));
 }
 
 /* ************************************************************************* */
@@ -89,7 +88,7 @@ TEST( Cal3Unify, assert_equal)
 /* ************************************************************************* */
 TEST( Cal3Unify, retract)
 {
-  Cal3Unify expected(0.01 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
+  Cal3Unify expected(0.1 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6,
       1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10);
   Vector d(10);
   d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;