Fixed problem in ISAM2 marginalizeLeaves (with Luca, port from other repo)

gtsam/inference/Conditional.h

@@ -116,6 +116,9 @@ namespace gtsam {
     /// @name Advanced Interface
     /// @{
 
+    /** Mutable version of nrFrontals */
+    size_t& nrFrontals() { return nrFrontals_; }
+
     /** Mutable iterator pointing to first frontal key. */
     typename FACTOR::iterator beginFrontals() { return asFactor().begin(); }
 

gtsam/inference/JunctionTree-inst.h

@@ -104,12 +104,12 @@ namespace gtsam {
           myData.myJTNode->keys.insert(myData.myJTNode->keys.begin(), childToMerge.keys.begin(), childToMerge.keys.end());
           myData.myJTNode->factors.insert(myData.myJTNode->factors.end(), childToMerge.factors.begin(), childToMerge.factors.end());
           myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end());
+          // Increment problem size
+          combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_);
           // Remove child from list.
           myData.myJTNode->children.erase(myData.myJTNode->children.begin() + (child - nrMergedChildren));
           // Increment number of merged children
           ++ nrMergedChildren;
-          // Increment problem size
-          combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_);
         }
       }
       myData.myJTNode->problemSize_ = combinedProblemSize;

gtsam/inference/VariableIndex-inl.h

@@ -75,8 +75,10 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG&
 template<typename ITERATOR>
 void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
   for(ITERATOR key = firstKey; key != lastKey; ++key) {
-    assert(internalAt(*key).empty());
+    KeyMap::iterator entry = index_.find(*key);
-    index_.erase(*key);
+    if(!entry->second.empty())
+      throw std::invalid_argument("Asking to remove variables from the variable index that are not unused");
+    index_.erase(entry);
   }
 }
 

gtsam/linear/JacobianFactor.cpp

@@ -134,6 +134,8 @@ namespace gtsam {
     model_ = noiseModel::Unit::Create(maxrank);
   }
 
