From 274265a4016e8d0b86f2eab3ba11ad5e4bb28e13 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 21 Feb 2010 00:01:43 +0000 Subject: [PATCH] Errors:axpy and testErrors --- .cproject | 13 +++++++++++-- cpp/Errors.cpp | 15 +++++++++++++-- cpp/Errors.h | 5 +++++ cpp/Makefile.am | 8 ++++++-- cpp/testErrors.cpp | 35 +++++++++++++++++++++++++++++++++++ 5 files changed, 70 insertions(+), 6 deletions(-) create mode 100644 cpp/testErrors.cpp diff --git a/.cproject b/.cproject index 109495aaf..285f36fc3 100644 --- a/.cproject +++ b/.cproject @@ -469,6 +469,7 @@ make + testBayesTree.run true false @@ -476,7 +477,6 @@ make - testSymbolicBayesNet.run true false @@ -484,6 +484,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +676,7 @@ make + testGraph.run true false @@ -730,7 +732,6 @@ make - testSimulated2D.run true false @@ -776,6 +777,14 @@ true true + +make + +testErrors.run +true +false +true + make -j2 diff --git a/cpp/Errors.cpp b/cpp/Errors.cpp index 12749a869..5bb564b5e 100644 --- a/cpp/Errors.cpp +++ b/cpp/Errors.cpp @@ -38,9 +38,11 @@ bool Errors::equals(const Errors& expected, double tol) const { /* ************************************************************************* */ Errors Errors::operator-(const Errors& b) const { +#ifndef NDEBUG size_t m = size(); - if (b.size()!=m) + if (b.size()!=m) throw(std::invalid_argument("Errors::operator-: incompatible sizes")); +#endif Errors result; Errors::const_iterator it = b.begin(); BOOST_FOREACH(const Vector& ai, *this) @@ -50,16 +52,25 @@ Errors Errors::operator-(const Errors& b) const { /* ************************************************************************* */ double dot(const Errors& a, const Errors& b) { +#ifndef NDEBUG size_t m = a.size(); if (b.size()!=m) throw(std::invalid_argument("Errors::dot: incompatible sizes")); - double result = 0.0; +#endif + double result = 0.0; Errors::const_iterator it = b.begin(); BOOST_FOREACH(const Vector& ai, a) result += gtsam::dot(ai, *(it++)); return result; } +/* ************************************************************************* */ +void axpy(double alpha, const Errors& x, Errors& y) { + Errors::const_iterator it = x.begin(); + BOOST_FOREACH(Vector& yi, y) + axpy(alpha,*(it++),yi); +} + /* ************************************************************************* */ void print(const Errors& a, const string& s) { a.print(s); diff --git a/cpp/Errors.h b/cpp/Errors.h index ca0bc1c60..9503086a8 100644 --- a/cpp/Errors.h +++ b/cpp/Errors.h @@ -36,6 +36,11 @@ namespace gtsam { */ double dot(const Errors& a, const Errors& b); + /** + * BLAS level 2 style + */ + void axpy(double alpha, const Errors& x, Errors& y); + /** print with optional string */ void print(const Errors& a, const std::string& s = "Error"); diff --git a/cpp/Makefile.am b/cpp/Makefile.am index f898768db..671877c6f 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -93,8 +93,10 @@ testBinaryBayesNet_LDADD = libgtsam.la # Gaussian inference headers += GaussianFactorSet.h SharedGaussian.h SharedDiagonal.h VectorConfig.h -sources += NoiseModel.cpp Errors.cpp VectorMap.cpp VectorBTree.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp -check_PROGRAMS += testVectorMap testVectorBTree testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel +sources += NoiseModel.cpp Errors.cpp VectorMap.cpp VectorBTree.cpp GaussianFactor.cpp +sources += GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp +check_PROGRAMS += testVectorMap testVectorBTree testGaussianFactor testGaussianFactorGraph +check_PROGRAMS += testGaussianConditional testGaussianBayesNet testNoiseModel testErrors testVectorMap_SOURCES = testVectorMap.cpp testVectorMap_LDADD = libgtsam.la testVectorBTree_SOURCES = testVectorBTree.cpp @@ -109,6 +111,8 @@ testGaussianBayesNet_SOURCES = testGaussianBayesNet.cpp testGaussianBayesNet_LDADD = libgtsam.la testNoiseModel_SOURCES = testNoiseModel.cpp testNoiseModel_LDADD = libgtsam.la +testErrors_SOURCES = testErrors.cpp +testErrors_LDADD = libgtsam.la # Iterative Methods headers += iterative-inl.h SubgraphPreconditioner-inl.h diff --git a/cpp/testErrors.cpp b/cpp/testErrors.cpp new file mode 100644 index 000000000..a47f05c02 --- /dev/null +++ b/cpp/testErrors.cpp @@ -0,0 +1,35 @@ +/* + * testErrors.cpp + * + * Created on: Feb 20, 2010 + * @Author: Frank Dellaert + */ + +#include // for += +using namespace boost::assign; + +#include +#include "Errors.h" + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( Errors, arithmetic ) +{ + Errors e; + e += Vector_(2,1.0,2.0), Vector_(3,3.0,4.0,5.0); + DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9); + + axpy(2.0,e,e); + Errors expected; + expected += Vector_(2,3.0,6.0), Vector_(3,9.0,12.0,15.0); + CHECK(assert_equal(expected,e)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */