First pass at marginalization of leaves in iSAM2 - passes unit tests but needs to be tested on real data

gtsam/nonlinear/ISAM2-impl.cpp

@@ -65,7 +65,7 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
                                   Values& theta, VariableIndex& variableIndex,
                                   VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd,
                                   std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes,
-                                  GaussianFactorGraph& linearFactors) {
+                                  GaussianFactorGraph& linearFactors, FastSet<Key>& fixedVariables) {
 
      // Get indices of unused keys
      vector<Index> unusedIndices;  unusedIndices.reserve(unusedKeys.size());
@@ -113,12 +113,13 @@ void ISAM2::Impl::RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Cli
        nodes.swap(newNodes);
      }
 
-     // Reorder and remove from ordering and solution
+     // Reorder and remove from ordering, solution, and fixed keys
      ordering.permuteInPlace(unusedToEnd);
      BOOST_REVERSE_FOREACH(Key key, unusedKeys) {
        Ordering::value_type removed = ordering.pop_back();
        assert(removed.first == key);
        theta.erase(key);
+       fixedVariables.erase(key);
      }
 
      // Finally, permute references to variables

gtsam/nonlinear/ISAM2-impl.h

@@ -57,7 +57,7 @@ struct ISAM2::Impl {
   static void RemoveVariables(const FastSet<Key>& unusedKeys, const ISAM2Clique::shared_ptr& root,
     Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton,
     VectorValues& RgProd, std::vector<bool>& replacedKeys, Ordering& ordering, Base::Nodes& nodes,
-    GaussianFactorGraph& linearFactors);
+    GaussianFactorGraph& linearFactors, FastSet<Key>& fixedVariables);
 
   /**
    * Extract the set of variable indices from a NonlinearFactorGraph.  For each Symbol

gtsam/nonlinear/ISAM2.cpp

@@ -31,7 +31,8 @@ using namespace boost::assign;
 
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>
-
+#include <gtsam/nonlinear/nonlinearExceptions.h>
+#include <gtsam/nonlinear/LinearContainerFactor.h>
 
 namespace gtsam {
 
@@ -673,6 +674,9 @@ ISAM2Result ISAM2::update(
     if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging
 
     // Remove from relinKeys any keys whose linearization points are fixed
+    BOOST_FOREACH(Key key, fixedVariables_) {
+      relinKeys.erase(ordering_[key]);
+    }
     if(noRelinKeys) {
       BOOST_FOREACH(Key key, *noRelinKeys) {
         relinKeys.erase(ordering_[key]);
@@ -765,7 +769,7 @@ ISAM2Result ISAM2::update(
   if(!unusedKeys.empty()) {
     gttic(remove_variables);
     Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
-        deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_);
+        deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_, fixedVariables_);
     gttoc(remove_variables);
   }
   result.cliques = this->nodes().size();
@@ -780,6 +784,134 @@ ISAM2Result ISAM2::update(
   return result;
 }
 
+/* ************************************************************************* */
+void ISAM2::experimentalMarginalizeLeaves(const FastList<Key>& leafKeys)
+{
+  // Convert set of keys into a set of indices
+  FastSet<Index> indices;
+  BOOST_FOREACH(Key key, leafKeys) {
+    indices.insert(ordering_[key]);
+  }
+
+  // For each clique containing variables to be marginalized, we need to
+  // reeliminate the marginalized variables and add their linear contribution
+  // factor into the factor graph.  This results in some extra work if we are
+  // marginalizing multiple cliques on the same path.
+
+  // To do this, first build a map containing all cliques containing variables to be
+  // marginalized that determines the last-ordered variable eliminated in that
+  // clique.
+  FastMap<sharedClique, Index> cliquesToMarginalize;
+  BOOST_FOREACH(Index jI, indices)
+  {
+    assert(nodes_[jI]); // Assert that the nodes structure contains this index
+    pair<FastMap<sharedClique,Index>::iterator, bool> insertResult =
+      cliquesToMarginalize.insert(make_pair(nodes_[jI], jI));
+    // If insert found a map entry existing, see if our current index is
+    // eliminated later and modify the entry if so.
+    if(!insertResult.second && jI > insertResult.first->second)
+      insertResult.first->second = jI;
+  }
+
+#ifndef NDEBUG
+  // Check that the cliques have sorted frontal keys - we assume that here.
+  for(FastMap<sharedClique,Index>::const_iterator c = cliquesToMarginalize.begin();
+    c != cliquesToMarginalize.end(); ++c)
+  {
+    FastSet<Key> keys(c->first->conditional()->beginFrontals(), c->first->conditional()->endFrontals());
+    assert(std::equal(c->first->conditional()->beginFrontals(), c->first->conditional()->endFrontals(), keys.begin()));
+  }
+#endif
+
+  // Now loop over the indices
+  FastSet<size_t> factorIndicesToRemove;
+  GaussianFactorGraph factorsToAdd;
+  for(FastSet<Index>::iterator jI = indices.begin(); jI != indices.end(); )
+  {
+    const Index j = *jI;
+
+    // Retrieve the clique and the latest-ordered index corresponding to this index
+    FastMap<sharedClique,Index>::iterator clique_lastIndex = cliquesToMarginalize.find(nodes_[*jI]);
+    assert(clique_lastIndex != cliquesToMarginalize.end()); // Assert that we indexed the clique
+
+    // Check that the clique has no children
+    if(!clique_lastIndex->first->children().empty())
+      throw MarginalizeNonleafException(ordering_.key(*jI), params_.keyFormatter);
+
+    // Mark factors to be removed
+    BOOST_FOREACH(size_t i, variableIndex_[*jI]) {
+      factorIndicesToRemove.insert(i);
+    }
+
+    // Check that all previous variables in the clique are also being eliminated and no later ones.
+    // At the same time, remove the indices marginalized with this clique from the indices set.
+    // This is where the iterator j is advanced.
+    size_t nFrontals = 0;
+    {
+      bool foundLast = false;
+      BOOST_FOREACH(Index cliqueVar, *clique_lastIndex->first->conditional()) {
+        if(!foundLast && indices.find(cliqueVar) == indices.end())
+          throw MarginalizeNonleafException(ordering_.key(j), params_.keyFormatter);
+        if(foundLast && indices.find(cliqueVar) != indices.end())
+          throw MarginalizeNonleafException(ordering_.key(j), params_.keyFormatter);
+        if(!foundLast) {
+          ++ nFrontals;
+          if(cliqueVar == *jI)
+            indices.erase(jI++); // j gets advanced here
+          else
+            indices.erase(cliqueVar);
+        }
+        if(cliqueVar == clique_lastIndex->second)
+          foundLast = true;
+      }
+    }
+
+    // Reeliminate the factored conditional + marginal that was previously making up the clique
+    GaussianFactorGraph cliqueGraph;
+    cliqueGraph.push_back(clique_lastIndex->first->conditional()->toFactor());
+    assert(clique_lastIndex->first->cachedFactor()); // Assert that we have the cached marginal piece
+    cliqueGraph.push_back(clique_lastIndex->first->cachedFactor());
+    pair<Clique::sharedConditional, GaussianFactor::shared_ptr> eliminationResult =
+      params_.factorization==ISAM2Params::QR ?
+      EliminateQR(cliqueGraph, nFrontals) :
+      EliminatePreferCholesky(cliqueGraph, nFrontals);
+
+    // Add the marginal into the factor graph
+    factorsToAdd.push_back(eliminationResult.second);
+
+    // Remove the clique
+    this->removeClique(clique_lastIndex->first);
+  }
+
+  // Remove any factors touching the marginalized-out variables
+  vector<size_t> removedFactorIndices;
+  SymbolicFactorGraph removedFactors;
+  BOOST_FOREACH(size_t i, factorIndicesToRemove) {
+    removedFactorIndices.push_back(i);
+    removedFactors.push_back(nonlinearFactors_[i]->symbolic(ordering_));
+    nonlinearFactors_.remove(i);
+    if(params_.cacheLinearizedFactors)
+      linearFactors_.remove(i);
+  }
+
+  // Add the new factors and fix linearization points of involved variables
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factorsToAdd) {
+    nonlinearFactors_.push_back(boost::make_shared<LinearContainerFactor>(
+      factor, ordering_));
+    if(params_.cacheLinearizedFactors)
+      linearFactors_.push_back(factor);
+    BOOST_FOREACH(Index factorIndex, *factor) {
+      fixedVariables_.insert(ordering_.key(factorIndex));
+    }
+  }
+  variableIndex_.augment(factorsToAdd); // Augment the variable index
+
+  // Remove the marginalized variables
+  variableIndex_.remove(removedFactorIndices, removedFactors);
+  Impl::RemoveVariables(FastSet<Key>(leafKeys.begin(), leafKeys.end()), root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_,
+    deltaReplacedMask_, ordering_, nodes_, linearFactors_, fixedVariables_);
+}
+
 /* ************************************************************************* */
 void ISAM2::updateDelta(bool forceFullSolve) const {
 

gtsam/nonlinear/ISAM2.h

@@ -494,6 +494,10 @@ protected:
   /** The current Dogleg Delta (trust region radius) */
   mutable boost::optional<double> doglegDelta_;
 
+  /** Set of variables that are involved with linear factors from marginalized
+   * variables and thus cannot have their linearization points changed. */
+  FastSet<Key> fixedVariables_;
+
 public:
 
   typedef ISAM2 This; ///< This class
@@ -545,6 +549,8 @@ public:
       const boost::optional<FastList<Key> >& noRelinKeys = boost::none,
       bool force_relinearize = false);
 