+#undef GTSAM_EXTRA_CONSISTENCY_CHECKS
+
   /* ************************************************************************* */
   // Helper functions for combine constructor
   namespace {
@@ -144,43 +146,62 @@ namespace gtsam {
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
       FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
 #else
-      FastVector<DenseIndex> varDims(variableSlots.size());
+      FastVector<DenseIndex> varDims(variableSlots.size(), numeric_limits<DenseIndex>::max());
 #endif
       DenseIndex m = 0;
       DenseIndex n = 0;
+      for(size_t jointVarpos = 0; jointVarpos < variableSlots.size(); ++jointVarpos)
       {
-        size_t jointVarpos = 0;
+        const VariableSlots::const_iterator& slots = variableSlots[jointVarpos];
-        BOOST_FOREACH(VariableSlots::const_iterator slots, variableSlots)
+
-        {
+        assert(slots->second.size() == factors.size());
-          assert(slots->second.size() == factors.size());
+
+        bool foundVariable = false;
+        for(size_t sourceFactorI = 0; sourceFactorI < slots->second.size(); ++sourceFactorI)
+        {
+          const size_t sourceVarpos = slots->second[sourceFactorI];
+          if(sourceVarpos < numeric_limits<size_t>::max()) {
+            const JacobianFactor& sourceFactor = *factors[sourceFactorI];
+            if(sourceFactor.cols() > 0) {
+              foundVariable = true;
+              DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
 
-          size_t sourceFactorI = 0;
-          BOOST_FOREACH(const size_t sourceVarpos, slots->second) {
-            if(sourceVarpos < numeric_limits<size_t>::max()) {
-              const JacobianFactor& sourceFactor = *factors[sourceFactorI];
-              if(sourceFactor.rows() > 0) {
-                DenseIndex vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
 #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
-                if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
+              if(varDims[jointVarpos] == numeric_limits<DenseIndex>::max()) {
-                  varDims[jointVarpos] = vardim;
-                  n += vardim;
-                } else
-                  assert(varDims[jointVarpos] == vardim);
-#else
                 varDims[jointVarpos] = vardim;
                 n += vardim;
-                break;
+              } else {
-#endif
+                if(!(varDims[jointVarpos] == vardim)) {
+                  cout << "Factor " << sourceFactorI << " variable " << DefaultKeyFormatter(sourceFactor.keys()[sourceVarpos]) <<
+                      " has different dimensionality of " << vardim << " instead of " << varDims[jointVarpos] << endl;
+                  exit(1);
+                }
               }
+#else
+
+              varDims[jointVarpos] = vardim;
+              n += vardim;
+              break;
+#endif
             }
-            ++ sourceFactorI;
           }
-          ++ jointVarpos;
-        }
-        BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
-          m += factor->rows();
         }
+
+        if(!foundVariable)
+          GaussianFactorGraph(factors).print("factors: ");
+        assert(foundVariable);
       }
+
+      BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
+        m += factor->rows();
+      }
+
+#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS
+      BOOST_FOREACH(DenseIndex d, varDims) {
+        assert(d != numeric_limits<DenseIndex>::max());
+      }
+#endif
+
       return boost::make_tuple(varDims, m, n);
     }
 

gtsam/linear/JacobianFactor.h

@@ -204,6 +204,9 @@ namespace gtsam {
     /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */
     const VerticalBlockMatrix& matrixObject() const { return Ab_; }
 
+    /** Mutable access to the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */
+    VerticalBlockMatrix& matrixObject() { return Ab_; }
+
     /**
      * Construct the corresponding anti-factor to negate information
      * stored stored in this factor.

gtsam/nonlinear/ISAM2.cpp

@@ -765,10 +765,28 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
 //  multimap<sharedClique, GaussianFactor::shared_ptr> marginalFactors;
   map<Index, vector<GaussianFactor::shared_ptr> > marginalFactors;
 
+  // Keep track of factors that get summarized by removing cliques
+  FastSet<size_t> factorIndicesToRemove;
+
+  // Keep track of variables removed in subtrees
+  FastSet<Key> leafKeysRemoved;
+
   // Remove each variable and its subtrees
-  BOOST_REVERSE_FOREACH(Key j, leafKeys) {
+  BOOST_FOREACH(Key j, leafKeys) {
-    if(nodes_.count(j)) { // If the index was not already removed by removing another subtree
+    if(!leafKeysRemoved.exists(j)) { // If the index was not already removed by removing another subtree
+
+      // Traverse up the tree to find the root of the marginalized subtree
       sharedClique clique = nodes_[j];
+      while(!clique->parent_._empty())
+      {
+        // Check if parent contains a marginalized leaf variable.  Only need to check the first
+        // variable because it is the closest to the leaves.
+        sharedClique parent = clique->parent();
+        if(leafKeys.exists(parent->conditional()->front()))
+          clique = parent;
+        else
+          break;
+      }
 
       // See if we should remove the whole clique
       bool marginalizeEntireClique = true;
@@ -791,9 +809,17 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
         const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques
         BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
           marginalFactors.erase(removedClique->conditional()->front());
-          BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) {
+          leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals());
+          BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals())
+          {
+            // Add to factors to remove
+            const VariableIndex::Factors& involvedFactors = variableIndex_[frontal];
+            factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end());
+
+            // Check for non-leaf keys
             if(!leafKeys.exists(frontal))
-              throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); }
+              throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used.");
+          }
         }
       }
       else {
@@ -823,68 +849,74 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
           childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end());
           BOOST_FOREACH(const sharedClique& removedClique, removedCliques) {
             marginalFactors.erase(removedClique->conditional()->front());
-            BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) {
+            leafKeysRemoved.insert(removedClique->conditional()->beginFrontals(), removedClique->conditional()->endFrontals());
+            BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals())
+            {
+              // Add to factors to remove
+              const VariableIndex::Factors& involvedFactors = variableIndex_[frontal];
+              factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end());
+
+              // Check for non-leaf keys
               if(!leafKeys.exists(frontal))
-                throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); }
+                throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used.");
+            }
           }
         }
 
-        // Gather remaining children after we removed marginalized subtrees
-        FastVector<sharedClique> orphans(clique->children.begin(), clique->children.end());
-
         // Add the factors that are pulled into the current clique by the marginalized variables.
         // These are the factors that involve *marginalized* frontal variables in this clique
         // but do not involve frontal variables of any of its children.
-        FastSet<size_t> factorsFromMarginalizedInClique;
+        // TODO: reuse cached linear factors
+        FastSet<size_t> factorsFromMarginalizedInClique_step1;
         BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) {
           if(leafKeys.exists(frontal))
-            factorsFromMarginalizedInClique.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); }
+            factorsFromMarginalizedInClique_step1.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); }
+        // Remove any factors in subtrees that we're removing at this step
         BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) {
           BOOST_FOREACH(Index indexInClique, removedChild->conditional()->frontals()) {
             BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) {
-              factorsFromMarginalizedInClique.erase(factorInvolving); } } }
+              factorsFromMarginalizedInClique_step1.erase(factorInvolving); } } }
-        BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique) {
+        // Create factor graph from factor indices
+        BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique_step1) {
           graph.push_back(nonlinearFactors_[i]->linearize(theta_)); }
 
-        // Remove the current clique
-        sharedClique parent = clique->parent();
-        this->removeClique(clique);
-
         // Reeliminate the linear graph to get the marginal and discard the conditional
         const FastSet<Index> cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals());
-        FastSet<Index> cliqueFrontalsToEliminate;
+        FastVector<Index> cliqueFrontalsToEliminate;
         std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(),
-          std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end()));
+          std::back_inserter(cliqueFrontalsToEliminate));
-        FastVector<Index> cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end());
         pair<GaussianConditional::shared_ptr, GaussianFactor::shared_ptr> eliminationResult1 =
-          params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminateV));
+          params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminate));
 
         // Add the resulting marginal
         if(eliminationResult1.second)
           marginalFactors[clique->conditional()->front()].push_back(eliminationResult1.second);
 
-        // Recover the conditional on the remaining subset of frontal variables
+        // Split the current clique
-        // of this clique being partially marginalized.
+        // Find the position of the last leaf key in this clique
-        GaussianConditional::iterator jPosition = std::find(
+        DenseIndex nToRemove = 0;
-          clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j);
+        while(leafKeys.exists(clique->conditional()->keys()[nToRemove]))
-        GaussianFactorGraph graph2;
+          ++ nToRemove;
-        graph2.push_back(clique->conditional());
-        GaussianFactorGraph::EliminationResult eliminationResult2 = 
-          params_.getEliminationFunction()(graph2, Ordering(
-          clique->conditional()->beginFrontals(), jPosition + 1));
-        GaussianFactorGraph graph3;
-        graph3.push_back(eliminationResult2.second);
-        GaussianFactorGraph::EliminationResult eliminationResult3 = 
-          params_.getEliminationFunction()(graph3, Ordering(jPosition + 1, clique->conditional()->endFrontals()));
-        sharedClique newClique = boost::make_shared<Clique>();
-        newClique->setEliminationResult(make_pair(eliminationResult3.first, clique->cachedFactor()));
 
-        // Add the marginalized clique to the BayesTree
+        // Make the clique's matrix appear as a subset
-        this->addClique(newClique, parent);
+        const DenseIndex dimToRemove = clique->conditional()->matrixObject().offset(nToRemove);
+        clique->conditional()->matrixObject().firstBlock() = nToRemove;
+        clique->conditional()->matrixObject().rowStart() = dimToRemove;
 
-        // Add the orphans
+        // Change the keys in the clique
-        BOOST_FOREACH(const sharedClique& orphan, orphans) {
+        FastVector<Key> originalKeys; originalKeys.swap(clique->conditional()->keys());
-          this->addClique(orphan, newClique); }
+        clique->conditional()->keys().assign(originalKeys.begin() + nToRemove, originalKeys.end());
+        clique->conditional()->nrFrontals() -= nToRemove;
+
+        // Add to factors to remove factors involved in frontals of current clique
+        BOOST_FOREACH(Key frontal, cliqueFrontalsToEliminate)
+        {
+          const VariableIndex::Factors& involvedFactors = variableIndex_[frontal];
+          factorIndicesToRemove.insert(involvedFactors.begin(), involvedFactors.end());
+        }
+
+        // Add removed keys
+        leafKeysRemoved.insert(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end());
       }
     }
   }
@@ -912,9 +944,6 @@ void ISAM2::marginalizeLeaves(const FastList<Key>& leafKeysList,
   variableIndex_.augment(factorsToAdd); // Augment the variable index
 
   // Remove the factors to remove that have been summarized in the newly-added marginal factors
-  FastSet<size_t> factorIndicesToRemove;
-  BOOST_FOREACH(Key j, leafKeys) {
-    factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); }
   NonlinearFactorGraph removedFactors;
   BOOST_FOREACH(size_t i, factorIndicesToRemove) {
     removedFactors.push_back(nonlinearFactors_[i]);