diff --git a/gtsam/linear/Errors.cpp b/gtsam/linear/Errors.cpp index e7d893d25..92f7bb4b8 100644 --- a/gtsam/linear/Errors.cpp +++ b/gtsam/linear/Errors.cpp @@ -26,16 +26,18 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Errors::Errors(const VectorValues& V) { - for(const Vector& e: V | boost::adaptors::map_values) { - push_back(e); +Errors createErrors(const VectorValues& V) { + Errors result; + for (const Vector& e : V | boost::adaptors::map_values) { + result.push_back(e); } + return result; } /* ************************************************************************* */ -void Errors::print(const std::string& s) const { +void print(const Errors& e, const string& s) { cout << s << endl; - for(const Vector& v: *this) + for(const Vector& v: e) gtsam::print(v); } @@ -48,50 +50,49 @@ struct equalsVector : public std::function { } }; -bool Errors::equals(const Errors& expected, double tol) const { - if( size() != expected.size() ) return false; - return equal(begin(),end(),expected.begin(),equalsVector(tol)); +bool equality(const Errors& actual, const Errors& expected, double tol) { + if (actual.size() != expected.size()) return false; + return equal(actual.begin(), actual.end(), expected.begin(), + equalsVector(tol)); } /* ************************************************************************* */ -Errors Errors::operator+(const Errors& b) const { +Errors operator+(const Errors& a, const Errors& b) { #ifndef NDEBUG - size_t m = size(); + size_t m = a.size(); if (b.size()!=m) throw(std::invalid_argument("Errors::operator+: incompatible sizes")); #endif Errors result; Errors::const_iterator it = b.begin(); - for(const Vector& ai: *this) + for(const Vector& ai: a) result.push_back(ai + *(it++)); return result; } /* ************************************************************************* */ -Errors Errors::operator-(const Errors& b) const { +Errors operator-(const Errors& a, const Errors& b) { #ifndef NDEBUG - size_t m = size(); + size_t m = a.size(); if (b.size()!=m) throw(std::invalid_argument("Errors::operator-: incompatible sizes")); #endif Errors result; Errors::const_iterator it = b.begin(); - for(const Vector& ai: *this) + for(const Vector& ai: a) result.push_back(ai - *(it++)); return result; } /* ************************************************************************* */ -Errors Errors::operator-() const { +Errors operator-(const Errors& a) { Errors result; - for(const Vector& ai: *this) + for(const Vector& ai: a) result.push_back(-ai); return result; } - - /* ************************************************************************* */ double dot(const Errors& a, const Errors& b) { #ifndef NDEBUG @@ -102,7 +103,7 @@ double dot(const Errors& a, const Errors& b) { double result = 0.0; Errors::const_iterator it = b.begin(); for(const Vector& ai: a) - result += gtsam::dot(ai, *(it++)); + result += gtsam::dot(ai, *(it++)); return result; } @@ -113,11 +114,6 @@ void axpy(double alpha, const Errors& x, Errors& y) { yi += alpha * (*(it++)); } -/* ************************************************************************* */ -void print(const Errors& a, const string& s) { - a.print(s); -} - /* ************************************************************************* */ } // gtsam diff --git a/gtsam/linear/Errors.h b/gtsam/linear/Errors.h index f5bea5501..faf83d4d1 100644 --- a/gtsam/linear/Errors.h +++ b/gtsam/linear/Errors.h @@ -20,64 +20,54 @@ #pragma once #include -#include #include +#include #include namespace gtsam { - // Forward declarations - class VectorValues; +// Forward declarations +class VectorValues; - /** vector of errors */ - class GTSAM_EXPORT Errors : public FastList { +/// Errors is a vector of errors. +using Errors = FastList; - using Base = FastList; +/// Break V into pieces according to its start indices. +GTSAM_EXPORT Errors createErrors(const VectorValues& V); - public: +/// Print an Errors instance. +GTSAM_EXPORT void print(const Errors& e, const std::string& s = "Errors"); - using Base::Base; // inherit constructors +// Check equality for unit testing. +GTSAM_EXPORT bool equality(const Errors& actual, const Errors& expected, + double tol = 1e-9); - /** Default constructor */ - Errors() = default; +/// Addition. +GTSAM_EXPORT Errors operator+(const Errors& a, const Errors& b); - /** break V into pieces according to its start indices */ - Errors(const VectorValues&V); +/// Subtraction. +GTSAM_EXPORT Errors operator-(const Errors& a, const Errors& b); - /** print */ - void print(const std::string& s = "Errors") const; +/// Negation. +GTSAM_EXPORT Errors operator-(const Errors& a); - /** equals, for unit testing */ - bool equals(const Errors& expected, double tol=1e-9) const; +/// Dot product. +GTSAM_EXPORT double dot(const Errors& a, const Errors& b); - /** Addition */ - Errors operator+(const Errors& b) const; +/// BLAS level 2 style AXPY, `y := alpha*x + y` +GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y); - /** subtraction */ - Errors operator-(const Errors& b) const; +/// traits +template <> +struct traits { + static void Print(const Errors& e, const std::string& str = "") { + print(e, str); + } + static bool Equals(const Errors& actual, const Errors& expected, + double tol = 1e-8) { + return equality(actual, expected, tol); + } +}; - /** negation */ - Errors operator-() const ; - - }; // Errors - - /** - * dot product - */ - GTSAM_EXPORT double dot(const Errors& a, const Errors& b); - - /** - * BLAS level 2 style - */ - GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y); - - /** print with optional string */ - GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error"); - - /// traits - template<> - struct traits : public Testable { - }; - -} //\ namespace gtsam +} // namespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 6689cdbed..4e761a3fd 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -110,7 +110,7 @@ VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { /* ************************************************************************* */ double SubgraphPreconditioner::error(const VectorValues& y) const { - Errors e(y); + Errors e = createErrors(y); VectorValues x = this->x(y); Errors e2 = Ab2_.gaussianErrors(x); return 0.5 * (dot(e, e) + dot(e2,e2)); @@ -129,7 +129,7 @@ VectorValues SubgraphPreconditioner::gradient(const VectorValues &y) const { /* ************************************************************************* */ // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] Errors SubgraphPreconditioner::operator*(const VectorValues &y) const { - Errors e(y); + Errors e = createErrors(y); VectorValues x = Rc1_.backSubstitute(y); /* x=inv(R1)*y */ Errors e2 = Ab2_ * x; /* A2*x */ e.splice(e.end(), e2);