Added Errors class and operator* for GaussianFactor and GaussianFactorGraph. Also moved a few functions to cpp.

cpp/Errors.cpp

@@ -0,0 +1,36 @@
+/**
+ * @file    Errors.cpp
+ * @brief   Factor Graph Configuration
+ * @brief   Errors
+ * @author  Carlos Nieto
+ * @author  Christian Potthast
+ */
+
+#include <boost/foreach.hpp>
+#include "Errors.h"
+
+using namespace std;
+
+namespace gtsam {
+
+/* ************************************************************************* */
+void Errors::print(const std::string& s) const {
+  odprintf("%s:\n", s.c_str());
+  for (size_t i=0;i<size();i++) {
+    odprintf("%d:", i);
+    gtsam::print((*this)[i]);
+  }
+}
+
+/* ************************************************************************* */
+bool Errors::equals(const Errors& expected, double tol) const {
+  if( size() != expected.size() ) return false;
+  for (size_t i=0;i<size();i++)
+    if(!equal_with_abs_tol(expected[i],(*this)[i],tol))
+    	return false;
+  return true;
+}
+
+/* ************************************************************************* */
+
+} // gtsam

cpp/Errors.h

@@ -0,0 +1,31 @@
+/**
+ * @file    Errors.h
+ * @brief   vector of errors
+ * @author  Frank Dellaert
+ */
+
+// \callgraph
+
+#pragma once
+
+#include <vector>
+
+#include "Testable.h"
+#include "Vector.h"
+
+namespace gtsam {
+	
+  /** vector of errors */
+  class Errors : public std::vector<Vector>, public Testable<Errors> {
+
+  public:
+
+  	/** print */
+    void print(const std::string& s = "Errors") const;
+
+    /** equals, for unit testing */
+    bool equals(const Errors& expected, double tol=1e-9) const;
+
+  }; // Errors
+
+} // gtsam

cpp/GaussianFactor.cpp

@@ -131,14 +131,13 @@ Vector GaussianFactor::unweighted_error(const VectorConfig& c) const {
   if (empty()) return e;
   string j; Matrix Aj;
   FOREACH_PAIR(j, Aj, As_)
-    e += Vector(Aj * c[j]);
+    e += (Aj * c[j]);
   return e;
 }
 
 /* ************************************************************************* */
 Vector GaussianFactor::error_vector(const VectorConfig& c) const {
-  Vector e = -b_;
+  if (empty()) return (-b_);
-  if (empty()) return e;
   return ediv(unweighted_error(c),sigmas_);
 }
 
@@ -402,6 +401,18 @@ GaussianFactor::shared_ptr GaussianFactor::alphaFactor(const VectorConfig& x,
 	return factor;
 }
 
+/* ************************************************************************* */
+Vector GaussianFactor::operator*(const VectorConfig& x) const {
+	Vector Ax = zero(b_.size());
+  if (empty()) return Ax;
+
+  string j; Matrix Aj;
+  FOREACH_PAIR(j, Aj, As_)
+    Ax += (Aj * x[j]);
+
+  return ediv(Ax,sigmas_);
+}
+
 /* ************************************************************************* */
 namespace gtsam {
 

cpp/GaussianFactor.h

@@ -230,6 +230,11 @@ public:
 	 */
 	shared_ptr alphaFactor(const VectorConfig& x, const VectorConfig& d) const;
 
+	/**
+	 * Return A*x
+	 */
+	Vector operator*(const VectorConfig& x) const;
+
 	/* ************************************************************************* */
 	// MUTABLE functions. FD:on the path to being eradicated
 	/* ************************************************************************* */

cpp/GaussianFactorGraph.cpp

@@ -31,6 +31,22 @@ GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) :
 	FactorGraph<GaussianFactor> (CBN) {
 }
 
+/* ************************************************************************* */
+double GaussianFactorGraph::error(const VectorConfig& x) const {
+	double total_error = 0.;
+	BOOST_FOREACH(sharedFactor factor,factors_)
+		total_error += factor->error(x);
+	return total_error;
+}
+
+/* ************************************************************************* */
+Errors GaussianFactorGraph::operator*(const VectorConfig& x) const {
+	Errors e;
+	BOOST_FOREACH(sharedFactor factor,factors_)
+		e.push_back((*factor)*x);
+	return e;
+}
+
 /* ************************************************************************* */
 set<string> GaussianFactorGraph::find_separator(const string& key) const
 {

cpp/GaussianFactorGraph.h

@@ -13,6 +13,7 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
+#include "Errors.h"
 #include "FactorGraph.h"
 #include "GaussianFactor.h"
 #include "GaussianBayesNet.h" // needed for MATLAB toolbox !!
@@ -73,14 +74,10 @@ namespace gtsam {
   	}
 
 		/** unnormalized error */
-		double error(const VectorConfig& c) const {
+		double error(const VectorConfig& x) const;
-			double total_error = 0.;
-			// iterate over all the factors_ to accumulate the log probabilities
-			for (const_iterator factor = factors_.begin(); factor != factors_.end(); factor++)
-				total_error += (*factor)->error(c);
 
-			return total_error;
+		/** return A*x */
-		}
+		Errors operator*(const VectorConfig& x) const;
 
 		/** Unnormalized probability. O(n) */
 		double probPrime(const VectorConfig& c) const {

cpp/Makefile.am

@@ -97,7 +97,7 @@ testBinaryBayesNet_LDADD      = libgtsam.la
 
 # Gaussian inference 
 headers += GaussianFactorSet.h
-sources += VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp
+sources += Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp
 check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet 
 testVectorConfig_SOURCES        = testVectorConfig.cpp
 testVectorConfig_LDADD          = libgtsam.la

cpp/testGaussianFactorGraph.cpp

@@ -11,6 +11,7 @@ using namespace std;
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
 #include <boost/assign/std/list.hpp> // for operator +=
+#include <boost/assign/std/vector.hpp> // for operator +=
 using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
@@ -576,12 +577,17 @@ TEST( GaussianFactorGraph, gradient )
 	CHECK(assert_equal(zero,actual2));
 }
 
-/* ************************************************************************* *
+/* ************************************************************************* */
 TEST( GaussianFactorGraph, multiplication )
 {
 	GaussianFactorGraph A = createGaussianFactorGraph();
   VectorConfig x = createConfig();
-  ErrorConfig actual = A * x;
+  Errors actual = A * x;
+  Errors expected;
+  expected += Vector_(2, 0.0, 0.0);
+  expected += Vector_(2,15.0, 0.0);
+  expected += Vector_(2, 0.0,-5.0);
+  expected += Vector_(2,-7.5,-5.0);
 	CHECK(assert_equal(expected,actual));
 }