Working on ISAM

gtsam/inference/BayesTree.h

@@ -31,6 +31,7 @@ namespace gtsam {
   // Forward declarations
   template<class FACTOR> 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 CLIQUE>
+  class BayesTreeOrphanWrapper : public CLIQUE::FactorType
+  {
+  public:
+    typedef CLIQUE CliqueType;
+    typedef typename CLIQUE::FactorType Base;
+
+    boost::shared_ptr<CliqueType> clique;
+
+    BayesTreeOrphanWrapper(const boost::shared_ptr<CliqueType>& 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

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 <gtsam/inference/ISAM.h>
+
+namespace gtsam {
+
+  /* ************************************************************************* */
+  template<class BAYESTREE>
+  void ISAM<BAYESTREE>::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<Key> newFactorKeys = newFactors.keys();
+      this->removeTop(std::vector<Key>(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<BayesTreeOrphanWrapper<Clique> >(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<class BAYESTREE>
+  void ISAM<BAYESTREE>::update(const FactorGraphType& newFactors, const Eliminate& function)
+  {
+    Cliques orphans;
+    this->update_internal(newFactors, orphans, function);
+  }
+
+}
+/// namespace gtsam

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 BAYESTREE>
+  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

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 += node->factors;
-      gatheredFactors.push_back(myData.childFactors.begin(), myData.childFactors.end());
+      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<BTNode>* asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get()))
+        {
+          myData.bayesTreeNode->children.push_back(asSubtree->clique);
+          asSubtree->clique->parent_ = myData.bayesTreeNode;
+        }
+      }
 
       // Do dense elimination step
       std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =

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 <gtsam/symbolic/SymbolicISAM.h>
+#include <gtsam/inference/ISAM-inst.h>
+
+namespace gtsam {
+
+  /* ************************************************************************* */
+  SymbolicISAM::SymbolicISAM() {}
+
+  /* ************************************************************************* */
+  SymbolicISAM::SymbolicISAM(const SymbolicBayesTree& bayesTree) :
+    Base(bayesTree) {}
+
+}

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 <gtsam/symbolic/SymbolicBayesTree.h>
+#include <gtsam/inference/ISAM.h>
+
+namespace gtsam {
+
+  class GTSAM_EXPORT SymbolicISAM : public ISAM<SymbolicBayesTree>
+  {
+  public:
+    typedef ISAM<SymbolicBayesTree> Base;
+    typedef SymbolicISAM This;
+    typedef boost::shared_ptr<This> shared_ptr;
+
+    /// @name Standard Constructors
+    /// @{
+
+    /** Create an empty Bayes Tree */
+    SymbolicISAM();
+
+    /** Copy constructor */
+    SymbolicISAM(const SymbolicBayesTree& bayesTree);
+
+    /// @}
+
+  };
+
+}

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 ) {
 

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 <boost/foreach.hpp>
+#include <boost/assign/std/list.hpp> // for operator +=
+using namespace boost::assign;
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/symbolic/SymbolicISAM.h>
+#include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
+
+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);}
+/* ************************************************************************* */