From 9ddeb569b5772d43b7212458be2ccad27dec2835 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 17 Jan 2010 01:28:15 +0000
Subject: [PATCH] GaussianNoiseModel

---
 cpp/NoiseModel.cpp     | 12 ++++++++++--
 cpp/NoiseModel.h       | 37 ++++++++++++++++++++++++++++++++-----
 cpp/testNoiseModel.cpp | 14 ++++++++++++++
 3 files changed, 56 insertions(+), 7 deletions(-)

diff --git a/cpp/NoiseModel.cpp b/cpp/NoiseModel.cpp
index a6e28efe0..bcbe26866 100644
--- a/cpp/NoiseModel.cpp
+++ b/cpp/NoiseModel.cpp
@@ -6,12 +6,20 @@
  *      Author: Frank Dellaert
  */
 
-//#include <boost/numeric/ublas/traits.hpp>
-
 #include "NoiseModel.h"
 
+namespace ublas = boost::numeric::ublas;
+typedef ublas::matrix_column<Matrix> column;
+
 namespace gtsam {
 
+	Matrix GaussianNoiseModel::whiten(const Matrix& H) {
+	  size_t n = H.size2(), m = H.size1();
+		Matrix G(m,n);
+		for(int j=0;j<n;j++)
+			column(G, j) = NoiseModel::whiten(column(H, j));
+	}
+
 	Vector Isotropic::whiten(const Vector& v) const {
 		return v * invsigma_;
 	}
diff --git a/cpp/NoiseModel.h b/cpp/NoiseModel.h
index 8b84c0464..15a7d196b 100644
--- a/cpp/NoiseModel.h
+++ b/cpp/NoiseModel.h
@@ -14,8 +14,9 @@
 namespace gtsam {
 
   /**
-   * NoiseModel is the abstract base class for all noise models.  NoiseModels
-   * must implement a 'whiten' function to normalize an error vector, and an
+   * NoiseModel is the abstract base class for all noise models.
+   *
+   * It must implement a 'whiten' function to normalize an error vector, and an
    * 'unwhiten' function to unnormalize an error vector.
    */
   struct NoiseModel {
@@ -31,12 +32,38 @@ namespace gtsam {
     virtual Vector unwhiten(const Vector& v) const = 0;
   };
 
+  /**
+	 * GaussianNoiseModel implements the mathematical model
+	 *  |R*x|^2 = |y|^2 with R'*R=inv(Sigma)
+	 * where
+	 *   y = whiten(x) = R*x
+	 *   x = unwhiten(x) = inv(R)*y
+	 * as indeed
+	 *   |y|^2 = y'*y = x'*R'*R*x
+	 * Various simplified models are available that implement this efficiently.
+	 */
+	struct GaussianNoiseModel : public NoiseModel {
+
+	  /**
+	   * Multiply a derivative with R (derivative of whiten)
+	   * Equivalent to whitening each column of the input matrix.
+	   */
+	  Matrix whiten(const Matrix& H);
+
+	};
+
+	/**
+	 * We identify the Gaussian noise model with R
+	 */
+	// FD: does not work, ambiguous overload :-(
+  // inline Vector operator*(const GaussianNoiseModel& R, const Vector& v) {return R.whiten(v);}
+
   /**
    * An isotropic noise model corresponds to a scaled diagonal covariance
    * This class has no public constructors.  Instead, use either either the
    * Sigma or Variance class.
    */
-  class Isotropic : public NoiseModel {
+  class Isotropic : public GaussianNoiseModel {
   protected:
     double sigma_;
     double invsigma_;
@@ -73,7 +100,7 @@ namespace gtsam {
    * elements of the diagonal specified in a Vector.  This class has no public
    * constructors, instead, use either the Sigmas or Variances class.
    */
-  class Diagonal : public NoiseModel {
+  class Diagonal : public GaussianNoiseModel {
   protected:
     Vector sigmas_;
     Vector invsigmas_;
@@ -111,7 +138,7 @@ namespace gtsam {
   /**
    * A full covariance noise model.
    */
-  class FullCovariance : public NoiseModel {
+  class FullCovariance : public GaussianNoiseModel {
   protected:
     Matrix sqrt_covariance_;
     Matrix sqrt_inv_covariance_;
diff --git a/cpp/testNoiseModel.cpp b/cpp/testNoiseModel.cpp
index 9f40dca99..7d0447e8a 100644
--- a/cpp/testNoiseModel.cpp
+++ b/cpp/testNoiseModel.cpp
@@ -45,6 +45,20 @@ TEST(NoiseModel, constructors)
 	CHECK(assert_equal(unwhitened,m3.unwhiten(whitened)));
 	CHECK(assert_equal(unwhitened,m4.unwhiten(whitened)));
 	CHECK(assert_equal(unwhitened,m5.unwhiten(whitened)));
+
+	Matrix H(Matrix_(3, 4,
+			1.0, 0.0, 0.0, 1.0,
+			0.0, 1.0, 0.0, 1.0,
+			0.0, 0.0, 1.0, 1.0));
+
+	Matrix expected(Matrix_(3, 4,
+			1.0, 0.0, 0.0, 1.0,
+			0.0, 1.0, 0.0, 1.0,
+			0.0, 0.0, 1.0, 1.0));
+
+	// test operator*
+//	CHECK(assert_equal(expected,m2.whiten(H)));
+
 }
 
 /* ************************************************************************* */