From a3fa194ca1c80d9b3fcd7511fefa9b57a686cc82 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 17 Jan 2010 03:56:42 +0000
Subject: [PATCH] Noisemodel works in PriorFactor

---
 cpp/PriorFactor.h      | 22 ++++++++++++++--------
 cpp/testPose2Prior.cpp | 10 +++-------
 2 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/cpp/PriorFactor.h b/cpp/PriorFactor.h
index 3eb381dc3..e968f2d71 100644
--- a/cpp/PriorFactor.h
+++ b/cpp/PriorFactor.h
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <ostream>
+#include "NoiseModel.h"
 #include "NonlinearFactor.h"
 #include "Pose2.h"
 
@@ -22,7 +23,7 @@ namespace gtsam {
 		typedef NonlinearFactor1<Config, Key, T> Base;
 
 		T prior_; /** The measurement */
-		Matrix square_root_inverse_covariance_; /** sqrt(inv(covariance)) */
+		boost::shared_ptr<GaussianNoiseModel> noiseModel_;
 
 	public:
 
@@ -30,9 +31,14 @@ namespace gtsam {
 		typedef typename boost::shared_ptr<PriorFactor> shared_ptr;
 
 		/** Constructor */
-		PriorFactor(const Key& key, const T& prior, const Matrix& covariance) :
-			Base(1.0, key), prior_(prior) {
-			square_root_inverse_covariance_ = inverse_square_root(covariance);
+		PriorFactor(const Key& key, const T& prior,
+				const boost::shared_ptr<GaussianNoiseModel>& model) :
+			Base(1.0, key), prior_(prior), noiseModel_(model) {
+		}
+
+		/** Constructor */
+		PriorFactor(const Key& key, const T& prior, const Matrix& cov) :
+			Base(1.0, key), prior_(prior), noiseModel_(new FullCovariance(cov)) {
 		}
 
 		/** implement functions needed for Testable */
@@ -41,8 +47,7 @@ namespace gtsam {
 		void print(const std::string& s) const {
 			Base::print(s);
 			prior_.print("prior");
-			gtsam::print(square_root_inverse_covariance_,
-					"Square Root Inverse Covariance");
+			// Todo print NoiseModel
 		}
 
 		/** equals */
@@ -51,15 +56,16 @@ namespace gtsam {
 					Config, Key, T>*> (&expected);
 			return e != NULL && Base::equals(expected) && this->prior_.equals(
 					e->prior_, tol);
+			// Todo check NoiseModel
 		}
 
 		/** implement functions needed to derive from Factor */
 
 		/** vector of errors */
 		Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
-			if (H) (*H) = square_root_inverse_covariance_;
+			if (H) (*H) = noiseModel_->R();
 			// manifold equivalent of h(x)-z -> log(z,h(x))
-			return square_root_inverse_covariance_ * logmap(prior_, p);
+			return noiseModel_->whiten(logmap(prior_, p));
 		}
 	};
 
diff --git a/cpp/testPose2Prior.cpp b/cpp/testPose2Prior.cpp
index 992c1c9da..6344b54e6 100644
--- a/cpp/testPose2Prior.cpp
+++ b/cpp/testPose2Prior.cpp
@@ -13,11 +13,7 @@ using namespace gtsam;
 
 // Common measurement covariance
 static double sx=0.5, sy=0.5,st=0.1;
-static Matrix covariance = Matrix_(3,3,
-		sx*sx, 0.0, 0.0,
-		0.0, sy*sy, 0.0,
-		0.0, 0.0, st*st
-		);
+static boost::shared_ptr<GaussianNoiseModel> model(new Sigmas(Vector_(3,sx,sy,st)));
 
 /* ************************************************************************* */
 // Very simple test establishing Ax-b \approx z-h(x)
@@ -29,7 +25,7 @@ TEST( Pose2Prior, error )
 	x0.insert(1, p1);
 
 	// Create factor
-	Pose2Prior factor(1, p1, covariance);
+	Pose2Prior factor(1, p1, model);
 
 	// Actual linearization
 	boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
@@ -52,7 +48,7 @@ TEST( Pose2Prior, error )
 /* ************************************************************************* */
 // common Pose2Prior for tests below
 static Pose2 prior(2,2,M_PI_2);
-static Pose2Prior factor(1,prior, covariance);
+static Pose2Prior factor(1,prior, model);
 
 /* ************************************************************************* */
 // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector