From 91e7dc5882937636e7b6b129ae9c6aa17bc8dcbc Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 11 Apr 2012 14:17:59 +0000 Subject: [PATCH] Caching linearized factors in iSAM2, improves speed when linearization is expensive Merge remote-tracking branch 'svn/branches/iSAM2_cache_linearized' into trunk Conflicts: .cproject --- gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/GaussianFactorGraph.cpp | 3 +- gtsam/linear/HessianFactor.h | 8 -- gtsam/linear/JacobianFactor.cpp | 37 -------- gtsam/linear/JacobianFactor.h | 7 -- .../linear/tests/testGaussianFactorGraph.cpp | 2 +- gtsam/nonlinear/ISAM2-impl.cpp | 3 + gtsam/nonlinear/ISAM2.cpp | 87 +++++++++++++------ gtsam/nonlinear/ISAM2.h | 19 +++- 9 files changed, 81 insertions(+), 87 deletions(-) diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 2d889ee64..3fdad19c6 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -96,7 +96,7 @@ namespace gtsam { * to already be inverted. This acts just as a change-of-name for each * variable. The order of the variables within the factor is not changed. */ - virtual void permuteWithInverse(const Permutation& inversePermutation) = 0; + virtual void permuteWithInverse(const Permutation& inversePermutation) { IndexFactor::permuteWithInverse(inversePermutation); } private: /** Serialization function */ diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 56a377c61..0241bc3b3 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -51,7 +51,8 @@ namespace gtsam { void GaussianFactorGraph::permuteWithInverse( const Permutation& inversePermutation) { BOOST_FOREACH(const sharedFactor& factor, factors_) { - factor->permuteWithInverse(inversePermutation); + if(factor) + factor->permuteWithInverse(inversePermutation); } } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 7cba0c744..91e048b3c 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -270,14 +270,6 @@ namespace gtsam { * @return The linear term \f$ g \f$ */ constColumn linearTerm() const; - /** - * Permutes the GaussianFactor, but for efficiency requires the permutation - * to already be inverted. This acts just as a change-of-name for each - * variable. The order of the variables within the factor is not changed. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation) { - IndexFactor::permuteWithInverse(inversePermutation); } - // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; friend class ::Constructor1HessianFactorTest; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index c48a62431..688e23fe4 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -250,43 +250,6 @@ namespace gtsam { } } - /* ************************************************************************* */ - void JacobianFactor::permuteWithInverse(const Permutation& inversePermutation) { - - // Build a map from the new variable indices to the old slot positions. - typedef FastMap SourceSlots; - SourceSlots sourceSlots; - for(size_t j=0; j dimensions(size() + 1); - size_t j = 0; - BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) { - dimensions[j++] = Ab_(sourceSlot.second).cols(); - } - assert(j == size()); - dimensions.back() = 1; - - // Copy the variables and matrix into the new order - vector oldKeys(size()); - keys_.swap(oldKeys); - AbMatrix oldMatrix; - BlockAb oldAb(oldMatrix, dimensions.begin(), dimensions.end(), Ab_.rows()); - Ab_.swap(oldAb); - j = 0; - BOOST_FOREACH(const SourceSlots::value_type& sourceSlot, sourceSlots) { - keys_[j] = sourceSlot.first; - Ab_(j++).noalias() = oldAb(sourceSlot.second); - } - Ab_(j).noalias() = oldAb(j); - - // Since we're permuting the variables, ensure that entire rows from this - // factor are copied when Combine is called - BOOST_FOREACH(size_t& varpos, firstNonzeroBlocks_) { varpos = 0; } - assertInvariants(); - } - /* ************************************************************************* */ Vector JacobianFactor::unweighted_error(const VectorValues& c) const { Vector e = -getb(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index ec661066d..5a756e43b 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -169,13 +169,6 @@ namespace gtsam { */ virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } - /** - * Permutes the GaussianFactor, but for efficiency requires the permutation - * to already be inverted. This acts just as a change-of-name for each - * variable. The order of the variables within the factor is not changed. - */ - virtual void permuteWithInverse(const Permutation& inversePermutation); - /** * return the number of rows in the corresponding linear system */ diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index ade5e65ce..34009f856 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -552,7 +552,7 @@ TEST(GaussianFactor, permuteWithInverse) actual.permuteWithInverse(inversePermutation); // actualIndex.permute(*inversePermutation.inverse()); - JacobianFactor expected(0, A3, 2, A2, 4, A1, b, sharedSigma(2, 1.0)); + JacobianFactor expected(4, A1, 2, A2, 0, A3, b, sharedSigma(2, 1.0)); GaussianFactorGraph expectedFG; expectedFG.push_back(JacobianFactor::shared_ptr(new JacobianFactor(expected))); // GaussianVariableIndex expectedIndex(expectedFG); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index e4cb2abd3..ec3b4edfd 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -257,6 +257,9 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, cout << "Full var-ordered eliminated BT:\n"; result.bayesTree->printTree(""); } + // Undo permutation on our subset of cached factors, we must later permute *all* of the cached factors + factors.permuteWithInverse(*affectedColamd); + factors.permuteWithInverse(affectedKeysSelector); toc(8,"permute eliminated"); return result; diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 6bb74dd8a..d8138366b 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -78,6 +78,12 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) { deltaUptodate_ = rhs.deltaUptodate_; deltaReplacedMask_ = rhs.deltaReplacedMask_; nonlinearFactors_ = rhs.nonlinearFactors_; + + linearFactors_ = GaussianFactorGraph(); + linearFactors_.reserve(rhs.linearFactors_.size()); + BOOST_FOREACH(const GaussianFactor::shared_ptr& linearFactor, rhs.linearFactors_) { + linearFactors_.push_back(linearFactor->clone()); } + ordering_ = rhs.ordering_; params_ = rhs.params_; doglegDelta_ = rhs.doglegDelta_; @@ -125,7 +131,7 @@ FastList ISAM2::getAffectedFactors(const FastList& keys) const { // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const { tic(1,"getAffectedFactors"); FastList candidates = getAffectedFactors(affectedKeys); @@ -139,24 +145,37 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); toc(2,"affectedKeysSet"); - tic(3,"check candidates"); + tic(3,"check candidates and linearize"); + FactorGraph::shared_ptr linearized = boost::make_shared >(); BOOST_FOREACH(size_t idx, candidates) { bool inside = true; + bool useCachedLinear = params_.cacheLinearizedFactors; BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { Index var = ordering_[key]; - if (affectedKeysSet.find(var) == affectedKeysSet.end()) { + if(affectedKeysSet.find(var) == affectedKeysSet.end()) { inside = false; break; } + if(useCachedLinear && relinKeys.find(var) != relinKeys.end()) + useCachedLinear = false; + } + if(inside) { + if(useCachedLinear) { + assert(linearFactors_[idx]); + linearized->push_back(linearFactors_[idx]); + assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->symbolic(ordering_)->keys()); + } else { + GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_); + linearized->push_back(linearFactor); + if(params_.cacheLinearizedFactors) { + assert(linearFactors_[idx]->keys() == linearFactor->keys()); + linearFactors_[idx] = linearFactor; + } + } } - if (inside) - nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); } - toc(3,"check candidates"); + toc(3,"check candidates and linearize"); - tic(4,"linearize"); - FactorGraph::shared_ptr linearized(nonlinearAffectedFactors.linearize(theta_, ordering_)); - toc(4,"linearize"); return linearized; } @@ -187,7 +206,7 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { } boost::shared_ptr > ISAM2::recalculate( - const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, + const FastSet& markedKeys, const FastSet& relinKeys, const FastVector& newKeys, const boost::optional >& constrainKeys, ISAM2Result& result) { // TODO: new factors are linearized twice, the newFactors passed in are not used. @@ -300,11 +319,11 @@ boost::shared_ptr > ISAM2::recalculate( toc(1,"reorder"); tic(2,"linearize"); - GaussianFactorGraph factors(*nonlinearFactors_.linearize(theta_, ordering_)); + linearFactors_ = *nonlinearFactors_.linearize(theta_, ordering_); toc(2,"linearize"); tic(5,"eliminate"); - JunctionTree jt(factors, variableIndex_); + JunctionTree jt(linearFactors_, variableIndex_); sharedClique newRoot; if(params_.factorization == ISAM2Params::LDL) newRoot = jt.eliminate(EliminatePreferLDL); @@ -325,7 +344,7 @@ boost::shared_ptr > ISAM2::recalculate( lastAffectedMarkedCount = markedKeys.size(); lastAffectedVariableCount = affectedKeysSet->size(); - lastAffectedFactorCount = factors.size(); + lastAffectedFactorCount = linearFactors_.size(); return affectedKeysSet; @@ -338,7 +357,7 @@ boost::shared_ptr > ISAM2::recalculate( affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); affectedAndNewKeys.insert(affectedAndNewKeys.end(), newKeys.begin(), newKeys.end()); tic(1,"relinearizeAffected"); - GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys)); + GaussianFactorGraph factors(*relinearizeAffectedFactors(affectedAndNewKeys, relinKeys)); if(debug) factors.print("Relinearized factors: "); toc(1,"relinearizeAffected"); @@ -360,11 +379,7 @@ boost::shared_ptr > ISAM2::recalculate( // add the cached intermediate results from the boundary of the orphans ... GaussianFactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); if(debug) cachedBoundary.print("Boundary factors: "); - factors.reserve(factors.size() + cachedBoundary.size()); - // Copy so that we can later permute factors - BOOST_FOREACH(const GaussianFactor::shared_ptr& cached, cachedBoundary) { - factors.push_back(cached->clone()); - } + factors.push_back(cachedBoundary); toc(2,"cached"); // END OF COPIED CODE @@ -410,6 +425,15 @@ boost::shared_ptr > ISAM2::recalculate( tic(5,"permute ordering"); ordering_.permuteWithInverse(partialSolveResult.fullReorderingInverse); toc(5,"permute ordering"); + if(params_.cacheLinearizedFactors) { + tic(6,"permute cached linear"); + //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse); + FastList permuteLinearIndices = getAffectedFactors(affectedAndNewKeys); + BOOST_FOREACH(size_t idx, permuteLinearIndices) { + linearFactors_[idx]->permuteWithInverse(partialSolveResult.fullReorderingInverse); + } + toc(6,"permute cached linear"); + } toc(4,"reorder and eliminate"); @@ -526,11 +550,12 @@ ISAM2Result ISAM2::update( toc(4,"gather involved keys"); // Check relinearization if we're at the nth step, or we are using a looser loop relin threshold + FastSet relinKeys; 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); + relinKeys = Impl::CheckRelinearization(delta_, ordering_, params_.relinearizeThreshold); if(disableReordering) relinKeys = Impl::CheckRelinearization(delta_, ordering_, 0.0); // This is used for debugging // Add the variables being relinearized to the marked keys @@ -563,15 +588,21 @@ ISAM2Result ISAM2::update( } tic(8,"linearize new"); - tic(1,"linearize"); // 7. Linearize new factors - FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); - toc(1,"linearize"); + if(params_.cacheLinearizedFactors) { + tic(1,"linearize"); + FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); + linearFactors_.push_back(*linearFactors); + assert(nonlinearFactors_.size() == linearFactors_.size()); + toc(1,"linearize"); - tic(2,"augment VI"); - // Augment the variable index with the new factors - variableIndex_.augment(*linearFactors); - toc(2,"augment VI"); + tic(2,"augment VI"); + // Augment the variable index with the new factors + variableIndex_.augment(*linearFactors); + toc(2,"augment VI"); + } else { + variableIndex_.augment(*newFactors.symbolic(ordering_)); + } toc(8,"linearize new"); tic(9,"recalculate"); @@ -587,7 +618,7 @@ ISAM2Result ISAM2::update( } boost::shared_ptr > replacedKeys; if(!markedKeys.empty() || !newKeys.empty()) - replacedKeys = recalculate(markedKeys, newKeys, linearFactors, constrainedIndices, result); + replacedKeys = recalculate(markedKeys, relinKeys, newKeys, constrainedIndices, result); // Update replaced keys mask (accumulates until back-substitution takes place) if(replacedKeys) { diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 5e7adc19d..21f929ed9 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -118,6 +118,13 @@ struct ISAM2Params { */ Factorization factorization; + /** Whether to cache linear factors (default: true). + * This can improve performance if linearization is expensive, but can hurt + * performance if linearization is very cleap due to computation to look up + * additional keys. + */ + bool cacheLinearizedFactors; + KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter) /** Specify parameters as constructor arguments */ @@ -128,11 +135,12 @@ struct ISAM2Params { bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError Factorization _factorization = ISAM2Params::LDL, ///< see ISAM2Params::factorization + bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), evaluateNonlinearError(_evaluateNonlinearError), factorization(_factorization), - keyFormatter(_keyFormatter) {} + cacheLinearizedFactors(_cacheLinearizedFactors), keyFormatter(_keyFormatter) {} }; /** @@ -341,6 +349,9 @@ protected: /** All original nonlinear factors are stored here to use during relinearization */ NonlinearFactorGraph nonlinearFactors_; + /** The current linear factors, which are only updated as needed */ + mutable GaussianFactorGraph linearFactors_; + /** The current elimination ordering Symbols to Index (integer) keys. * * We keep it up to date as we add and reorder variables. @@ -453,11 +464,11 @@ public: private: FastList getAffectedFactors(const FastList& keys) const; - FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys) const; + FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); - boost::shared_ptr > recalculate(const FastSet& markedKeys, - const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, + boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, + const FastVector& newKeys, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); void updateDelta(bool forceFullSolve = false) const;