diff --git a/cpp/Errors.cpp b/cpp/Errors.cpp index 3dabf012b..55bbeb091 100644 --- a/cpp/Errors.cpp +++ b/cpp/Errors.cpp @@ -31,6 +31,17 @@ bool Errors::equals(const Errors& expected, double tol) const { return true; } +/* ************************************************************************* */ +double dot(const Errors& a, const Errors& b) { + size_t m = a.size(); + if (b.size()!=m) + throw(std::invalid_argument("Errors::dot: incompatible sizes")); + double result = 0.0; + for (size_t i = 0; i < m; i++) + result += gtsam::dot(a[i], b[i]); + return result; +} + /* ************************************************************************* */ } // gtsam diff --git a/cpp/Errors.h b/cpp/Errors.h index 26304479f..b0f6e90bf 100644 --- a/cpp/Errors.h +++ b/cpp/Errors.h @@ -28,4 +28,9 @@ namespace gtsam { }; // Errors + /** + * dot product + */ + double dot(const Errors& a, const Errors& b); + } // gtsam diff --git a/cpp/VectorConfig.cpp b/cpp/VectorConfig.cpp index 2331e21b3..0b802d803 100644 --- a/cpp/VectorConfig.cpp +++ b/cpp/VectorConfig.cpp @@ -69,6 +69,15 @@ VectorConfig VectorConfig::operator*(double s) const { return scale(s); } +/* ************************************************************************* */ +VectorConfig VectorConfig::operator-() const { + VectorConfig result; + string j; Vector v; + FOREACH_PAIR(j, v, values) + result.insert(j, -v); + return result; +} + /* ************************************************************************* */ void VectorConfig::operator+=(const VectorConfig& b) { string j; Vector b_j; diff --git a/cpp/VectorConfig.h b/cpp/VectorConfig.h index 4676815e7..4f8941563 100644 --- a/cpp/VectorConfig.h +++ b/cpp/VectorConfig.h @@ -62,7 +62,9 @@ namespace gtsam { const Vector& get(const std::string& name) const; /** operator[] syntax for get */ - inline const Vector& operator[](const std::string& name) const { return get(name); } + inline const Vector& operator[](const std::string& name) const { + return get(name); + } bool contains(const std::string& name) const { const_iterator it = values.find(name); @@ -79,6 +81,9 @@ namespace gtsam { VectorConfig scale(double s) const; VectorConfig operator*(double s) const; + /** Negation */ + VectorConfig operator-() const; + /** Add in place */ void operator+=(const VectorConfig &b); @@ -109,6 +114,9 @@ namespace gtsam { } }; // VectorConfig + /** scalar product */ + inline VectorConfig operator*(double s, const VectorConfig& x) {return x*s;} + /** Dot product */ double dot(const VectorConfig&, const VectorConfig&); diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 9ec3fd11f..2e7593fc2 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -606,6 +606,11 @@ TEST( GaussianFactorGraph, transposeMultiplication ) CHECK(assert_equal(expected,actual)); } +/* ************************************************************************* */ +VectorConfig gradient(const GaussianFactorGraph& Ab, const VectorConfig& x) { + return Ab.gradient(x); +} + /* ************************************************************************* */ typedef pair System; @@ -640,7 +645,7 @@ Vector operator^(const System& Ab, const Vector& x) { // "Vector" class V needs dot(v,v), -v, v+v, s*v // "Vector" class E needs dot(v,v) template -Vector conjugateGradientDescent(const S& Ab, V x, double threshold = 1e-9) { +V CGD(const S& Ab, V x, double threshold = 1e-9) { // Start with g0 = A'*(A*x0-b), d0 = - g0 // i.e., first step is in direction of negative gradient @@ -676,11 +681,18 @@ Vector conjugateGradientDescent(const S& Ab, V x, double threshold = 1e-9) { } /* ************************************************************************* */ -// Method of conjugate gradients (CG) +// Method of conjugate gradients (CG), Matrix version Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, double threshold = 1e-9) { System Ab = make_pair(A, b); - return conjugateGradientDescent (Ab, x); + return CGD (Ab, x); +} + +/* ************************************************************************* */ +// Method of conjugate gradients (CG), Gaussian Factor Graph version +VectorConfig conjugateGradientDescent(const GaussianFactorGraph& Ab, + const VectorConfig& x, double threshold = 1e-9) { + return CGD (Ab, x); } /* ************************************************************************* */ @@ -699,8 +711,8 @@ TEST( GaussianFactorGraph, gradientDescent ) CHECK(assert_equal(expected,actual,1e-2)); // Do conjugate gradient descent - VectorConfig actual2 = fg2.conjugateGradientDescent(zero); - //VectorConfig actual2 = conjugateGradientDescent(fg2,zero,zero); + //VectorConfig actual2 = fg2.conjugateGradientDescent(zero); + VectorConfig actual2 = conjugateGradientDescent(fg2,zero); CHECK(assert_equal(expected,actual2,1e-2)); // Do conjugate gradient descent, Matrix version @@ -715,7 +727,7 @@ TEST( GaussianFactorGraph, gradientDescent ) // Do conjugate gradient descent, System version System Ab = make_pair(A,b); - Vector actualX2 = conjugateGradientDescent(Ab,x0); + Vector actualX2 = CGD(Ab,x0); CHECK(assert_equal(expectedX,actualX2,1e-9)); }