+  void experimentalMarginalizeLeaves(const FastList<Key>& leafKeys);
+
   /** Access the current linearization point */
   const Values& getLinearizationPoint() const { return theta_; }
 

gtsam/nonlinear/nonlinearExceptions.h

@@ -0,0 +1,52 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    nonlinearExceptions.h
+ * @brief   Exceptions that may be thrown by nonlinear optimization components
+ * @author  Richard Roberts
+ * @date    Aug 17, 2012
+ */
+#pragma once
+
+#include <gtsam/nonlinear/Key.h>
+#include <boost/lexical_cast.hpp>
+#include <exception>
+
+namespace gtsam {
+
+  /**
+  Thrown when requesting to marginalize out variables from ISAM2 that are not
+  leaves.  To make the variables you would like to marginalize be leaves, their
+  ordering should be constrained using the constrainedKeys argument to
+  ISAM2::update().
+  */
+  class MarginalizeNonleafException : public std::exception {
+    Key key_;
+    KeyFormatter formatter_;
+    mutable std::string what_;
+  public:
+    MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) throw() :
+      key_(key), formatter_(formatter) {}
+    virtual ~MarginalizeNonleafException() throw() {}
+    Key key() const { return key_; }
+    virtual const char* what() const throw() {
+      if(what_.empty())
+        what_ = 
+"\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\
+is not a leaf.  To make the variables you would like to marginalize be leaves,\n\
+their ordering should be constrained using the constrainedKeys argument to\n\
+ISAM2::update().\n";
+      return what_.c_str();
+    }
+  };
+
+}

