From d9431d3d400f40bb51ba86ca7286aedb05dde4df Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:09:04 -0500 Subject: [PATCH 01/10] Added expected values for hessianBlockDiagonal --- gtsam/linear/tests/testHessianFactorUnordered.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index faa1ebd65..0df621688 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -460,11 +460,18 @@ TEST(HessianFactor, hessianDiagonal) HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); + // hessianDiagonal VectorValues expected; expected.insert(0, (Vector(1) << 1)); expected.insert(1, (Vector(2) << 1,1)); EXPECT(assert_equal(expected, factor.hessianDiagonal())); -} + + // hessianBlockDiagonal + map expectedBD; + expected.insert(make_pair(0,G11)); + expected.insert(make_pair(1,G22)); + //EXPECT(assert_equal(expectedBD, factor.hessianBlockDiagonal())); + } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} From b837cb1b03de0eb2537abad86941a4a740bb3b44 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:18:46 -0500 Subject: [PATCH 02/10] Declared hessianBlockDiagonal --- gtsam/linear/GaussianFactor.h | 3 +++ gtsam/linear/HessianFactor.h | 3 +++ gtsam/linear/JacobianFactor.h | 3 +++ 3 files changed, 9 insertions(+) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index f91e07d5b..58eaf4460 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -99,6 +99,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const = 0; + /// Return the block diagonal of the Hessian for this factor + virtual std::map hessianBlockDiagonal() const = 0; + /** Clone a factor (make a deep copy) */ virtual GaussianFactor::shared_ptr clone() const = 0; diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index d6b4781d9..18c238e57 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -339,6 +339,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; + /// Return the block diagonal of the Hessian for this factor + virtual std::map hessianBlockDiagonal() const; + /** * Return (dense) matrix associated with factor * @param ordering of variables needed for matrix column order diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 9041c8a8a..a444d514a 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -184,6 +184,9 @@ namespace gtsam { /// Return the diagonal of the Hessian for this factor virtual VectorValues hessianDiagonal() const; + /// Return the block diagonal of the Hessian for this factor + virtual std::map hessianBlockDiagonal() const; + /** * @brief Returns (dense) A,b pair associated with factor, bakes in the weights */ From 83918be8cd31f47ed10f91d1caa7e94d960a8e8d Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:18:59 -0500 Subject: [PATCH 03/10] Put in empty bodies for hessianBlockDiagonal --- gtsam/linear/HessianFactor.cpp | 6 ++++++ gtsam/linear/JacobianFactor.cpp | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index c93035d42..a56e02461 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -363,6 +363,12 @@ VectorValues HessianFactor::hessianDiagonal() const { return d; } +/* ************************************************************************* */ +map HessianFactor::hessianBlockDiagonal() const { + map blocks; + return blocks; +} + /* ************************************************************************* */ Matrix HessianFactor::augmentedJacobian() const { diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index f7d550390..66f7a5f05 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -455,6 +455,12 @@ namespace gtsam { return d; } + /* ************************************************************************* */ + map JacobianFactor::hessianBlockDiagonal() const { + map blocks; + return blocks; + } + /* ************************************************************************* */ Vector JacobianFactor::operator*(const VectorValues& x) const { Vector Ax = zero(Ab_.rows()); From 6e4433f589df724738220ca19b0a649363a7b2cd Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:19:17 -0500 Subject: [PATCH 04/10] Made correct test for HessianFactor::hessianBlockDiagonal --- gtsam/linear/tests/testHessianFactorUnordered.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index 0df621688..9cd18c974 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -467,10 +467,10 @@ TEST(HessianFactor, hessianDiagonal) EXPECT(assert_equal(expected, factor.hessianDiagonal())); // hessianBlockDiagonal - map expectedBD; - expected.insert(make_pair(0,G11)); - expected.insert(make_pair(1,G22)); - //EXPECT(assert_equal(expectedBD, factor.hessianBlockDiagonal())); + map actualBD = factor.hessianBlockDiagonal(); + LONGS_EQUAL(2,actualBD.size()); + EXPECT(assert_equal(G11,actualBD[0])); + EXPECT(assert_equal(G22,actualBD[1])); } /* ************************************************************************* */ From 7abcd811502c881fe936559a22ce76f5154628ea Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:22:13 -0500 Subject: [PATCH 05/10] Working and tested implementation of hessianBlockDiagonal --- gtsam/linear/HessianFactor.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index a56e02461..ce5f093dc 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -366,6 +366,12 @@ VectorValues HessianFactor::hessianDiagonal() const { /* ************************************************************************* */ map HessianFactor::hessianBlockDiagonal() const { map blocks; + // Loop over all variables + for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { + // Get the diagonal block, and insert it + Matrix B = info_(j, j).selfadjointView(); + blocks.insert(make_pair(keys_[j],B)); + } return blocks; } From 411381fd884bff8007340797a5121673bec882a6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:31:21 -0500 Subject: [PATCH 06/10] unit test for Jacobian::hessianBlockDiagonal --- gtsam/linear/tests/testJacobianFactorUnordered.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactorUnordered.cpp index a812e1768..aacf38199 100644 --- a/gtsam/linear/tests/testJacobianFactorUnordered.cpp +++ b/gtsam/linear/tests/testJacobianFactorUnordered.cpp @@ -240,6 +240,13 @@ TEST(JacobianFactor, matrices) expectDiagonal.insert(10, (Vector(3) << 4, 4, 4)/0.25); expectDiagonal.insert(15, (Vector(3) << 9, 9, 9)/0.25); EXPECT(assert_equal(expectDiagonal, factor.hessianDiagonal())); + + // hessianBlockDiagonal + map actualBD = factor.hessianBlockDiagonal(); + LONGS_EQUAL(3,actualBD.size()); + EXPECT(assert_equal(4*eye(3),actualBD[5])); + EXPECT(assert_equal(16*eye(3),actualBD[10])); + EXPECT(assert_equal(36*eye(3),actualBD[15])); } /* ************************************************************************* */ From a2829fffad0ca30a5fe32d00a3f78944e5156018 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:31:40 -0500 Subject: [PATCH 07/10] Working and tested implementation of hessianBlockDiagonal --- gtsam/linear/JacobianFactor.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 66f7a5f05..85eb6880f 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -458,6 +458,12 @@ namespace gtsam { /* ************************************************************************* */ map JacobianFactor::hessianBlockDiagonal() const { map blocks; + for(size_t pos=0; posWhiten(Ab_(pos)); + blocks.insert(make_pair(j,Aj.transpose()*Aj)); + } return blocks; } From 38d8de15376607cf33db889a43c0d03c22b7142b Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:39:07 -0500 Subject: [PATCH 08/10] Put in test --- .../tests/testGaussianFactorGraphUnordered.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 6543e9d51..60fca2d8f 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -112,10 +112,14 @@ TEST(GaussianFactorGraph, matrices) { // 9 10 0 11 12 13 // 0 0 0 14 15 16 + Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7); + Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0); + Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15); + GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); - gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model); - gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model); + gfg.add(0, A00, (Vector(2) << 4., 8.), model); + gfg.add(0, A10, 1, A11, (Vector(2) << 13.,16.), model); Matrix Ab(4,6); Ab << @@ -124,25 +128,35 @@ TEST(GaussianFactorGraph, matrices) { 9,10, 0,11,12,13, 0, 0, 0,14,15,16; + // augmented versions EXPECT(assert_equal(Ab, gfg.augmentedJacobian())); EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian())); + // jacobian Matrix A = Ab.leftCols(Ab.cols()-1); Vector b = Ab.col(Ab.cols()-1); Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian(); EXPECT(assert_equal(A, actualA)); EXPECT(assert_equal(b, actualb)); + // hessian Matrix L = A.transpose() * A; Vector eta = A.transpose() * b; Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); EXPECT(assert_equal(L, actualL)); EXPECT(assert_equal(eta, actualeta)); + // hessianBlockDiagonal 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())); + + // hessianBlockDiagonal + map actualBD = gfg.hessianBlockDiagonal(); + LONGS_EQUAL(2,actualBD.size()); + EXPECT(assert_equal(A00.transpose()*A00 + A10.transpose()*A10,actualBD[0])); + EXPECT(assert_equal(A11.transpose()*A11,actualBD[1])); } /* ************************************************************************* */ From bb9ada6c7a73af97311aa0ec4acc7e6d2c2796c6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:39:36 -0500 Subject: [PATCH 09/10] declared hessianBlockDiagonal --- gtsam/linear/GaussianFactorGraph.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index fc104a961..28b9eab55 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -224,6 +224,9 @@ namespace gtsam { /** Return only the diagonal of the Hessian A'*A, as a VectorValues */ VectorValues hessianDiagonal() const; + /** Return the block diagonal of the Hessian for this factor */ + std::map hessianBlockDiagonal() 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 From 2865aab027f5b80435d59743510600c8cefe603f Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 14 Feb 2014 13:47:47 -0500 Subject: [PATCH 10/10] Working and tested implementation of hessianBlockDiagonal --- gtsam/linear/GaussianFactorGraph.cpp | 32 ++++++++++++++++++++++------ 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 91b27af4f..766aceec6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -79,7 +79,7 @@ namespace gtsam { } /* ************************************************************************* */ - std::vector > GaussianFactorGraph::sparseJacobian() const { + vector > GaussianFactorGraph::sparseJacobian() const { // First find dimensions of each variable vector dims; BOOST_FOREACH(const sharedFactor& factor, *this) { @@ -146,7 +146,7 @@ namespace gtsam { // call sparseJacobian typedef boost::tuple triplet; - std::vector result = sparseJacobian(); + vector result = sparseJacobian(); // translate to base 1 matrix size_t nzmax = result.size(); @@ -169,7 +169,7 @@ namespace gtsam { } /* ************************************************************************* */ - std::pair GaussianFactorGraph::jacobian( + pair GaussianFactorGraph::jacobian( boost::optional optionalOrdering) const { Matrix augmented = augmentedJacobian(optionalOrdering); return make_pair(augmented.leftCols(augmented.cols() - 1), @@ -188,7 +188,7 @@ namespace gtsam { } /* ************************************************************************* */ - std::pair GaussianFactorGraph::hessian( + pair GaussianFactorGraph::hessian( boost::optional optionalOrdering) const { Matrix augmented = augmentedHessian(optionalOrdering); return make_pair( @@ -206,6 +206,24 @@ namespace gtsam { return d; } + /* ************************************************************************* */ + map GaussianFactorGraph::hessianBlockDiagonal() const { + map blocks; + BOOST_FOREACH(const sharedFactor& factor, *this) { + map BD = factor->hessianBlockDiagonal(); + map::const_iterator it = BD.begin(); + for(;it!=BD.end();it++) { + Key j = it->first; // variable key for this block + const Matrix& Bj = it->second; + if (blocks.count(j)) + blocks[j] += Bj; + else + blocks.insert(make_pair(j,Bj)); + } + } + return blocks; + } + /* ************************************************************************* */ VectorValues GaussianFactorGraph::optimize(OptionalOrdering ordering, const Eliminate& function) const { @@ -217,9 +235,9 @@ namespace gtsam { namespace { JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast(gf); - if( !result ) { - result = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) - } + if( !result ) + // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + result = boost::make_shared(*gf); return result; } }