From 6a383799d7100897d6e2c9ae35df18a9c0a7fca8 Mon Sep 17 00:00:00 2001 From: Pablo Fernandez Alcantarilla Date: Thu, 24 Oct 2013 15:52:32 +0000 Subject: [PATCH] Skeleton code for GaussianFactorGraph::multiplyHessian --- gtsam/linear/GaussianFactor.h | 3 ++ gtsam/linear/GaussianFactorGraph.cpp | 24 ++++++++----- gtsam/linear/GaussianFactorGraph.h | 3 ++ gtsam/linear/HessianFactor.cpp | 6 ++++ gtsam/linear/HessianFactor.h | 4 +-- gtsam/linear/JacobianFactor.cpp | 6 ++++ gtsam/linear/JacobianFactor.h | 3 ++ .../testGaussianFactorGraphUnordered.cpp | 35 +++++++++++++++++++ 8 files changed, 74 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 1c9589ce2..f29be5a34 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -106,6 +106,9 @@ namespace gtsam { */ virtual GaussianFactor::shared_ptr negate() const = 0; + /// y += alpha * A'*A*x + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y)=0; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index d1505a89d..a7d7de0be 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -246,6 +246,14 @@ namespace gtsam { return e; } + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::multiplyHessian(const VectorValues& x) const { + VectorValues y; + BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) + f->multiplyHessianAdd(1.0,x,y); + return y; + } + /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { multiplyInPlace(x, e.begin()); @@ -275,15 +283,15 @@ namespace gtsam { /* ************************************************************************* */ // x += alpha*A'*e - void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& x) const - { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Ai->transposeMultiplyAdd(alpha, *(ei++), x); - } +void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorValues& x) const { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha, *(ei++), x); } +} ///* ************************************************************************* */ //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index aaca09b12..76f97607b 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -269,6 +269,9 @@ namespace gtsam { ///** return A*x */ Errors operator*(const VectorValues& x) const; + ///** return A'A*x */ + VectorValues multiplyHessian(const VectorValues& x) const; + ///** In-place version e <- A*x that overwrites e. */ void multiplyInPlace(const VectorValues& x, Errors& e) const; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9575971cb..23634fb12 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -499,6 +499,12 @@ GaussianFactor::shared_ptr HessianFactor::negate() const return result; } +/* ************************************************************************* */ +void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) { + +} + /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 9fd65a578..6cd6c817d 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -356,8 +356,8 @@ namespace gtsam { */ void updateATA(const HessianFactor& update, const Scatter& scatter); - /** Return A'A*x */ - // Vector operator*(const VectorValues& x) const; + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y); /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2e81b0b14..d794a7a12 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -432,6 +432,12 @@ namespace gtsam { return model_ ? model_->whiten(Ax) : Ax; } + /* ************************************************************************* */ + void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, + VectorValues& y) { + + } + /* ************************************************************************* */ void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index d1d4fda2a..f5a2e9c62 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -263,6 +263,9 @@ namespace gtsam { /** Return A*x */ Vector operator*(const VectorValues& x) const; + /** y += alpha * A'*A*x */ + void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y); + /** x += A'*e. If x is initially missing any values, they are created and assumed to start as * zero vectors. */ void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index d3eec4fee..736ca4a4d 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -220,6 +220,41 @@ TEST(GaussianFactorGraph, eliminate_empty ) EXPECT(assert_equal(*remainingGFG, expectedLF)); } +/* ************************************************************************* */ +//TEST( GaussianFactorGraph, multiplyHessian ) +//{ +// // A is : +// // x1 x2 x3 x4 x5 +// // 1 2 3 0 0 +// // 5 6 7 0 0 +// // 9 10 0 11 12 +// // 0 0 0 14 15 +// GaussianFactorGraph A = createSimpleGaussianFactorGraph(); +// +// VectorValues x = map_list_of +// (0, (Vec(2) << 1,2)) +// (1, (Vec(2) << 3,4)) +// (2, (Vec(1) << 5)); +// +// // AtA is : +// // x1 x2 x3 x4 x5 +// // 107 122 38 99 108 +// // 122 140 48 110 120 +// // 38 48 58 0 0 +// // 99 110 0 317 342 +// // 108 120 0 342 369 +// +// // AtAx is: +// // 1401 1586 308 3297 3561 +// VectorValues expected; +// expected.insert(0, (Vec(2) << 1401,1586)); +// expected.insert(1, (Vec(2) << 308,3297)); +// expected.insert(2, (Vec(1) << 3561)); +// +// VectorValues actual = A.multiplyHessian(x); +// EXPECT(assert_equal(expected, actual)); +//} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */