From d87ffe7ecedcee6f36f5f278bfadd15e5767d897 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 26 Jan 2011 22:53:14 +0000 Subject: [PATCH] Bug fix and unit test HessianFactor::CombineAndEliminate when keys are not sorted --- gtsam/inference/BayesTree-inl.h | 27 +++++++- gtsam/inference/BayesTree.h | 4 ++ gtsam/linear/HessianFactor.cpp | 35 +++++++---- gtsam/linear/tests/testHessianFactor.cpp | 79 ++++++++++++++++++++++++ 4 files changed, 133 insertions(+), 12 deletions(-) diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 910869def..6074b11b9 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -41,15 +41,31 @@ namespace gtsam { using namespace std; + /* ************************************************************************* */ + template + void BayesTree::Clique::assertInvariants() const { +#ifndef NDEBUG + // We rely on the keys being sorted + typename BayesNet::const_iterator cond = this->begin(); + if(cond != this->end()) { + Index lastKey = (*cond)->key(); + ++cond; + for(; cond != this->end(); ++cond) + assert(lastKey < (*cond)->key()); + } +#endif + } + /* ************************************************************************* */ template - BayesTree::Clique::Clique() {} + BayesTree::Clique::Clique() { assertInvariants(); } /* ************************************************************************* */ template BayesTree::Clique::Clique(const sharedConditional& conditional) { separator_.assign(conditional->parents().begin(), conditional->parents().end()); this->push_back(conditional); + assertInvariants(); } /* ************************************************************************* */ @@ -84,6 +100,7 @@ namespace gtsam { // separator_.assign((*bayesNet.rbegin())->parents().begin(), (*bayesNet.rbegin())->parents().end()); separator_.assign((*bayesNet.rbegin())->beginParents(), (*bayesNet.rbegin())->endParents()); } + assertInvariants(); } /* ************************************************************************* */ @@ -137,6 +154,7 @@ namespace gtsam { BOOST_FOREACH(const shared_ptr& child, children_) { child->permuteWithInverse(inversePermutation); } + assertInvariants(); } /* ************************************************************************* */ @@ -158,6 +176,7 @@ namespace gtsam { (void)child->permuteSeparatorWithInverse(inversePermutation); } } + assertInvariants(); return changed; } @@ -350,6 +369,7 @@ namespace gtsam { p_S_R.permuteWithInverse(toBack); // return the parent shortcut P(Sp|R) + assertInvariants(); return p_S_R; } @@ -372,6 +392,7 @@ namespace gtsam { // Find marginal on the keys we are interested in FactorGraph p_FSRfg(p_FSR); + assertInvariants(); return *GenericSequentialSolver(p_FSR).jointFactorGraph(keys()); } @@ -400,6 +421,7 @@ namespace gtsam { // Calculate the marginal vector keys12vector; keys12vector.reserve(keys12.size()); keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end()); + assertInvariants(); return *GenericSequentialSolver(joint).jointFactorGraph(keys12vector); } @@ -429,6 +451,7 @@ namespace gtsam { new_clique->parent_ = parent_clique; parent_clique->children_.push_back(new_clique); } + new_clique->assertInvariants(); return new_clique; } @@ -443,6 +466,7 @@ namespace gtsam { new_clique->children_ = child_cliques; BOOST_FOREACH(sharedClique& child, child_cliques) child->parent_ = new_clique; + new_clique->assertInvariants(); return new_clique; } @@ -477,6 +501,7 @@ namespace gtsam { nodes_[key] = clique; clique->push_front(conditional); if(debug) clique->print("Expanded clique is "); + clique->assertInvariants(); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 372afb566..4ce0fd735 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -53,6 +53,10 @@ namespace gtsam { */ struct Clique: public BayesNet { + protected: + void assertInvariants() const; + + public: typedef typename boost::shared_ptr shared_ptr; typedef typename boost::weak_ptr weak_ptr; weak_ptr parent_; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6f064f41c..bff931050 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -61,8 +61,8 @@ namespace gtsam { /* ************************************************************************* */ HessianFactor::HessianFactor(const Vector& b_in) : info_(matrix_) { JacobianFactor jf(b_in); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); info_.copyStructureFrom(jf.Ab_); + ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); } /* ************************************************************************* */ @@ -70,8 +70,8 @@ namespace gtsam { const Vector& b, const SharedDiagonal& model) : GaussianFactor(i1), info_(matrix_) { JacobianFactor jf(i1, A1, b, model); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); info_.copyStructureFrom(jf.Ab_); + ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); } /* ************************************************************************* */ @@ -79,8 +79,8 @@ namespace gtsam { const Vector& b, const SharedDiagonal& model) : GaussianFactor(i1,i2), info_(matrix_) { JacobianFactor jf(i1, A1, i2, A2, b, model); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); info_.copyStructureFrom(jf.Ab_); + ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); } /* ************************************************************************* */ @@ -88,8 +88,8 @@ namespace gtsam { Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) : GaussianFactor(i1,i2,i3), info_(matrix_) { JacobianFactor jf(i1, A1, i2, A2, i3, A3, b, model); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); info_.copyStructureFrom(jf.Ab_); + ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); } /* ************************************************************************* */ @@ -97,8 +97,8 @@ namespace gtsam { const Vector &b, const SharedDiagonal& model) : info_(matrix_) { JacobianFactor jf(terms, b, model); keys_ = jf.keys_; - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); info_.copyStructureFrom(jf.Ab_); + ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); } /* ************************************************************************* */ @@ -106,16 +106,16 @@ namespace gtsam { const Vector &b, const SharedDiagonal& model) : info_(matrix_) { JacobianFactor jf(terms, b, model); keys_ = jf.keys_; - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); info_.copyStructureFrom(jf.Ab_); + ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); } /* ************************************************************************* */ HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) { JacobianFactor jf(cg); + info_.copyStructureFrom(jf.Ab_); ublas::noalias(ublas::symmetric_adaptor(matrix_)) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); - info_.copyStructureFrom(jf.Ab_); } /* ************************************************************************* */ @@ -126,8 +126,8 @@ namespace gtsam { if(dynamic_cast(&gf)) { const JacobianFactor& jf(static_cast(gf)); JacobianFactor whitened(jf.whiten()); - matrix_ = ublas::prod(ublas::trans(whitened.matrix_), whitened.matrix_); info_.copyStructureFrom(whitened.Ab_); + matrix_ = ublas::prod(ublas::trans(whitened.matrix_), whitened.matrix_); } else if(dynamic_cast(&gf)) { const HessianFactor& hf(static_cast(gf)); info_.assignNoalias(hf.info_); @@ -289,9 +289,22 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2]; for(size_t j1=0; j1<=j2; ++j1) { size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; - if(debug) - cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - ublas::noalias(this->info_(slot1, slot2)) += update.info_(j1,j2); + if(slot2 > slot1) { + if(debug) + cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + ublas::noalias(this->info_(slot1, slot2)) += update.info_(j1,j2); + } else if(slot1 > slot2) { + if(debug) + cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; + ublas::noalias(this->info_(slot2, slot1)) += ublas::trans(update.info_(j1,j2)); + } else { + if(debug) + cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + Block thisBlock(this->info_(slot2, slot1)); + constBlock updateBlock(update.info_(j1,j2)); + ublas::noalias(ublas::symmetric_adaptor(thisBlock)) += + ublas::symmetric_adaptor(updateBlock); + } } } toc(2, "update"); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 33de4bd0e..69ca0f92b 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -205,6 +205,85 @@ TEST(GaussianFactor, eliminate2 ) EXPECT(assert_equal(HessianFactor(expectedLF), *actual_Chol.second, 1.5e-3)); } +/* ************************************************************************* */ +TEST_UNSAFE(GaussianFactor, eliminateUnsorted) { + JacobianFactor::shared_ptr factor1( + new JacobianFactor(0, + Matrix_(3,3, + 44.7214, 0.0, 0.0, + 0.0, 44.7214, 0.0, + 0.0, 0.0, 44.7214), + 1, + Matrix_(3,3, + -0.179168, -44.721, 0.717294, + 44.721, -0.179168, -44.9138, + 0.0, 0.0, -44.7214), + Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17), + noiseModel::Unit::Create(3))); + HessianFactor::shared_ptr unsorted_factor2( + new HessianFactor(0, + Matrix_(6,3, + 25.8367, 0.1211, 0.0593, + 0.0, 23.4099, 30.8733, + 0.0, 0.0, 25.8729, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0), + 1, + Matrix_(6,3, + 25.7429, -1.6897, 0.4587, + 1.6400, 23.3095, -8.4816, + 0.0034, 0.0509, -25.7855, + 0.9997, -0.0002, 0.0824, + 0.0, 0.9973, 0.9517, + 0.0, 0.0, 0.9973), + Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + noiseModel::Unit::Create(6))); + Permutation permutation(2); + permutation[0] = 1; + permutation[1] = 0; + unsorted_factor2->permuteWithInverse(permutation); + + HessianFactor::shared_ptr sorted_factor2( + new HessianFactor(0, + Matrix_(6,3, + 25.7429, -1.6897, 0.4587, + 1.6400, 23.3095, -8.4816, + 0.0034, 0.0509, -25.7855, + 0.9997, -0.0002, 0.0824, + 0.0, 0.9973, 0.9517, + 0.0, 0.0, 0.9973), + 1, + Matrix_(6,3, + 25.8367, 0.1211, 0.0593, + 0.0, 23.4099, 30.8733, + 0.0, 0.0, 25.8729, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0), + Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), + noiseModel::Unit::Create(6))); + + GaussianFactorGraph sortedGraph; +// sortedGraph.push_back(factor1); + sortedGraph.push_back(sorted_factor2); + + GaussianFactorGraph unsortedGraph; +// unsortedGraph.push_back(factor1); + unsortedGraph.push_back(unsorted_factor2); + + GaussianBayesNet::shared_ptr expected_bn; + GaussianFactor::shared_ptr expected_factor; + boost::tie(expected_bn, expected_factor) = GaussianFactor::CombineAndEliminate(sortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY); + + GaussianBayesNet::shared_ptr actual_bn; + GaussianFactor::shared_ptr actual_factor; + boost::tie(actual_bn, actual_factor) = GaussianFactor::CombineAndEliminate(unsortedGraph, 1, GaussianFactor::SOLVE_PREFER_CHOLESKY); + + CHECK(assert_equal(*expected_bn, *actual_bn, 1e-10)); + CHECK(assert_equal(*expected_factor, *actual_factor, 1e-10)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */