Added optional argument to VariableIndex::augment for specifying the indices of the new factors (needed for ISAM2 to reuse empty factor slots)

gtsam/inference/VariableIndex-inl.h

@@ -23,7 +23,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 template<class FG>
-void VariableIndex::augment(const FG& factors)
+void VariableIndex::augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices)
 {
   gttic(VariableIndex_augment);
 
@@ -31,15 +31,31 @@ void VariableIndex::augment(const FG& factors)
   const size_t originalNFactors = nFactors_;
 
   // Augment index for each factor
-  for(size_t i = 0; i < factors.size(); ++i) {
+  for(size_t i = 0; i < factors.size(); ++i)
-    if(factors[i]) {
+  {
-      const size_t globalI = originalNFactors + i;
+    if(factors[i])
-      BOOST_FOREACH(const Key key, *factors[i]) {
+    {
+      const size_t globalI =
+        newFactorIndices ?
+        (*newFactorIndices)[i] :
+        nFactors_;
+      BOOST_FOREACH(const Key key, *factors[i])
+      {
         index_[key].push_back(globalI);
         ++ nEntries_;
       }
     }
-    ++ nFactors_; // Increment factor count even if factors are null, to keep indices consistent
+
+    // Increment factor count even if factors are null, to keep indices consistent
+    if(newFactorIndices)
+    {
+      if((*newFactorIndices)[i] >= nFactors_)
+        nFactors_ = (*newFactorIndices)[i] + 1;
+    }
+    else
+    {
+      ++ nFactors_;
+    }
   }
 }
 

gtsam/inference/VariableIndex.h

@@ -125,7 +125,7 @@ public:
    * solving problems incrementally.
    */
   template<class FG>
-  void augment(const FG& factors);
+  void augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices = boost::none);
 
   /**
    * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement

gtsam/symbolic/tests/testVariableIndexUnordered.cpp

@@ -17,6 +17,7 @@
  */
 
 #include <boost/assign/std/list.hpp>
+#include <boost/assign/list_of.hpp>
 using namespace boost::assign;
 
 #include <CppUnitLite/TestHarness.h>
@@ -52,6 +53,35 @@ TEST(VariableIndex, augment) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(VariableIndex, augment2) {
+
+  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(SymbolicFactor::shared_ptr()); // Add an extra empty factor
+  fgCombined.push_back(fg2);
+
+  VariableIndex expected(fgCombined);
+
+  FastVector<size_t> newIndices = list_of(5)(6)(7)(8);
+  VariableIndex actual(fg1);
+  actual.augment(fg2, newIndices);
+
+  LONGS_EQUAL(16, (long) actual.nEntries());
+  LONGS_EQUAL(9, (long) actual.nFactors());
+  EXPECT(assert_equal(expected, actual));
+}
+
 /* ************************************************************************* */
 TEST(VariableIndex, remove) {