From fa5d08d9a657cab55fa745808fd3583a95127f9a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 18 Nov 2013 19:23:09 +0000 Subject: [PATCH] Option to reuse old factor slots in ISAM2 --- gtsam/nonlinear/ISAM2-impl.cpp | 41 ++++++++++++++++++++++++++++++++++ gtsam/nonlinear/ISAM2-impl.h | 6 +++++ gtsam/nonlinear/ISAM2.cpp | 28 ++++++++++++++--------- gtsam/nonlinear/ISAM2.h | 11 +++++++-- tests/testGaussianISAM2.cpp | 28 +++++++++++++++++++++++ 5 files changed, 101 insertions(+), 13 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 60e034d9b..4308d02e1 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -42,6 +42,47 @@ void ISAM2::Impl::AddVariables( RgProd.insert(newTheta.zeroVectors()); } +/* ************************************************************************* */ +void ISAM2::Impl::AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, + NonlinearFactorGraph& nonlinearFactors, FastVector& newFactorIndices) +{ + newFactorIndices.resize(newFactors.size()); + + if(useUnusedSlots) + { + size_t globalFactorIndex = 0; + for(size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex) + { + // Loop to find the next available factor slot + do + { + // If we need to add more factors than we have room for, resize nonlinearFactors, + // filling the new slots with NULL factors. Otherwise, check if the current + // factor in nonlinearFactors is already used, and if so, increase + // globalFactorIndex. If the current factor in nonlinearFactors is unused, break + // out of the loop and use the current slot. + if(globalFactorIndex >= nonlinearFactors.size()) + nonlinearFactors.resize(nonlinearFactors.size() + newFactors.size() - newFactorIndex); + else if(nonlinearFactors[globalFactorIndex]) + ++ globalFactorIndex; + else + break; + } while(true); + + // Use the current slot, updating nonlinearFactors and newFactorSlots. + nonlinearFactors[globalFactorIndex] = newFactors[newFactorIndex]; + newFactorIndices[newFactorIndex] = globalFactorIndex; + } + } + else + { + // We're not looking for unused slots, so just add the factors at the end. + for(size_t i = 0; i < newFactors.size(); ++i) + newFactorIndices[i] = i + nonlinearFactors.size(); + nonlinearFactors.push_back(newFactors); + } +} + /* ************************************************************************* */ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const FastVector& roots, Values& theta, VariableIndex& variableIndex, diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 62755785f..d76201f7f 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -47,6 +47,12 @@ struct GTSAM_EXPORT ISAM2::Impl { static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + + /// Perform the first part of the bookkeeping updates for adding new factors. Adds them to the + /// complete list of nonlinear factors, and populates the list of new factor indices, both + /// optionally finding and reusing empty factor slots. + static void AddFactorsStep1(const NonlinearFactorGraph& newFactors, bool useUnusedSlots, + NonlinearFactorGraph& nonlinearFactors, FastVector& newFactorIndices); /** * Remove variables from the ISAM2 system. diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index a789b96fb..f57e02ca8 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -550,14 +550,10 @@ ISAM2Result ISAM2::update( } gttic(push_back_factors); - // Add the new factor indices to the result struct - result.newFactorsIndices.resize(newFactors.size()); - for(size_t i=0; ibegin(), replacedKeys->end()); gttoc(recalculate); - // After the top of the tree has been redone and may have index gaps from - // unused keys, condense the indices to remove gaps by rearranging indices - // in all data structures. + // Update data structures to remove unused keys if(!unusedKeys.empty()) { gttic(remove_variables); Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 56e3c4e7e..66a1ce1ae 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -164,6 +164,11 @@ struct GTSAM_EXPORT ISAM2Params { */ bool enablePartialRelinearizationCheck; + /// When you will be removing many factors, e.g. when using ISAM2 as a fixed-lag smoother, enable this option to + /// add factors in the first available factor slots, to avoid accumulating NULL factor slots, at the cost of + /// having to search for slots every time a factor is added. + bool findUnusedFactorSlots; + /** Specify parameters as constructor arguments */ ISAM2Params( OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams @@ -178,7 +183,8 @@ struct GTSAM_EXPORT ISAM2Params { relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter), - enableDetailedResults(false), enablePartialRelinearizationCheck(false) {} + enableDetailedResults(false), enablePartialRelinearizationCheck(false), + findUnusedFactorSlots(false) {} void print(const std::string& str = "") const { std::cout << str << "\n"; @@ -204,6 +210,7 @@ struct GTSAM_EXPORT ISAM2Params { std::cout << "cacheLinearizedFactors: " << cacheLinearizedFactors << "\n"; std::cout << "enableDetailedResults: " << enableDetailedResults << "\n"; std::cout << "enablePartialRelinearizationCheck: " << enablePartialRelinearizationCheck << "\n"; + std::cout << "findUnusedFactorSlots: " << findUnusedFactorSlots << "\n"; std::cout.flush(); } @@ -301,7 +308,7 @@ struct GTSAM_EXPORT ISAM2Result { * factors passed as \c newFactors to ISAM2::update(). These indices may be * used later to refer to the factors in order to remove them. */ - std::vector newFactorsIndices; + FastVector newFactorsIndices; /** A struct holding detailed results, which must be enabled with * ISAM2Params::enableDetailedResults. diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 48c349877..e438f4753 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -284,6 +284,34 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent; } +/* ************************************************************************* */ +TEST(ISAM2, AddFactorsStep1) +{ + NonlinearFactorGraph nonlinearFactors; + nonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); + nonlinearFactors += NonlinearFactor::shared_ptr(); + nonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); + + NonlinearFactorGraph newFactors; + newFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); + newFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + + NonlinearFactorGraph expectedNonlinearFactors; + expectedNonlinearFactors += PriorFactor(10, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(1, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(11, LieVector(), gtsam::SharedNoiseModel()); + expectedNonlinearFactors += PriorFactor(2, LieVector(), gtsam::SharedNoiseModel()); + + const FastVector expectedNewFactorIndices = list_of(1)(3); + + FastVector actualNewFactorIndices; + + ISAM2::Impl::AddFactorsStep1(newFactors, true, nonlinearFactors, actualNewFactorIndices); + + EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); + EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices)); +} + /* ************************************************************************* */ TEST(ISAM2, simple) {