Function to remove factors from a VariableIndex

gtsam/inference/VariableIndex.cpp

@@ -16,6 +16,7 @@
  */
 
 #include <iostream>
+
 #include <gtsam/inference/VariableIndex.h>
 
 namespace gtsam {
@@ -49,7 +50,8 @@ bool VariableIndex::equals(const VariableIndex& other, double tol) const {
 
 /* ************************************************************************* */
 void VariableIndex::print(const std::string& str) const {
-  std::cout << str;
+  std::cout << str << "\n";
+  std::cout << "nEntries = " << this->nEntries_ << ", nFactors = " << this->nFactors_ << "\n";
   Index var = 0;
   BOOST_FOREACH(const Factors& variable, index_.container()) {
     Permutation::const_iterator rvar = find(index_.permutation().begin(), index_.permutation().end(), var);

gtsam/inference/VariableIndex.h

@@ -17,12 +17,13 @@
 
 #pragma once
 
+#include <vector>
+#include <stdexcept>
+#include <boost/foreach.hpp>
+
 #include <gtsam/base/FastList.h>
 #include <gtsam/inference/Permutation.h>
 
-#include <vector>
-#include <boost/foreach.hpp>
-
 namespace gtsam {
 
 class Inference;
@@ -94,7 +95,15 @@ public:
    * Augment the variable index with new factors.  This can be used when
    * solving problems incrementally.
    */
-  template<class FactorGraph> void augment(const FactorGraph& factorGraph);
+  template<class FactorGraph> void augment(const FactorGraph& factors);
+
+  /**
+   * Remove entries corresponding to the specified factors.
+   * @param indices The indices of the factors to remove, which must match \c factors
+   * @param factors The factors being removed, which must symbolically correspond
+   * exactly to the factors with the specified \c indices that were added.
+   */
+  template<typename CONTAINER, class FactorGraph> void remove(const CONTAINER& indices, const FactorGraph& factors);
 
   /** Test for equality (for unit tests and debug assertions). */
   bool equals(const VariableIndex& other, double tol=0.0) const;
@@ -119,7 +128,7 @@ template<class FactorGraph>
 void VariableIndex::fill(const FactorGraph& factorGraph) {
 
   // Build index mapping from variable id to factor index
-  for(size_t fi=0; fi<factorGraph.size(); ++fi)
+  for(size_t fi=0; fi<factorGraph.size(); ++fi) {
     if(factorGraph[fi]) {
       BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
         if(key < index_.size()) {
@@ -127,7 +136,8 @@ void VariableIndex::fill(const FactorGraph& factorGraph) {
           ++ nEntries_;
         }
       }
-      ++ nFactors_;
+    }
+    ++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
   }
 }
 
@@ -167,13 +177,13 @@ VariableIndex::VariableIndex(const FactorGraph& factorGraph, Index nVariables) :
 
 /* ************************************************************************* */
 template<class FactorGraph>
-void VariableIndex::augment(const FactorGraph& factorGraph) {
+void VariableIndex::augment(const FactorGraph& factors) {
   // If the factor graph is empty, return an empty index because inside this
   // if block we assume at least one factor.
-  if(factorGraph.size() > 0) {
+  if(factors.size() > 0) {
     // Find highest-numbered variable
     Index maxVar = 0;
-    BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factorGraph) {
+    BOOST_FOREACH(const typename FactorGraph::sharedFactor& factor, factors) {
       if(factor) {
         BOOST_FOREACH(const Index key, factor->keys()) {
           if(key > maxVar)
@@ -191,13 +201,30 @@ void VariableIndex::augment(const FactorGraph& factorGraph) {
 
     // Augment index mapping from variable id to factor index
     size_t orignFactors = nFactors_;
-    for(size_t fi=0; fi<factorGraph.size(); ++fi)
-      if(factorGraph[fi]) {
-        BOOST_FOREACH(const Index key, factorGraph[fi]->keys()) {
+    for(size_t fi=0; fi<factors.size(); ++fi) {
+      if(factors[fi]) {
+        BOOST_FOREACH(const Index key, factors[fi]->keys()) {
           index_[key].push_back(orignFactors + fi);
           ++ nEntries_;
         }
-        ++ nFactors_;
+      }
+      ++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
+    }
+  }
+}
+
+/* ************************************************************************* */
+template<typename CONTAINER, class FactorGraph>
+void VariableIndex::remove(const CONTAINER& indices, const FactorGraph& factors) {
+  for(size_t fi=0; fi<factors.size(); ++fi)
+    if(factors[fi]) {
+      for(size_t ji = 0; ji < factors[fi]->keys().size(); ++ji) {
+        Factors& factorEntries = index_[factors[fi]->keys()[ji]];
+        Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), indices[fi]);
+        if(entry == factorEntries.end())
+          throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
+        factorEntries.erase(entry);
+        -- nEntries_;
       }
     }
 }

gtsam/inference/tests/testVariableIndex.cpp

@@ -46,6 +46,38 @@ TEST(VariableIndex, augment) {
   CHECK(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(VariableIndex, remove) {
+
+  SymbolicFactorGraph fg1, fg2;
+  fg1.push_factor(0, 1);
+  fg1.push_factor(0, 2);
+  fg1.push_factor(5, 9);
+  fg1.push_factor(2, 3);
+  fg2.push_factor(1, 3);
+  fg2.push_factor(2, 4);
+  fg2.push_factor(3, 5);
+  fg2.push_factor(5, 6);
+
+  SymbolicFactorGraph fgCombined; fgCombined.push_back(fg1); fgCombined.push_back(fg2);
+
+  // Create a factor graph containing only the factors from fg2 and with null
+  // factors in the place of those of fg1, so that the factor indices are correct.
+  SymbolicFactorGraph fg2removed(fgCombined);
+  fg2removed.remove(0); fg2removed.remove(1); fg2removed.remove(2); fg2removed.remove(3);
+
+  // The expected VariableIndex has the same factor indices as fgCombined but
+  // with entries from fg1 removed, and still has all 10 variables.
+  VariableIndex expected(fg2removed, 10);
+  VariableIndex actual(fgCombined);
+  vector<size_t> indices;
+  indices.push_back(0); indices.push_back(1); indices.push_back(2); indices.push_back(3);
+  actual.remove(indices, fg1);
+
+  CHECK(assert_equal(expected, actual));
+
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;