From a66f08a5e0974bfaba4db7a2f7db9d7fab521b04 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 11 Dec 2009 18:03:43 +0000
Subject: [PATCH] testing new alphaFactor

---
 cpp/GaussianFactor.cpp     | 27 ++++++++++++++++++++-------
 cpp/GaussianFactor.h       |  1 +
 cpp/testGaussianFactor.cpp | 20 ++++++++++++++++++++
 3 files changed, 41 insertions(+), 7 deletions(-)

diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp
index 9cbaf20df..e1cb81933 100644
--- a/cpp/GaussianFactor.cpp
+++ b/cpp/GaussianFactor.cpp
@@ -126,13 +126,20 @@ bool GaussianFactor::equals(const Factor<VectorConfig>& f, double tol) const {
 }
 
 /* ************************************************************************* */
-Vector GaussianFactor::error_vector(const VectorConfig& c) const {
+Vector GaussianFactor::unweighted_error(const VectorConfig& c) const {
   Vector e = -b_;
   if (empty()) return e;
   string j; Matrix Aj;
   FOREACH_PAIR(j, Aj, As_)
     e += Vector(Aj * c[j]);
-  return ediv(e,sigmas_);
+  return e;
+}
+
+/* ************************************************************************* */
+Vector GaussianFactor::error_vector(const VectorConfig& c) const {
+  Vector e = -b_;
+  if (empty()) return e;
+  return ediv(unweighted_error(c),sigmas_);
 }
 
 /* ************************************************************************* */
@@ -364,7 +371,8 @@ GaussianFactor::eliminate(const string& key) const
 }
 
 /* ************************************************************************* */
-void GaussianFactor::addGradientContribution(const VectorConfig& x, VectorConfig& g) const {
+void GaussianFactor::addGradientContribution(const VectorConfig& x,
+		VectorConfig& g) const {
 	// calculate the value of the factor
 	Vector e = GaussianFactor::error_vector(x);
   Vector et = trans(e); // transpose
@@ -385,13 +393,18 @@ void GaussianFactor::addGradientContribution(const VectorConfig& x, VectorConfig
 /* ************************************************************************* */
 GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const VectorConfig& x,
 		const VectorConfig& d) const {
+
+	// Calculate A matrix
 	size_t m = b_.size();
-	Vector A = zero(m); Vector b = b_;
+	Vector A = zero(m);
   string j; Matrix Aj;
-  FOREACH_PAIR(j, Aj, As_) {
+  FOREACH_PAIR(j, Aj, As_)
   	A += Aj * d[j];
-  	b -= Aj * x[j];
-  }
+
+  // calculate the value of the factor for RHS
+	Vector b = - unweighted_error(x);
+
+	// construct factor
 	shared_ptr factor(new GaussianFactor("alpha",Matrix_(A),b,sigmas_));
 	return factor;
 }
diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h
index 36df56396..c13edcc17 100644
--- a/cpp/GaussianFactor.h
+++ b/cpp/GaussianFactor.h
@@ -117,6 +117,7 @@ public:
 
 	// Implementing Factor virtual functions
 
+	Vector unweighted_error(const VectorConfig& c) const; /** (A*x-b) */
 	Vector error_vector(const VectorConfig& c) const; /** (A*x-b)/sigma */
 	double error(const VectorConfig& c) const; /**  0.5*(A*x-b)'*D*(A*x-b) */
 	std::size_t size() const { return As_.size();}
diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp
index 6478c4ac3..d89a1abb1 100644
--- a/cpp/testGaussianFactor.cpp
+++ b/cpp/testGaussianFactor.cpp
@@ -636,6 +636,26 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional )
 	CHECK(assert_equal(expectedLF,actualLF,1e-5));
 }
 
+/* ************************************************************************* */
+TEST( GaussianFactor, alphaFactor )
+{
+	GaussianFactorGraph fg = createGaussianFactorGraph();
+
+	// get alphafactor for first factor in fg at zero, in gradient direction
+  VectorConfig x = createZeroDelta();
+	VectorConfig d = fg.gradient(x);
+	GaussianFactor::shared_ptr factor = fg[0];
+	GaussianFactor::shared_ptr actual = factor->alphaFactor(x,d);
+
+	// calculate expected
+	Matrix A = Matrix_(2,1,30.0,5.0);
+	Vector b = Vector_(2,-0.1,-0.1);
+	Vector sigmas = Vector_(2,0.1,0.1);
+	GaussianFactor expected("alpha",A,b,sigmas);
+
+	CHECK(assert_equal(expected,*actual));
+}
+
 /* ************************************************************************* */
 TEST ( GaussianFactor, constraint_eliminate1 )
 {