From f91a1f0192708b18b728b0123c6bbe98314553df Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Fri, 11 Dec 2009 17:42:54 +0000
Subject: [PATCH] error_vector

---
 cpp/GaussianFactor.cpp | 30 ++++++++++++++++--------------
 cpp/GaussianFactor.h   |  1 +
 2 files changed, 17 insertions(+), 14 deletions(-)

diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp
index d71473d2f..9cbaf20df 100644
--- a/cpp/GaussianFactor.cpp
+++ b/cpp/GaussianFactor.cpp
@@ -126,14 +126,19 @@ bool GaussianFactor::equals(const Factor<VectorConfig>& f, double tol) const {
 }
 
 /* ************************************************************************* */
-// we might have multiple As, so iterate and subtract from b
-double GaussianFactor::error(const VectorConfig& c) const {
-  if (empty()) return 0;
-  Vector e = b_;
+Vector GaussianFactor::error_vector(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]);
-  Vector weighted = ediv(e,sigmas_);
+    e += Vector(Aj * c[j]);
+  return ediv(e,sigmas_);
+}
+
+/* ************************************************************************* */
+double GaussianFactor::error(const VectorConfig& c) const {
+  if (empty()) return 0;
+  Vector weighted = error_vector(c);
   return 0.5 * inner_prod(weighted,weighted);
 }
 
@@ -361,17 +366,14 @@ GaussianFactor::eliminate(const string& key) const
 /* ************************************************************************* */
 void GaussianFactor::addGradientContribution(const VectorConfig& x, VectorConfig& g) const {
 	// calculate the value of the factor
-	Vector e = -b_;
-  string j; Matrix Aj;
-  FOREACH_PAIR(j, Aj, As_) e += Aj * x[j];
-
-  // transpose
-  Vector et = trans(e);
+	Vector e = GaussianFactor::error_vector(x);
+  Vector et = trans(e); // transpose
 
   // contribute to gradient for each connected variable
+  string j; Matrix Aj;
   FOREACH_PAIR(j, Aj, As_) {
-  	Vector dj = trans(et*Aj);  // this factor's contribution to gradient on j
-  	Vector wdj = ediv(dj,emul(sigmas_,sigmas_)); // properly weight by sigmas
+  	Vector dj = trans(et*Aj);      // this factor's contribution to gradient on j
+  	Vector wdj = ediv(dj,sigmas_); // properly weight by sigmas
   	g.add(j,wdj);
   }
 }
diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h
index af1e5fa84..36df56396 100644
--- a/cpp/GaussianFactor.h
+++ b/cpp/GaussianFactor.h
@@ -117,6 +117,7 @@ public:
 
 	// Implementing Factor virtual functions
 
+	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();}