From 16da0895a8863b1c5396250fed9dbc529ccd6f7a Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Tue, 8 Dec 2009 20:48:13 +0000
Subject: [PATCH] square_root_inverse

---
 cpp/Matrix.cpp | 17 +++++++++++++++++
 cpp/Matrix.h   |  3 +++
 2 files changed, 20 insertions(+)

diff --git a/cpp/Matrix.cpp b/cpp/Matrix.cpp
index 8704fba55..e6b2ddbb2 100644
--- a/cpp/Matrix.cpp
+++ b/cpp/Matrix.cpp
@@ -569,6 +569,23 @@ void svd(const Matrix& A, Matrix& U, Vector& s, Matrix& V) {
   svd(U,s,V); // call in-place version
 }
 
+/* ************************************************************************* */
+// TODO, would be faster with Cholesky
+Matrix inverse_square_root(const Matrix& A) {
+  size_t m = A.size2(), n = A.size1();
+	if (m!=n)
+		throw invalid_argument("inverse_square_root: A must be square");
+
+	// Perform SVD, TODO: symmetric SVD?
+	Matrix U,V;
+	Vector S;
+	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);
+  return vector_scale(S, V); // V*S;
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam
diff --git a/cpp/Matrix.h b/cpp/Matrix.h
index 8a882b56f..ac7030d46 100644
--- a/cpp/Matrix.h
+++ b/cpp/Matrix.h
@@ -238,6 +238,9 @@ void svd(const Matrix& A, Matrix& U, Vector& S, Matrix& V);
 // in-place version
 void svd(Matrix& A, Vector& S, Matrix& V);
 
+/** Use SVD to calculate inverse square root of a matrix */
+Matrix inverse_square_root(const Matrix& A);
+
 // macro for unit tests
 #define EQUALITY(expected,actual)\
   { if (!assert_equal(expected,actual)) \