From f1f21a85891c6574a764c30c81c5accde0632762 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 10 Jan 2010 13:53:31 +0000
Subject: [PATCH] inverted sign of square root to make inv sqrt positive for
 simple covariance matrices

---
 cpp/Matrix.cpp     |  3 ++-
 cpp/testMatrix.cpp | 11 ++++++-----
 2 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index 50e0274c2..65bed6976 100644
--- a/cpp/Matrix.cpp
+++ b/cpp/Matrix.cpp
@@ -618,7 +618,8 @@ Matrix inverse_square_root(const Matrix& A) {
 	svd(A,U,S,V);
 
 	// invert and sqrt diagonal of S
-  for(size_t i = 0; i<m; i++) S(i) = pow(S(i),-0.5);
+	// We also arbitrarily choose sign to make result have positive signs
+  for(size_t i = 0; i<m; i++) S(i) = - pow(S(i),-0.5);
   return vector_scale(S, V); // V*S;
 }
 
diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp
index df8d8c1bc..29a308ab2 100644
--- a/cpp/testMatrix.cpp
+++ b/cpp/testMatrix.cpp
@@ -593,13 +593,14 @@ TEST( matrix, inverse_square_root )
 			);
 	Matrix actual = inverse_square_root(measurement_covariance);
 
-	Matrix square_root_inverse_covariance = Matrix_(3,3,
-			-2.0, 0.0, 0.0,
-			0.0, -2.0, 0.0,
-			0.0, 0.0, -10.0
+	Matrix expected = Matrix_(3,3,
+			2.0, 0.0, 0.0,
+			0.0, 2.0, 0.0,
+			0.0, 0.0, 10.0
 			);
 
-	EQUALITY(square_root_inverse_covariance,actual);
+	EQUALITY(expected,actual);
+	EQUALITY(measurement_covariance,inverse(actual*actual));
 }
 
 /* ************************************************************************* */