From fb0ca07bf164b3981f7ec22d3b3adfe274ce6218 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Tue, 19 Jan 2010 16:52:01 +0000
Subject: [PATCH] Added and tested whitening parameter for matrix() and
 matrix_augmented()

---
 cpp/GaussianFactor.cpp     |  8 ++++--
 cpp/GaussianFactor.h       |  4 +--
 cpp/testGaussianFactor.cpp | 51 ++++++++++++++++++++++++++++++++------
 3 files changed, 52 insertions(+), 11 deletions(-)

diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp
index d71a511e1..e15112d02 100644
--- a/cpp/GaussianFactor.cpp
+++ b/cpp/GaussianFactor.cpp
@@ -215,7 +215,7 @@ pair<Matrix,Vector> GaussianFactor::matrix(const Ordering& ordering, bool weight
 }
 
 /* ************************************************************************* */
-Matrix GaussianFactor::matrix_augmented(const Ordering& ordering) const {
+Matrix GaussianFactor::matrix_augmented(const Ordering& ordering, bool weight) const {
 	// get pointers to the matrices
 	vector<const Matrix *> matrices;
 	BOOST_FOREACH(const Symbol& j, ordering) {
@@ -229,7 +229,11 @@ Matrix GaussianFactor::matrix_augmented(const Ordering& ordering) const {
 		B_mat(i,0) = b_(i);
 	matrices.push_back(&B_mat);
 
-	return collect(matrices);
+	// divide in sigma so error is indeed 0.5*|Ax-b|
+	Matrix Ab = collect(matrices);
+	if (weight) model_->WhitenInPlace(Ab);
+
+	return Ab;
 }
 
 /* ************************************************************************* */
diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h
index 8324f9f97..6cd77b1d2 100644
--- a/cpp/GaussianFactor.h
+++ b/cpp/GaussianFactor.h
@@ -233,10 +233,10 @@ public:
 	/**
 	 * Return (dense) matrix associated with factor
 	 * The returned system is an augmented matrix: [A b]
-	 * The standard deviations are NOT baked into A and b
 	 * @param ordering of variables needed for matrix column order
+	 * @param set weight to use whitening to bake in weights
 	 */
-	Matrix matrix_augmented(const Ordering& ordering) const;
+	Matrix matrix_augmented(const Ordering& ordering, bool weight = true) const;
 
 	/**
 	 * Return vectors i, j, and s to generate an m-by-n sparse matrix
diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp
index acded2ba7..666a53dda 100644
--- a/cpp/testGaussianFactor.cpp
+++ b/cpp/testGaussianFactor.cpp
@@ -25,6 +25,7 @@ using namespace boost::assign;
 using namespace std;
 using namespace gtsam;
 using namespace example;
+using namespace boost;
 
 /* ************************************************************************* */
 TEST( GaussianFactor, linearFactor )
@@ -510,16 +511,36 @@ TEST( GaussianFactor, matrix )
 	Ordering ord;
 	ord += "x1","x2";
 
-	Matrix A; Vector b;
-	boost::tie(A,b) = lf->matrix(ord);
+	// Test whitened version
+	Matrix A_act1; Vector b_act1;
+	boost::tie(A_act1,b_act1) = lf->matrix(ord, true);
 
 	Matrix A1 = Matrix_(2,4,
 			-10.0,  0.0, 10.0,  0.0,
 			000.0,-10.0,  0.0, 10.0 );
 	Vector b1 = Vector_(2, 2.0, -1.0);
 
-	EQUALITY(A,A1);
-	EQUALITY(b,b1);
+	EQUALITY(A_act1,A1);
+	EQUALITY(b_act1,b1);
+
+	// Test unwhitened version
+	Matrix A_act2; Vector b_act2;
+	boost::tie(A_act2,b_act2) = lf->matrix(ord, false);
+
+
+	Matrix A2 = Matrix_(2,4,
+			-1.0,  0.0, 1.0,  0.0,
+			000.0,-1.0,  0.0, 1.0 );
+	Vector b2 = Vector_(2, 0.2, -0.1);
+
+	EQUALITY(A_act2,A2);
+	EQUALITY(b_act2,b2);
+
+	// Ensure that whitening is consistent
+	shared_ptr<noiseModel::Gaussian> model = lf->get_model();
+	model->WhitenSystem(A_act2, b_act2);
+	EQUALITY(A_act1, A_act2);
+	EQUALITY(b_act1, b_act2);
 }
 
 /* ************************************************************************* */
@@ -535,14 +556,30 @@ TEST( GaussianFactor, matrix_aug )
 	Ordering ord;
 	ord += "x1","x2";
 
-	Matrix Ab;
-	Ab = lf->matrix_augmented(ord);
+	// Test unwhitened version
+	Matrix Ab_act1;
+	Ab_act1 = lf->matrix_augmented(ord, false);
 
 	Matrix Ab1 = Matrix_(2,5,
 			-1.0,  0.0, 1.0,  0.0,  0.2,
 			00.0,- 1.0, 0.0,  1.0, -0.1 );
 
-	EQUALITY(Ab,Ab1);
+	EQUALITY(Ab_act1,Ab1);
+
+	// Test whitened version
+	Matrix Ab_act2;
+	Ab_act2 = lf->matrix_augmented(ord, true);
+
+	Matrix Ab2 = Matrix_(2,5,
+		   -10.0,  0.0, 10.0,  0.0,  2.0,
+			00.0, -10.0,  0.0, 10.0, -1.0 );
+
+	EQUALITY(Ab_act2,Ab2);
+
+	// Ensure that whitening is consistent
+	shared_ptr<noiseModel::Gaussian> model = lf->get_model();
+	model->WhitenInPlace(Ab_act1);
+	EQUALITY(Ab_act1, Ab_act2);
 }
 
 /* ************************************************************************* */