tests/testGaussianISAM2.cpp

@@ -286,8 +286,9 @@ TEST_UNSAFE(ISAM2, ImplRemoveVariables) {
   SymbolicFactorGraph removedFactors; removedFactors.push_back(sfg[1]);
   variableIndex.remove(removedFactorsI, removedFactors);
   GaussianFactorGraph linearFactors;
+  FastSet<Key> fixedVariables;
   ISAM2::Impl::RemoveVariables(unusedKeys, ISAM2::sharedClique(), theta, variableIndex, delta, deltaNewton, deltaRg,
-    replacedKeys, ordering, nodes, linearFactors);
+    replacedKeys, ordering, nodes, linearFactors, fixedVariables);
 
   EXPECT(assert_equal(thetaExpected, theta));
   EXPECT(assert_equal(variableIndexExpected, variableIndex));
@@ -820,7 +821,6 @@ TEST(ISAM2, constrained_ordering)
 /* ************************************************************************* */
 TEST(ISAM2, slamlike_solution_partial_relinearization_check)
 {
-
   // These variables will be reused and accumulate factors and values
   Values fullinit;
   NonlinearFactorGraph fullgraph;
@@ -832,6 +832,43 @@ TEST(ISAM2, slamlike_solution_partial_relinearization_check)
   CHECK(isam_check(fullgraph, fullinit, isam, *this, result_));
 }
 
+/* ************************************************************************* */
+TEST_UNSAFE(ISAM2, marginalizeLeaves)
+{
+  // Create isam2
+  ISAM2 isam = createSlamlikeISAM2();
+  
+  // Get linearization point
+  Values soln = isam.calculateBestEstimate();
+
+  // Calculate expected marginal
+  GaussianFactorGraph isamAsGraph(isam);
+  GaussianSequentialSolver solver(isamAsGraph);
+  vector<Index> toKeep;
+  const Index lastVar = isam.getOrdering().size() - 1;
+  for(Index i=0; i<=lastVar; ++i)
+    if(i != isam.getOrdering()[0])
+      toKeep.push_back(i);
+  GaussianFactorGraph marginalgfg = *solver.jointFactorGraph(toKeep);
+  vector<Index> toFrontI;
+  toFrontI.push_back(isam.getOrdering()[0]);
+  Permutation toFront = Permutation::PullToFront(toFrontI, lastVar+1);
+  marginalgfg.permuteWithInverse(*toFront.inverse());
+  Matrix expectedAugmentedHessian = marginalgfg.augmentedHessian();
+
+  // Marginalize
+  FastList<Key> marginalizeKeys;
+  marginalizeKeys.push_back(isam.getOrdering().key(0));
+  isam.experimentalMarginalizeLeaves(marginalizeKeys);
+
+  // Check
+  GaussianFactorGraph actualMarginalGraph(isam);
+  Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian();
+
+  LONGS_EQUAL(lastVar-1, isam.getOrdering().size()-1);
+  EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */