VariableIndex can augment an existing factor

gtsam/inference/VariableIndex.cpp

@@ -55,4 +55,17 @@ void VariableIndex::outputMetisFormat(ostream& os) const {
   os << flush;
 }
 
+/* ************************************************************************* */
+void VariableIndex::augmentExistingFactor(const FactorIndex factorIndex, const KeySet & newKeys)
+{
+  gttic(VariableIndex_augmentExistingFactor);
+
+  for(const Key key: newKeys) {
+    index_[key].push_back(factorIndex);
+    ++nEntries_;
+  }
+
+  gttoc(VariableIndex_augmentExistingFactor);
+}
+
 }

gtsam/inference/VariableIndex.h

@@ -125,6 +125,13 @@ public:
   template<class FG>
   void augment(const FG& factors, boost::optional<const FactorIndices&> newFactorIndices = boost::none);
 
+  /**
+   * Augment the variable index after an existing factor now affects to more
+   * variable Keys. This can be used when solving problems incrementally, with
+   * smart factors or in general with factors with a dynamic number of Keys.
+   */
+  void augmentExistingFactor(const FactorIndex factorIndex, const KeySet & newKeys);
+
   /**
    * Remove entries corresponding to the specified factors. NOTE: We intentionally do not decrement
    * nFactors_ because the factor indices need to remain consistent.  Removing factors from a factor