diff --git a/gtsam/inference/VariableIndex.cpp b/gtsam/inference/VariableIndex.cpp index a29d5e432..4e5eb8649 100644 --- a/gtsam/inference/VariableIndex.cpp +++ b/gtsam/inference/VariableIndex.cpp @@ -76,4 +76,15 @@ void VariableIndex::permuteInPlace(const Permutation& permutation) { index_.swap(newIndex); } +/* ************************************************************************* */ +void VariableIndex::removeUnusedAtEnd(size_t nToRemove) { +#ifndef NDEBUG + for(size_t i = this->size() - nToRemove; i < this->size(); ++i) + if(!(*this)[i].empty()) + throw std::invalid_argument("Attempting to remove non-empty variables with VariableIndex::removeUnusedAtEnd()"); +#endif + + index_.resize(this->size() - nToRemove); +} + } diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index be47cbd5a..739067dd2 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -119,6 +119,11 @@ public: /** * 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 graph + * does not shift the indices of other factors. Also, we keep nFactors_ + * one greater than the highest-numbered factor referenced in a VariableIndex. + * * @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. @@ -128,6 +133,12 @@ public: /// Permute the variables in the VariableIndex according to the given permutation void permuteInPlace(const Permutation& permutation); + /** Remove unused empty variables at the end of the ordering (in debug mode + * verifies they are empty). + * @param nToRemove The number of unused variables at the end to remove + */ + void removeUnusedAtEnd(size_t nToRemove); + protected: Factor_iterator factorsBegin(Index variable) { checkVar(variable); return index_[variable].begin(); } /// void VariableIndex::remove(const CONTAINER& indices, const FactorGraph& factors) { + // NOTE: We intentionally do not decrement nFactors_ because the factor + // indices need to remain consistent. Removing factors from a factor graph + // does not shift the indices of other factors. Also, we keep nFactors_ + // one greater than the highest-numbered factor referenced in a VariableIndex. for(size_t fi=0; fikeys().size(); ++ji) { diff --git a/gtsam/inference/tests/testVariableIndex.cpp b/gtsam/inference/tests/testVariableIndex.cpp index 7c8e6fa9e..412b3fa04 100644 --- a/gtsam/inference/tests/testVariableIndex.cpp +++ b/gtsam/inference/tests/testVariableIndex.cpp @@ -44,7 +44,9 @@ TEST(VariableIndex, augment) { VariableIndex actual(fg1); actual.augment(fg2); - CHECK(assert_equal(expected, actual)); + LONGS_EQUAL(16, actual.nEntries()); + LONGS_EQUAL(8, actual.nFactors()); + EXPECT(assert_equal(expected, actual)); } /* ************************************************************************* */