From 60d5feb5cf6f86ddb7e796f0dbb180e98dacf77d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 29 Jul 2013 23:56:04 +0000 Subject: [PATCH] Working on ISAM --- gtsam/inference/BayesTree.h | 20 +++ gtsam/inference/ISAM-inst.h | 63 +++++++++ gtsam/inference/ISAM.h | 69 +++++++++ gtsam/inference/JunctionTree-inst.h | 14 +- .../{testISAM.cpp => testISAMObsolete.cpp} | 0 gtsam/symbolic/SymbolicISAM.cpp | 31 ++++ gtsam/symbolic/SymbolicISAM.h | 46 ++++++ .../symbolic/tests/testSymbolicBayesTree.cpp | 22 +++ gtsam/symbolic/tests/testSymbolicISAM.cpp | 132 ++++++++++++++++++ 9 files changed, 395 insertions(+), 2 deletions(-) create mode 100644 gtsam/inference/ISAM-inst.h create mode 100644 gtsam/inference/ISAM.h rename gtsam/inference/tests/{testISAM.cpp => testISAMObsolete.cpp} (100%) create mode 100644 gtsam/symbolic/SymbolicISAM.cpp create mode 100644 gtsam/symbolic/SymbolicISAM.h create mode 100644 gtsam/symbolic/tests/testSymbolicISAM.cpp diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index e6e0e4482..541e49469 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -31,6 +31,7 @@ namespace gtsam { // Forward declarations template class FactorGraph; + /* ************************************************************************* */ /** * Bayes tree * @tparam CONDITIONAL The type of the conditional densities, i.e. the type of node in the underlying Bayes chain, @@ -256,4 +257,23 @@ namespace gtsam { }; // BayesTree + /* ************************************************************************* */ + template + class BayesTreeOrphanWrapper : public CLIQUE::FactorType + { + public: + typedef CLIQUE CliqueType; + typedef typename CLIQUE::FactorType Base; + + boost::shared_ptr clique; + + BayesTreeOrphanWrapper(const boost::shared_ptr& clique) : + clique(clique) + { + // Store parent keys in our base type factor so that eliminating those parent keys will pull + // this subtree into the elimination. + keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents()); + } + }; + } /// namespace gtsam diff --git a/gtsam/inference/ISAM-inst.h b/gtsam/inference/ISAM-inst.h new file mode 100644 index 000000000..5849e02ad --- /dev/null +++ b/gtsam/inference/ISAM-inst.h @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM-inl.h + * @brief Incremental update functionality (iSAM) for BayesTree. + * @author Michael Kaess + */ + +#pragma once + +#include + +namespace gtsam { + + /* ************************************************************************* */ + template + void ISAM::update_internal(const FactorGraphType& newFactors, Cliques& orphans, const Eliminate& function) + { + // Remove the contaminated part of the Bayes tree + // Throw exception if disconnected + BayesNetType bn; + if (!this->empty()) { + const FastSet newFactorKeys = newFactors.keys(); + this->removeTop(std::vector(newFactorKeys.begin(), newFactorKeys.end()), bn, orphans); + if (bn.empty()) + throw std::runtime_error( + "ISAM::update_internal(): no variables in common between existing Bayes tree and incoming factors!"); + } + + // Add the removed top and the new factors + FactorGraphType factors; + factors += bn; + factors += newFactors; + + // Add the orphaned subtrees + BOOST_FOREACH(const sharedClique& orphan, orphans) + factors += boost::make_shared >(orphan); + + // eliminate into a Bayes net + Base bayesTree = *factors.eliminateMultifrontal(boost::none, function); + this->roots_.insert(this->roots_.end(), bayesTree.roots().begin(), bayesTree.roots().end()); + this->nodes_.insert(bayesTree.nodes().begin(), bayesTree.nodes().end()); + } + + /* ************************************************************************* */ + template + void ISAM::update(const FactorGraphType& newFactors, const Eliminate& function) + { + Cliques orphans; + this->update_internal(newFactors, orphans, function); + } + +} +/// namespace gtsam diff --git a/gtsam/inference/ISAM.h b/gtsam/inference/ISAM.h new file mode 100644 index 000000000..e6837884b --- /dev/null +++ b/gtsam/inference/ISAM.h @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ISAM.h + * @brief Incremental update functionality (iSAM) for BayesTree. + * @author Michael Kaess + */ + +// \callgraph +#pragma once + +namespace gtsam { + + /** + * A Bayes tree with an update methods that implements the iSAM algorithm. + * Given a set of new factors, it re-eliminates the invalidated part of the tree. + * \nosubgrouping + */ + template + class ISAM: public BAYESTREE + { + private: + + typedef BAYESTREE Base; + typedef typename Base::FactorGraphType FactorGraphType; + typedef typename Base::Cliques Cliques; + typedef typename Base::Eliminate Eliminate; + typedef typename Base::EliminationTraits EliminationTraits; + + public: + + /// @name Standard Constructors + /// @{ + + /** Create an empty Bayes Tree */ + ISAM() {} + + /** Copy constructor */ + ISAM(const Base& bayesTree) : Base(bayesTree) {} + + /// @} + /// @name Advanced Interface Interface + /// @{ + + /** + * update the Bayes tree with a set of new factors, typically derived from measurements + * @param newFactors is a factor graph that contains the new factors + * @param function an elimination routine + */ + void update(const FactorGraphType& newFactors, const Eliminate& function = EliminationTraits::DefaultEliminate); + + /** update_internal provides access to list of orphans for drawing purposes */ + void update_internal(const FactorGraphType& newFactors, Cliques& orphans, + const Eliminate& function = EliminationTraits::DefaultEliminate); + + /// @} + + }; + +}/// namespace gtsam diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index 4e24aaac1..16d37f639 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -177,8 +177,18 @@ namespace gtsam { // Gather factors FactorGraphType gatheredFactors; gatheredFactors.reserve(node->factors.size() + node->children.size()); - gatheredFactors.push_back(node->factors.begin(), node->factors.end()); - gatheredFactors.push_back(myData.childFactors.begin(), myData.childFactors.end()); + gatheredFactors += node->factors; + gatheredFactors += myData.childFactors; + + // Check for Bayes tree orphan subtrees, and add them to our children + BOOST_FOREACH(const sharedFactor& f, node->factors) + { + if(const BayesTreeOrphanWrapper* asSubtree = dynamic_cast*>(f.get())) + { + myData.bayesTreeNode->children.push_back(asSubtree->clique); + asSubtree->clique->parent_ = myData.bayesTreeNode; + } + } // Do dense elimination step std::pair, boost::shared_ptr > eliminationResult = diff --git a/gtsam/inference/tests/testISAM.cpp b/gtsam/inference/tests/testISAMObsolete.cpp similarity index 100% rename from gtsam/inference/tests/testISAM.cpp rename to gtsam/inference/tests/testISAMObsolete.cpp diff --git a/gtsam/symbolic/SymbolicISAM.cpp b/gtsam/symbolic/SymbolicISAM.cpp new file mode 100644 index 000000000..f53b96754 --- /dev/null +++ b/gtsam/symbolic/SymbolicISAM.cpp @@ -0,0 +1,31 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicISAM.cpp + * @date July 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include +#include + +namespace gtsam { + + /* ************************************************************************* */ + SymbolicISAM::SymbolicISAM() {} + + /* ************************************************************************* */ + SymbolicISAM::SymbolicISAM(const SymbolicBayesTree& bayesTree) : + Base(bayesTree) {} + +} diff --git a/gtsam/symbolic/SymbolicISAM.h b/gtsam/symbolic/SymbolicISAM.h new file mode 100644 index 000000000..3f85facbf --- /dev/null +++ b/gtsam/symbolic/SymbolicISAM.h @@ -0,0 +1,46 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SymbolicISAM.h + * @date July 29, 2013 + * @author Frank Dellaert + * @author Richard Roberts + */ + +#pragma once + +#include +#include + +namespace gtsam { + + class GTSAM_EXPORT SymbolicISAM : public ISAM + { + public: + typedef ISAM Base; + typedef SymbolicISAM This; + typedef boost::shared_ptr shared_ptr; + + /// @name Standard Constructors + /// @{ + + /** Create an empty Bayes Tree */ + SymbolicISAM(); + + /** Copy constructor */ + SymbolicISAM(const SymbolicBayesTree& bayesTree); + + /// @} + + }; + +} diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 413696246..3d3116e27 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -341,6 +341,28 @@ TEST( BayesTreeOrdered, removeTop4 ) EXPECT(orphans.empty()); } +/* ************************************************************************* */ +TEST( BayesTreeOrdered, removeTop5 ) +{ + // Remove top called with variables that are not in the Bayes tree + SymbolicFactorGraph graph = list_of + (SymbolicFactor(L(5))) + (SymbolicFactor(X(4), L(5))) + (SymbolicFactor(X(2), X(4))) + (SymbolicFactor(X(3), X(2))); + SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal( + Ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) )); + + // Remove nonexistant + SymbolicBayesNet bn; + SymbolicBayesTree::Cliques orphans; + bayesTree.removeTop(list_of(X(10)), bn, orphans); + + SymbolicBayesNet expectedBn; + EXPECT(assert_equal(expectedBn, bn)); + EXPECT(orphans.empty()); +} + /* ************************************************************************* */ TEST( SymbolicBayesTree, thinTree ) { diff --git a/gtsam/symbolic/tests/testSymbolicISAM.cpp b/gtsam/symbolic/tests/testSymbolicISAM.cpp new file mode 100644 index 000000000..52dfbacb8 --- /dev/null +++ b/gtsam/symbolic/tests/testSymbolicISAM.cpp @@ -0,0 +1,132 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testISAM.cpp + * @brief Unit tests for ISAM + * @author Michael Kaess + */ + +#include +#include // for operator += +using namespace boost::assign; + +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Some numbers that should be consistent among all smoother tests + +//static double sigmax1 = 0.786153, sigmax2 = 0.687131 ,sigmax3 = 0.671512, +//sigmax4 = 0.669534, sigmax5 = sigmax3,, sigmax7 = sigmax1, sigmax6 = sigmax2; + +/* ************************************************************************* */ + +//// SLAM example from RSS sqrtSAM paper +//SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")), +// x2(new SymbolicConditional("x2","x3")), +// x1(new SymbolicConditional("x1","x2","x3")), +// l1(new SymbolicConditional("l1","x1","x2")), +// l2(new SymbolicConditional("l2","x1","x3")); +// +//// ISAM for sqrtSAM example +//SymbolicISAM createSlamSymbolicISAM(){ +// // Create using insert +// SymbolicISAM bayesTree_slam; +// bayesTree_slam.insert(x3); +// bayesTree_slam.insert(x2); +// bayesTree_slam.insert(x1); +// bayesTree_slam.insert(l2); +// bayesTree_slam.insert(l1); +// return bayesTree_slam; +//} + +/* ************************************************************************* */ + +//// Conditionals for ASIA example from the tutorial with A and D evidence +//SymbolicConditional::shared_ptr +// B(new SymbolicConditional("B")), +// L(new SymbolicConditional("L", "B")), +// E(new SymbolicConditional("E", "B", "L")), +// S(new SymbolicConditional("S", "L", "B")), +// T(new SymbolicConditional("T", "E", "L")), +// X(new SymbolicConditional("X", "E")); +// +//// ISAM for Asia example +//SymbolicISAM createAsiaSymbolicISAM() { +// SymbolicISAM bayesTree; +// bayesTree.insert(B); +// bayesTree.insert(L); +// bayesTree.insert(E); +// bayesTree.insert(S); +// bayesTree.insert(T); +// bayesTree.insert(X); +// return bayesTree; +//} + +/* ************************************************************************* */ +TEST( SymbolicISAM, iSAM ) +{ + // Now we modify the Bayes tree by inserting a new factor over B and S + + SymbolicFactorGraph fullGraph; + fullGraph += asiaGraph; + fullGraph += SymbolicFactor(_B_, _S_); + + // This ordering is chosen to match the one chosen by COLAMD during the ISAM update + SymbolicBayesTree expected = *fullGraph.eliminateMultifrontal(Ordering(list_of(_X_)(_B_)(_S_)(_E_)(_L_)(_T_))); + + // Add factor on B and S + SymbolicISAM actual = *asiaGraph.eliminateMultifrontal(); + + // Check whether the same + EXPECT(assert_equal(expected, (const SymbolicBayesTree&)actual)); +} + +/* ************************************************************************* */ +//TEST( ISAM, iSAM_slam ) +//{ +// // Create using insert +// SymbolicISAM bayesTree_slam = createSlamSymbolicISAM(); +// +// //New conditionals for the expected Bayes tree +// SymbolicConditional::shared_ptr +// l1_(new SymbolicConditional("l1","x1","x2","x3")); +// +// // Create expected Bayes tree +// SymbolicISAM expected_slam; +// expected_slam.insert(x3); +// expected_slam.insert(x2); +// expected_slam.insert(x1); +// expected_slam.insert(l1_); +// expected_slam.insert(l2); +// +// +// // create new factors to be inserted +// SymbolicFactorGraph factorGraph_slam; +// factorGraph_slam.push_factor("x3","l1"); +// factorGraph_slam.push_factor("x3"); +// +// // do incremental inference +// bayesTree_slam.update(factorGraph_slam); +// +// // Check whether the same +// CHECK(assert_equal(expected_slam,bayesTree_slam)); +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */