From 76959d4d18c224c9b0588f71fcdc825defaad6de Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 01:14:32 -0500 Subject: [PATCH] Added hessianDiagonal; works for Gaussian factor graphs with JacobianFactors --- gtsam/linear/GaussianFactor.h | 3 +++ gtsam/linear/GaussianFactorGraph.cpp | 10 ++++++++++ gtsam/linear/GaussianFactorGraph.h | 3 +++ gtsam/linear/HessianFactor.cpp | 6 ++++++ gtsam/linear/HessianFactor.h | 5 ++++- gtsam/linear/JacobianFactor.cpp | 20 ++++++++++++++++++- gtsam/linear/JacobianFactor.h | 3 +++ .../testGaussianFactorGraphUnordered.cpp | 8 ++++---- 8 files changed, 52 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 969529c78..f91e07d5b 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -96,6 +96,9 @@ namespace gtsam { */ virtual Matrix information() const = 0; + /// Return the diagonal of the Hessian for this factor + virtual VectorValues hessianDiagonal() const = 0; + /** Clone a factor (make a deep copy) */ virtual GaussianFactor::shared_ptr clone() const = 0; diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index cb9238866..91b27af4f 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -196,6 +196,16 @@ namespace gtsam { augmented.col(augmented.rows() - 1).head(augmented.rows() - 1)); } + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::hessianDiagonal() const { + VectorValues d; + BOOST_FOREACH(const sharedFactor& factor, *this) { + VectorValues di = factor->hessianDiagonal(); + d.addInPlace_(di); + } + return d; + } + /* ************************************************************************* */ VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const { diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index afb3a158b..fc104a961 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -221,6 +221,9 @@ namespace gtsam { */ std::pair hessian(boost::optional optionalOrdering = boost::none) const; + /** Return only the diagonal of the Hessian A'*A, as a VectorValues */ + VectorValues hessianDiagonal() const; + /** Solve the factor graph by performing multifrontal variable elimination in COLAMD order using * the dense elimination function specified in \c function (default EliminatePreferCholesky), * followed by back-substitution in the Bayes tree resulting from elimination. Is equivalent diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 17727a2a1..9811c4051 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -351,6 +351,12 @@ Matrix HessianFactor::information() const return info_.range(0, this->size(), 0, this->size()).selfadjointView(); } +/* ************************************************************************* */ +VectorValues HessianFactor::hessianDiagonal() const { + VectorValues d; + return d; +} + /* ************************************************************************* */ Matrix HessianFactor::augmentedJacobian() const { diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 1d4bc2b01..d6b4781d9 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -335,7 +335,10 @@ namespace gtsam { * GaussianFactor. */ virtual Matrix information() const; - + + /// Return the diagonal of the Hessian for this factor + virtual VectorValues hessianDiagonal() const; + /** * Return (dense) matrix associated with factor * @param ordering of variables needed for matrix column order diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index e18055edc..0630392b7 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -438,6 +438,23 @@ namespace gtsam { } } + /* ************************************************************************* */ + VectorValues JacobianFactor::hessianDiagonal() const { + VectorValues d; + for(size_t pos=0; pos xi = x.tryInsert(keys_[pos], Vector()); + pair xi = x.tryInsert(j, Vector()); if(xi.second) xi.first->second = Vector::Zero(getDim(begin() + pos)); gtsam::transposeMultiplyAdd(Ab_(pos), E, xi.first->second); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 90e39867b..9041c8a8a 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -181,6 +181,9 @@ namespace gtsam { */ virtual Matrix information() const; + /// Return the diagonal of the Hessian for this factor + virtual VectorValues hessianDiagonal() const; + /** * @brief Returns (dense) A,b pair associated with factor, bakes in the weights */ diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 2a5f9224a..6543e9d51 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -139,10 +139,10 @@ TEST(GaussianFactorGraph, matrices) { EXPECT(assert_equal(L, actualL)); EXPECT(assert_equal(eta, actualeta)); - Vector expectLdiagonal(5); // Make explicit that diagonal is sum-squares of columns - expectLdiagonal << 1+25+81, 4+36+100, 9+49, 121+196, 144+225; - EXPECT(assert_equal(L.diagonal(), expectLdiagonal)); - // EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal())); + VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns + expectLdiagonal.insert(0, (Vector(3) << 1+25+81, 4+36+100, 9+49)); + expectLdiagonal.insert(1, (Vector(2) << 121+196, 144+225)); + EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal())); } /* ************************************************************************* */