From f049c45863cf12b360a0af145f583e60740aeeff Mon Sep 17 00:00:00 2001
From: cbeall3 <cbeall3@gatech.edu>
Date: Tue, 30 Sep 2014 15:27:19 -0400
Subject: [PATCH] Handle SVD sign ambiguities. Fixes issue #112

---
 gtsam/base/tests/testMatrix.cpp              | 24 ++++++++++++++++++++
 gtsam/geometry/tests/testEssentialMatrix.cpp | 12 ++++++++++
 2 files changed, 36 insertions(+)

diff --git a/gtsam/base/tests/testMatrix.cpp b/gtsam/base/tests/testMatrix.cpp
index 4e857b143..9f1851308 100644
--- a/gtsam/base/tests/testMatrix.cpp
+++ b/gtsam/base/tests/testMatrix.cpp
@@ -1127,6 +1127,12 @@ TEST( matrix, svd2 )
 
   svd(sampleA, U, s, V);
 
+  // take care of sign ambiguity
+  if (U(0, 1) > 0) {
+    U = -U;
+    V = -V;
+  }
+
   EXPECT(assert_equal(expectedU,U));
   EXPECT(assert_equal(expected_s,s,1e-9));
   EXPECT(assert_equal(expectedV,V));
@@ -1143,6 +1149,13 @@ TEST( matrix, svd3 )
   Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.);
 
   svd(sampleAt, U, s, V);
+
+  // take care of sign ambiguity
+  if (U(0, 0) > 0) {
+    U = -U;
+    V = -V;
+  }
+
   Matrix S = diag(s);
   Matrix t = U * S;
   Matrix Vt = trans(V);
@@ -1176,6 +1189,17 @@ TEST( matrix, svd4 )
      0.6723,   0.7403);
 
   svd(A, U, s, V);
+
+  // take care of sign ambiguity
+  if (U(0, 0) < 0) {
+    U.col(0) = -U.col(0);
+    V.col(0) = -V.col(0);
+  }
+  if (U(0, 1) < 0) {
+    U.col(1) = -U.col(1);
+    V.col(1) = -V.col(1);
+  }
+
   Matrix reconstructed = U * diag(s) * trans(V);
 
   EXPECT(assert_equal(A, reconstructed, 1e-4));
diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp
index 318c3db7c..9d6f798cd 100644
--- a/gtsam/geometry/tests/testEssentialMatrix.cpp
+++ b/gtsam/geometry/tests/testEssentialMatrix.cpp
@@ -173,6 +173,12 @@ TEST (EssentialMatrix, epipoles) {
   Vector S;
   gtsam::svd(E.matrix(), U, S, V);
 
+  // take care of SVD sign ambiguity
+  if (U(0, 2) > 0) {
+    U = -U;
+    V = -V;
+  }
+
   // check rank 2 constraint
   CHECK(fabs(S(2))<1e-10);
 
@@ -186,6 +192,12 @@ TEST (EssentialMatrix, epipoles) {
   Unit3 actual = E.epipole_a();
   EXPECT(assert_equal(e1, actual));
 
+  // take care of SVD sign ambiguity
+  if (V(0, 2) < 0) {
+    U = -U;
+    V = -V;
+  }
+
   // Epipole in image 2 is E.rotation().unrotate(E.direction())
   Unit3 e2(V(0, 2), V(1, 2), V(2, 2));
   EXPECT(assert_equal(e2, E.epipole_b()));