From ecacda68c04313a4e9ce5a687b8439fdeaa64f61 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 1 Jun 2019 13:45:07 -0400 Subject: [PATCH] Further refactored pushNewFactors --- gtsam/nonlinear/ISAM2-impl.h | 65 +++++++++++++++++++++--------------- gtsam/nonlinear/ISAM2.cpp | 8 +---- tests/testGaussianISAM2.cpp | 9 +++-- 3 files changed, 45 insertions(+), 37 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index f4dfcb99b..352d26c8a 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -117,17 +117,33 @@ struct GTSAM_EXPORT UpdateImpl { UpdateImpl(const ISAM2Params& params, const ISAM2UpdateParams& updateParams) : params_(params), updateParams_(updateParams) {} + // Provide some debugging information at the start of update + static void LogStartingUpdate(const NonlinearFactorGraph& newFactors, + const ISAM2& isam2) { + gttic(pushBackFactors); + const bool debug = ISDEBUG("ISAM2 update"); + const bool verbose = ISDEBUG("ISAM2 update verbose"); + + if (verbose) { + std::cout << "ISAM2::update\n"; + isam2.print("ISAM2: "); + } + + // Add the new factor indices to the result struct + if (debug || verbose) { + newFactors.print("The new factors are: "); + } + } + /// 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, - FactorIndices* newFactorIndices) { - newFactorIndices->resize(newFactors.size()); + FactorIndices addFactorsStep1(const NonlinearFactorGraph& newFactors, + NonlinearFactorGraph* nonlinearFactors) const { + FactorIndices newFactorIndices(newFactors.size()); - if (useUnusedSlots) { + if (params_.findUnusedFactorSlots) { size_t globalFactorIndex = 0; for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size(); ++newFactorIndex) { @@ -150,14 +166,15 @@ struct GTSAM_EXPORT UpdateImpl { // Use the current slot, updating nonlinearFactors and newFactorSlots. (*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex]; - (*newFactorIndices)[newFactorIndex] = globalFactorIndex; + 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(); + newFactorIndices[i] = i + nonlinearFactors->size(); nonlinearFactors->push_back(newFactors); } + return newFactorIndices; } // 1. Add any new factors \Factors:=\Factors\cup\Factors'. @@ -167,13 +184,9 @@ struct GTSAM_EXPORT UpdateImpl { VariableIndex* variableIndex, ISAM2Result* result) const { gttic(pushBackFactors); - const bool debug = ISDEBUG("ISAM2 update"); - const bool verbose = ISDEBUG("ISAM2 update verbose"); // Add the new factor indices to the result struct - if (debug || verbose) newFactors.print("The new factors are: "); - AddFactorsStep1(newFactors, params_.findUnusedFactorSlots, nonlinearFactors, - &result->newFactorsIndices); + result->newFactorsIndices = addFactorsStep1(newFactors, nonlinearFactors); // Remove the removed factors NonlinearFactorGraph removedFactors; @@ -378,8 +391,8 @@ struct GTSAM_EXPORT UpdateImpl { * \c mask. Values are expmapped in-place. * \param mask Mask on linear indices, only \c true entries are expmapped */ - void expmapMasked(const VectorValues& delta, const KeySet& mask, - Values* theta) const { + static void ExpmapMasked(const VectorValues& delta, const KeySet& mask, + Values* theta) { assert(theta->size() == delta.size()); Values::iterator key_value; VectorValues::const_iterator key_delta; @@ -468,7 +481,7 @@ struct GTSAM_EXPORT UpdateImpl { gttic(expmap); // 6. Update linearization point for marked variables: // \Theta_{J}:=\Theta_{J}+\Delta_{J}. - if (!relinKeys.empty()) expmapMasked(delta, markedRelinMask, theta); + if (!relinKeys.empty()) ExpmapMasked(delta, markedRelinMask, theta); gttoc(expmap); result->variablesRelinearized = result->markedKeys.size(); @@ -511,7 +524,7 @@ struct GTSAM_EXPORT UpdateImpl { } } - void logRecalculateKeys(const ISAM2Result& result) const { + static void LogRecalculateKeys(const ISAM2Result& result) { const bool debug = ISDEBUG("ISAM2 recalculate"); if (debug) { @@ -528,8 +541,9 @@ struct GTSAM_EXPORT UpdateImpl { } } - FactorIndexSet getAffectedFactors(const KeyList& keys, - const VariableIndex& variableIndex) const { + static FactorIndexSet GetAffectedFactors(const KeyList& keys, + const VariableIndex& variableIndex) { + gttic(GetAffectedFactors); FactorIndexSet indices; for (const Key key : keys) { const FactorIndices& factors(variableIndex[key]); @@ -540,8 +554,8 @@ struct GTSAM_EXPORT UpdateImpl { // find intermediate (linearized) factors from cache that are passed into the // affected area - GaussianFactorGraph getCachedBoundaryFactors( - const ISAM2::Cliques& orphans) const { + static GaussianFactorGraph GetCachedBoundaryFactors( + const ISAM2::Cliques& orphans) { GaussianFactorGraph cachedBoundary; for (const auto& orphan : orphans) { @@ -559,9 +573,7 @@ struct GTSAM_EXPORT UpdateImpl { const NonlinearFactorGraph& nonlinearFactors, const VariableIndex& variableIndex, const Values& theta, GaussianFactorGraph* linearFactors) const { - gttic(getAffectedFactors); - FactorIndexSet candidates = getAffectedFactors(affectedKeys, variableIndex); - gttoc(getAffectedFactors); + FactorIndexSet candidates = GetAffectedFactors(affectedKeys, variableIndex); gttic(affectedKeysSet); // for fast lookup below @@ -614,7 +626,7 @@ struct GTSAM_EXPORT UpdateImpl { GaussianFactorGraph* linearFactors, ISAM2::Roots* roots, ISAM2::Nodes* nodes, ISAM2Result* result) const { gttic(recalculate); - logRecalculateKeys(*result); + LogRecalculateKeys(*result); // FactorGraph factors(affectedBayesNet); // bug was here: we cannot reuse the original factors, because then the @@ -754,7 +766,7 @@ struct GTSAM_EXPORT UpdateImpl { gttic(cached); // add the cached intermediate results from the boundary of the orphans // ... - GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); + GaussianFactorGraph cachedBoundary = GetCachedBoundaryFactors(orphans); if (debug) cachedBoundary.print("Boundary factors: "); factors.push_back(cachedBoundary); gttoc(cached); @@ -846,6 +858,5 @@ struct GTSAM_EXPORT UpdateImpl { return affectedKeysSet; } }; -/* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 0ce60fd6f..d6a48c6d5 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -116,8 +116,8 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, const Values& newTheta, const ISAM2UpdateParams& updateParams) { gttic(ISAM2_update); - this->update_count_++; + UpdateImpl::LogStartingUpdate(newFactors, *this); ISAM2Result result; if (params_.enableDetailedResults) @@ -127,12 +127,6 @@ ISAM2Result ISAM2::update(const NonlinearFactorGraph& newFactors, (params_.enableRelinearization && update_count_ % params_.relinearizeSkip == 0); - const bool verbose = ISDEBUG("ISAM2 update verbose"); - if (verbose) { - cout << "ISAM2::update\n"; - this->print("ISAM2: "); - } - // Update delta if we need it to check relinearization later if (relinearizeThisStep) { updateDelta(updateParams.forceFullSolve); diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index d8b829872..3f8619a77 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -305,9 +305,12 @@ TEST(ISAM2, AddFactorsStep1) const FactorIndices expectedNewFactorIndices = list_of(1)(3); - FactorIndices actualNewFactorIndices; - - UpdateImpl::AddFactorsStep1(newFactors, true, &nonlinearFactors, &actualNewFactorIndices); + ISAM2Params params; + ISAM2UpdateParams updateParams; + params.findUnusedFactorSlots = true; + UpdateImpl update(params, updateParams); + FactorIndices actualNewFactorIndices = + update.addFactorsStep1(newFactors, &nonlinearFactors); EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors)); EXPECT(assert_container_equality(expectedNewFactorIndices, actualNewFactorIndices));