From 0d216c8878b5446010cf456b6bcf7ceabd09e2fd Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 11 Mar 2012 22:10:51 +0000 Subject: [PATCH] Only do ISAM2 back-substitution when needed instead of during every update --- gtsam/nonlinear/ISAM2-impl-inl.h | 52 ++++++++++- gtsam/nonlinear/ISAM2-inl.h | 148 ++++++++++++++++++------------- gtsam/nonlinear/ISAM2.h | 38 ++++++-- tests/testGaussianISAM2.cpp | 7 +- 4 files changed, 171 insertions(+), 74 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index d3da8907e..4a3dfe86e 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -46,8 +46,8 @@ struct ISAM2::Impl { * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, - typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol @@ -121,12 +121,15 @@ struct ISAM2::Impl { static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode); + static size_t UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold); + }; /* ************************************************************************* */ template void ISAM2::Impl::AddVariables( - const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) { + const Values& newTheta, Values& theta, Permuted& delta, vector& replacedKeys, + Ordering& ordering,typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -153,6 +156,7 @@ void ISAM2::Impl::AddVariables( assert(ordering.size() == delta.size()); } assert(ordering.nVars() >= nodes.size()); + replacedKeys.resize(ordering.nVars(), false); nodes.resize(ordering.nVars()); } @@ -229,7 +233,8 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const Permute invalidateIfDebug = boost::optional&>(); #endif - assert(values.size() == ordering.size()); + assert(values.size() == ordering.nVars()); + assert(delta.size() == ordering.nVars()); Values::iterator key_value; Ordering::const_iterator key_index; for(key_value = values.begin(), key_index = ordering.begin(); @@ -354,4 +359,43 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, return result; } +/* ************************************************************************* */ +template +size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr >& root, std::vector& replacedKeys, Permuted& delta, double wildfireThreshold) { + + size_t lastBacksubVariableCount; + + if (wildfireThreshold <= 0.0) { + // Threshold is zero or less, so do a full recalculation + // Collect dimensions and allocate new VectorValues + vector dims(delta.size()); + for(size_t j=0; jdim(j); + VectorValues newDelta(dims); + + // Optimize full solution delta + optimize2(root, newDelta); + + // Copy solution into delta + delta.permutation() = Permutation::Identity(delta.size()); + delta.container() = newDelta; + + lastBacksubVariableCount = delta.size(); + + } else { + // Optimize with wildfire + lastBacksubVariableCount = optimize2(root, wildfireThreshold, replacedKeys, delta); // modifies delta_ + +#ifndef NDEBUG + for(size_t j=0; j).all()); +#endif + } + + // Clear replacedKeys + replacedKeys.assign(replacedKeys.size(), false); + + return lastBacksubVariableCount; +} + } diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 1b8130098..410042dae 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -41,7 +41,7 @@ static const double batchThreshold = 0.65; /* ************************************************************************* */ template ISAM2::ISAM2(const ISAM2Params& params): - delta_(Permutation(), deltaUnpermuted_), params_(params) { + delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true), params_(params) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -50,7 +50,7 @@ ISAM2::ISAM2(const ISAM2Params& params): /* ************************************************************************* */ template ISAM2::ISAM2(): - delta_(Permutation(), deltaUnpermuted_) { + delta_(Permutation(), deltaUnpermuted_), deltaUptodate_(true) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) doglegDelta_ = boost::get(params_.optimizationParams).initialDelta; @@ -413,13 +413,21 @@ ISAM2Result ISAM2::update( lastBacksubVariableCount = 0; lastNnzTop = 0; ISAM2Result result; + const bool relinearizeThisStep = force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0); if(verbose) { cout << "ISAM2::update\n"; this->print("ISAM2: "); } - tic(0,"push_back factors"); + // Update delta if we need it to check relinearization later + if(relinearizeThisStep) { + tic(0, "updateDelta"); + updateDelta(disableReordering); + toc(0, "updateDelta"); + } + + tic(1,"push_back factors"); // Add the new factor indices to the result struct result.newFactorsIndices.resize(newFactors.size()); for(size_t i=0; i::update( // Remove removed factors from the variable index so we do not attempt to relinearize them variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_)); - toc(0,"push_back factors"); + toc(1,"push_back factors"); - tic(1,"add new variables"); + tic(2,"add new variables"); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, ordering_, Base::nodes_); - toc(1,"add new variables"); + Impl::AddVariables(newTheta, theta_, delta_, deltaReplacedMask_, ordering_, Base::nodes_); + toc(2,"add new variables"); - tic(2,"evaluate error before"); + tic(3,"evaluate error before"); if(params_.evaluateNonlinearError) result.errorBefore.reset(nonlinearFactors_.error(calculateEstimate())); - toc(2,"evaluate error before"); + toc(3,"evaluate error before"); - tic(3,"gather involved keys"); + tic(4,"gather involved keys"); // 3. Mark linear update FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors // Also mark keys involved in removed factors @@ -462,11 +470,11 @@ ISAM2Result ISAM2::update( // is a vector of size_t, so the constructor unintentionally resolves to // vector(size_t count, Index value) instead of the iterator constructor. FastVector newKeys; newKeys.assign(markedKeys.begin(), markedKeys.end()); // Make a copy of these, as we'll soon add to them - toc(3,"gather involved keys"); + toc(4,"gather involved keys"); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold - if (force_relinearize || (params_.enableRelinearization && count % params_.relinearizeSkip == 0)) { // todo: every n steps - tic(4,"gather relinearize keys"); + if (relinearizeThisStep) { + tic(5,"gather relinearize keys"); vector markedRelinMask(ordering_.nVars(), false); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. FastSet relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); @@ -475,19 +483,19 @@ ISAM2Result ISAM2::update( // Add the variables being relinearized to the marked keys BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } markedKeys.insert(relinKeys.begin(), relinKeys.end()); - toc(4,"gather relinearize keys"); + toc(5,"gather relinearize keys"); - tic(5,"fluid find_all"); + tic(6,"fluid find_all"); // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. if (!relinKeys.empty() && this->root()) Impl::FindAll(this->root(), markedKeys, markedRelinMask); // add other cliques that have the marked ones in the separator - toc(5,"fluid find_all"); + toc(6,"fluid find_all"); - tic(6,"expmap"); + tic(7,"expmap"); // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. if (!relinKeys.empty()) Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); - toc(6,"expmap"); + toc(7,"expmap"); result.variablesRelinearized = markedKeys.size(); @@ -501,7 +509,7 @@ ISAM2Result ISAM2::update( #endif } - tic(7,"linearize new"); + tic(8,"linearize new"); tic(1,"linearize"); // 7. Linearize new factors FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); @@ -511,9 +519,9 @@ ISAM2Result ISAM2::update( // Augment the variable index with the new factors variableIndex_.augment(*linearFactors); toc(2,"augment VI"); - toc(7,"linearize new"); + toc(8,"linearize new"); - tic(8,"recalculate"); + tic(9,"recalculate"); // 8. Redo top of Bayes tree // Convert constrained symbols to indices boost::optional > constrainedIndices; @@ -526,49 +534,17 @@ ISAM2Result ISAM2::update( boost::shared_ptr > replacedKeys; if(!markedKeys.empty() || !newKeys.empty()) replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result); - toc(8,"recalculate"); - tic(9,"solve"); + // Update replaced keys mask (accumulates until back-substitution takes place) + if(replacedKeys) { + BOOST_FOREACH(const Index var, *replacedKeys) { + deltaReplacedMask_[var] = true; } } + toc(9,"recalculate"); + + //tic(9,"solve"); // 9. Solve - if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { - // See note in gtsam/base/boost_variant_with_workaround.h - const ISAM2GaussNewtonParams& gaussNewtonParams = - boost::get(params_.optimizationParams); - if (gaussNewtonParams.wildfireThreshold <= 0.0 || disableReordering) { - VectorValues newDelta(theta_.dims(ordering_)); - optimize2(this->root(), newDelta); - if(debug) newDelta.print("newDelta: "); - assert(newDelta.size() == delta_.size()); - delta_.permutation() = Permutation::Identity(delta_.size()); - delta_.container() = newDelta; - lastBacksubVariableCount = theta_.size(); - } else { - vector replacedKeysMask(variableIndex_.size(), false); - if(replacedKeys) { - BOOST_FOREACH(const Index var, *replacedKeys) { - replacedKeysMask[var] = true; } } - lastBacksubVariableCount = optimize2(this->root(), gaussNewtonParams.wildfireThreshold, replacedKeysMask, delta_); // modifies delta_ - -#ifndef NDEBUG - for(size_t j=0; j).all()); -#endif - } - } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { - // See note in gtsam/base/boost_variant_with_workaround.h - const ISAM2DoglegParams& doglegParams = - boost::get(params_.optimizationParams); - // Do one Dogleg iteration - tic(1, "Dogleg Iterate"); - DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); - toc(1, "Dogleg Iterate"); - // Update Delta and linear step - doglegDelta_ = doglegResult.Delta; - delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation - delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution - } - toc(9,"solve"); + if(debug) delta_.print("delta_: "); + //toc(9,"solve"); tic(10,"evaluate error after"); if(params_.evaluateNonlinearError) @@ -576,10 +552,48 @@ ISAM2Result ISAM2::update( toc(10,"evaluate error after"); result.cliques = this->nodes().size(); + deltaUptodate_ = false; return result; } + +/* ************************************************************************* */ +template +void ISAM2::updateDelta(bool forceFullSolve) const { + + if(params_.optimizationParams.type() == typeid(ISAM2GaussNewtonParams)) { + // If using Gauss-Newton, update with wildfireThreshold + const ISAM2GaussNewtonParams& gaussNewtonParams = + boost::get(params_.optimizationParams); + const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; + tic(0, "Wildfire update"); + lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold); + toc(0, "Wildfire update"); + + } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { + // If using Dogleg, do a Dogleg step + const ISAM2DoglegParams& doglegParams = + boost::get(params_.optimizationParams); + + // Do one Dogleg iteration + tic(1, "Dogleg Iterate"); + DoglegOptimizerImpl::IterationResult doglegResult = DoglegOptimizerImpl::Iterate( + *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose); + toc(1, "Dogleg Iterate"); + + // Update Delta and linear step + doglegDelta_ = doglegResult.Delta; + delta_.permutation() = Permutation::Identity(delta_.size()); // Dogleg solves for the full delta so there is no permutation + delta_.container() = doglegResult.dx_d; // Copy the VectorValues containing with the linear solution + + // Clear replaced mask + deltaReplacedMask_.assign(deltaReplacedMask_.size(), false); + } + + deltaUptodate_ = true; +} + /* ************************************************************************* */ template Values ISAM2::calculateEstimate() const { @@ -587,7 +601,7 @@ Values ISAM2::calculateEstimate() const { // handles Permuted Values ret(theta_); vector mask(ordering_.nVars(), true); - Impl::ExpmapMasked(ret, delta_, ordering_, mask); + Impl::ExpmapMasked(ret, getDelta(), ordering_, mask); return ret; } @@ -608,6 +622,14 @@ Values ISAM2::calculateBestEstimate() const { return theta_.retract(delta, ordering_); } +/* ************************************************************************* */ +template +const Permuted& ISAM2::getDelta() const { + if(!deltaUptodate_) + updateDelta(); + return delta_; +} + /* ************************************************************************* */ template VectorValues optimize(const ISAM2& isam) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 4a4bdea2c..1117a1a48 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -283,19 +283,42 @@ protected: /** The linear delta from the last linear solution, an update to the estimate in theta */ VectorValues deltaUnpermuted_; - /** @brief The permutation through which the deltaUnpermuted_ is + /** The permutation through which the deltaUnpermuted_ is * referenced. * * Permuting Vector entries would be slow, so for performance we * instead maintain this permutation through which we access the linear delta * indirectly + * + * This is \c mutable because it is a "cached" variable - it is not updated + * until either requested with getDelta() or calculateEstimate(), or needed + * during update() to evaluate whether to relinearize variables. */ - Permuted delta_; + mutable Permuted delta_; + + /** Indicates whether the current delta is up-to-date, only used + * internally - delta will always be updated if necessary when it is + * requested with getDelta() or calculateEstimate(). + * + * This is \c mutable because it is used internally to not update delta_ + * until it is needed. + */ + mutable bool deltaUptodate_; + + /** A cumulative mask for the variables that were replaced and have not yet + * been updated in the linear solution delta_, this is only used internally, + * delta will always be updated if necessary when requested with getDelta() + * or calculateEstimate(). + * + * This is \c mutable because it is used internally to not update delta_ + * until it is needed. + */ + mutable std::vector deltaReplacedMask_; /** All original nonlinear factors are stored here to use during relinearization */ GRAPH nonlinearFactors_; - /** @brief The current elimination ordering Symbols to Index (integer) keys. + /** The current elimination ordering Symbols to Index (integer) keys. * * We keep it up to date as we add and reorder variables. */ @@ -305,7 +328,7 @@ protected: ISAM2Params params_; /** The current Dogleg Delta (trust region radius) */ - boost::optional doglegDelta_; + mutable boost::optional doglegDelta_; private: #ifndef NDEBUG @@ -337,6 +360,8 @@ public: newISAM2->variableIndex_ = variableIndex_; newISAM2->deltaUnpermuted_ = deltaUnpermuted_; newISAM2->delta_ = delta_; + newISAM2->deltaUptodate_ = deltaUptodate_; + newISAM2->deltaReplacedMask_ = deltaReplacedMask_; newISAM2->nonlinearFactors_ = nonlinearFactors_; newISAM2->ordering_ = ordering_; newISAM2->params_ = params_; @@ -404,7 +429,7 @@ public: Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ - const Permuted& getDelta() const { return delta_; } + const Permuted& getDelta() const; /** Access the set of nonlinear factors */ const GRAPH& getFactorsUnsafe() const { return nonlinearFactors_; } @@ -416,7 +441,7 @@ public: size_t lastAffectedFactorCount; size_t lastAffectedCliqueCount; size_t lastAffectedMarkedCount; - size_t lastBacksubVariableCount; + mutable size_t lastBacksubVariableCount; size_t lastNnzTop; ISAM2Params params() const { return params_; } @@ -433,6 +458,7 @@ private: const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); + void updateDelta(bool forceFullSolve = false) const; }; // ISAM2 diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index f41cff097..82ddb96cd 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -47,6 +47,8 @@ TEST(ISAM2, AddVariables) { Permuted delta(permutation, deltaUnpermuted); + vector replacedKeys(2, false); + Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); GaussianISAM2<>::Nodes nodes(2); @@ -75,17 +77,20 @@ TEST(ISAM2, AddVariables) { Permuted deltaExpected(permutationExpected, deltaUnpermutedExpected); + vector replacedKeysExpected(3, false); + Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); GaussianISAM2<>::Nodes nodesExpected( 3, GaussianISAM2<>::sharedClique()); // Expand initial state - GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes); + GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, replacedKeys, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); EXPECT(assert_equal(deltaExpected.permutation(), delta.permutation())); + EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); EXPECT(assert_equal(orderingExpected, ordering)); }