diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index b71c81988..53d400223 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/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); +} + } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 5f9e05cb0..a96a53289 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -125,6 +125,13 @@ public: template void augment(const FG& factors, boost::optional 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