From 5e15564525c1367aad1187f1b9f353902c5f641b Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 18 Jan 2010 21:45:44 +0000
Subject: [PATCH] Speedups:  Matrix mult. in Gaussian NoiseModel.  Removed size
 check in operator* in Matrix.h - ublas does this check when in debug mode.

---
 cpp/Matrix.h       |  5 +++--
 cpp/NoiseModel.cpp | 30 ++++++++++++++++--------------
 2 files changed, 19 insertions(+), 16 deletions(-)

diff --git a/cpp/Matrix.h b/cpp/Matrix.h
index e79f7c93a..dcded7637 100644
--- a/cpp/Matrix.h
+++ b/cpp/Matrix.h
@@ -115,8 +115,9 @@ inline Vector operator*(const Vector & v, const Matrix& A) {
  * overload * for matrix multiplication (as BOOST does not)
  */
 inline Matrix operator*(const Matrix& A, const Matrix& B) {
-  if (A.size2()!=B.size1()) throw std::invalid_argument(
-  		boost::str(boost::format("Matrix operator* : A.n(%d)!=B.m(%d)") % A.size2() % B.size1()));
+  // richard: boost already does this check in debug mode I think
+//  if (A.size2()!=B.size1()) throw std::invalid_argument(
+//  		boost::str(boost::format("Matrix operator* : A.n(%d)!=B.m(%d)") % A.size2() % B.size1()));
   return prod(A,B);
 }
 
diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp
index 3733ee352..5fee1f3e6 100644
--- a/cpp/NoiseModel.cpp
+++ b/cpp/NoiseModel.cpp
@@ -47,24 +47,26 @@ namespace gtsam {
 
 		// functional
 		Matrix Gaussian::Whiten(const Matrix& H) const {
-			size_t m = H.size1(), n = H.size2();
-			Matrix W(m, n);
-			for (int j = 0; j < n; j++) {
-				Vector wj = whiten(column(H, j));
-				for (int i = 0; i < m; i++)
-					W(i, j) = wj(i);
-			}
-			return W;
+//			size_t m = H.size1(), n = H.size2();
+//			Matrix W(m, n);
+//			for (int j = 0; j < n; j++) {
+//				Vector wj = whiten(column(H, j));
+//				for (int i = 0; i < m; i++)
+//					W(i, j) = wj(i);
+//			}
+//			return W;
+		  return sqrt_information_ * H;
 		}
 
 		// in place
 		void Gaussian::WhitenInPlace(Matrix& H) const {
-			size_t m = H.size1(), n = H.size2();
-			for (int j = 0; j < n; j++) {
-				Vector wj = whiten(column(H, j));
-				for (int i = 0; i < m; i++)
-					H(i, j) = wj(i);
-			}
+//			size_t m = H.size1(), n = H.size2();
+//			for (int j = 0; j < n; j++) {
+//				Vector wj = whiten(column(H, j));
+//				for (int i = 0; i < m; i++)
+//					H(i, j) = wj(i);
+//			}
+		  H = sqrt_information_ * H;
 		}
 
 		/* ************************************************************************* */