From 7d1428de60056aa438c01200d7bdacb533ac4647 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Dec 2009 22:48:41 +0000 Subject: [PATCH] Added Errors class and operator* for GaussianFactor and GaussianFactorGraph. Also moved a few functions to cpp. --- cpp/Errors.cpp | 36 +++++++++++++++++++++++++++++++++ cpp/Errors.h | 31 ++++++++++++++++++++++++++++ cpp/GaussianFactor.cpp | 17 +++++++++++++--- cpp/GaussianFactor.h | 5 +++++ cpp/GaussianFactorGraph.cpp | 16 +++++++++++++++ cpp/GaussianFactorGraph.h | 11 ++++------ cpp/Makefile.am | 2 +- cpp/testGaussianFactorGraph.cpp | 10 +++++++-- 8 files changed, 115 insertions(+), 13 deletions(-) create mode 100644 cpp/Errors.cpp create mode 100644 cpp/Errors.h diff --git a/cpp/Errors.cpp b/cpp/Errors.cpp new file mode 100644 index 000000000..3dabf012b --- /dev/null +++ b/cpp/Errors.cpp @@ -0,0 +1,36 @@ +/** + * @file Errors.cpp + * @brief Factor Graph Configuration + * @brief Errors + * @author Carlos Nieto + * @author Christian Potthast + */ + +#include +#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 + +#include "Testable.h" +#include "Vector.h" + +namespace gtsam { + + /** vector of errors */ + class Errors : public std::vector, public Testable { + + 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 diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index 0cacada3c..4d6ae5772 100644 --- a/cpp/GaussianFactor.cpp +++ b/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 e; + if (empty()) return (-b_); 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 { diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index ac78789e3..87defea8b 100644 --- a/cpp/GaussianFactor.h +++ b/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 /* ************************************************************************* */ diff --git a/cpp/GaussianFactorGraph.cpp b/cpp/GaussianFactorGraph.cpp index 8faec9d47..320483ee7 100644 --- a/cpp/GaussianFactorGraph.cpp +++ b/cpp/GaussianFactorGraph.cpp @@ -31,6 +31,22 @@ GaussianFactorGraph::GaussianFactorGraph(const GaussianBayesNet& CBN) : FactorGraph (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 GaussianFactorGraph::find_separator(const string& key) const { diff --git a/cpp/GaussianFactorGraph.h b/cpp/GaussianFactorGraph.h index 1d6486aa9..8bf483252 100644 --- a/cpp/GaussianFactorGraph.h +++ b/cpp/GaussianFactorGraph.h @@ -13,6 +13,7 @@ #pragma once #include +#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 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); + double error(const VectorConfig& x) const; - return total_error; - } + /** return A*x */ + Errors operator*(const VectorConfig& x) const; /** Unnormalized probability. O(n) */ double probPrime(const VectorConfig& c) const { diff --git a/cpp/Makefile.am b/cpp/Makefile.am index b1b85bc0d..c5a30140c 100644 --- a/cpp/Makefile.am +++ b/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 diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 79cd9ed07..bb8dc1251 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -11,6 +11,7 @@ using namespace std; #include #include #include // for operator += +#include // for operator += using namespace boost::assign; #include @@ -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)); }