diff --git a/gtsam/inference/BayesTreeCliqueBase-inst.h b/gtsam/inference/BayesTreeCliqueBase-inst.h index 90ca98863..eb736f663 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inst.h +++ b/gtsam/inference/BayesTreeCliqueBase-inst.h @@ -22,6 +22,14 @@ namespace gtsam { + /* ************************************************************************* */ + template + void BayesTreeCliqueBase::setEliminationResult( + const typename FactorGraphType::EliminationResult& eliminationResult) + { + conditional_ = eliminationResult.first; + } + /* ************************************************************************* */ template bool BayesTreeCliqueBase::equals( diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index acf8156ab..56f4744fe 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -80,6 +80,11 @@ namespace gtsam { derived_weak_ptr parent_; std::vector children; + /// Fill the elimination result produced during elimination. Here this just stores the + /// conditional and ignores the remaining factor, but this is overridden in ISAM2Clique + /// to also cache the remaining factor. + void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult); + /// @name Testable /// @{ diff --git a/gtsam/inference/JunctionTree-inst.h b/gtsam/inference/JunctionTree-inst.h index e4e3e6630..cbf71a111 100644 --- a/gtsam/inference/JunctionTree-inst.h +++ b/gtsam/inference/JunctionTree-inst.h @@ -195,8 +195,8 @@ namespace gtsam { std::pair, boost::shared_ptr > eliminationResult = eliminationFunction(gatheredFactors, Ordering(node->keys)); - // Store conditional in BayesTree clique - myData.bayesTreeNode->conditional_ = eliminationResult.first; + // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor + myData.bayesTreeNode->setEliminationResult(eliminationResult); // Store remaining factor in parent's gathered factors if(!eliminationResult.second->empty()) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 871d8c688..fefb6d328 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -15,14 +15,13 @@ * @author Michael Kaess, Richard Roberts */ -#if 0 - #include #include // for operator += using namespace boost::assign; #include #include #include +namespace br { using namespace boost::range; using namespace boost::adaptors; } #include #include @@ -85,6 +84,38 @@ std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorizatio return s; } +/* ************************************************************************* */ +void ISAM2Clique::setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult) +{ + conditional_ = eliminationResult.first; + cachedFactor_ = eliminationResult.second; + // Compute gradient contribution + gradientContribution_.resize(conditional_->cols() - 1); + // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons + gradientContribution_ = -conditional_->get_R().transpose() * conditional_->get_d(), + -conditional_->get_S().transpose() * conditional_->get_d(); +} + +/* ************************************************************************* */ +bool ISAM2Clique::equals(const This& other, double tol) const { + return Base::equals(other) && + ((!cachedFactor_ && !other.cachedFactor_) + || (cachedFactor_ && other.cachedFactor_ + && cachedFactor_->equals(*other.cachedFactor_, tol))); +} + +/* ************************************************************************* */ +void ISAM2Clique::print(const std::string& s, const KeyFormatter& formatter) const +{ + Base::print(s,formatter); + if(cachedFactor_) + cachedFactor_->print(s + "Cached: ", formatter); + else + std::cout << s << "Cached empty" << std::endl; + if(gradientContribution_.rows() != 0) + gtsam::print(gradientContribution_, "Gradient contribution: "); +} + /* ************************************************************************* */ ISAM2::ISAM2(const ISAM2Params& params): deltaDoglegUptodate_(true), deltaUptodate_(true), params_(params) { @@ -105,14 +136,12 @@ ISAM2::ISAM2(const ISAM2& other) { } /* ************************************************************************* */ -ISAM2& ISAM2::operator=(const ISAM2& rhs) { +ISAM2& ISAM2::operator=(const ISAM2& rhs) +{ // Copy BayesTree this->Base::operator=(rhs); // Copy our variables - // When we have Permuted<...>, it is only necessary to copy this permuted - // view and not the original, because copying the permuted view automatically - // copies the original. theta_ = rhs.theta_; variableIndex_ = rhs.variableIndex_; delta_ = rhs.delta_; @@ -122,16 +151,9 @@ 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 ? linearFactor->clone() : GaussianFactor::shared_ptr()); } - - ordering_ = rhs.ordering_; + linearFactors_ = rhs.linearFactors_; params_ = rhs.params_; doglegDelta_ = rhs.doglegDelta_; - lastAffectedVariableCount = rhs.lastAffectedVariableCount; lastAffectedFactorCount = rhs.lastAffectedFactorCount; lastAffectedCliqueCount = rhs.lastAffectedCliqueCount; @@ -143,15 +165,15 @@ ISAM2& ISAM2::operator=(const ISAM2& rhs) { } /* ************************************************************************* */ -FastList ISAM2::getAffectedFactors(const FastList& keys) const { +FastList ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; - if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } + if(debug) { BOOST_FOREACH(const Key key, keys) { cout << key << " "; } } if(debug) cout << endl; - FactorGraph allAffected; + NonlinearFactorGraph allAffected; FastList indices; - BOOST_FOREACH(const Index key, keys) { + BOOST_FOREACH(const Key key, keys) { // const list l = nonlinearFactors_.factors(key); // indices.insert(indices.begin(), l.begin(), l.end()); const VariableIndex::Factors& factors(variableIndex_[key]); @@ -172,9 +194,9 @@ 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 FastSet& relinKeys) const { - +GaussianFactorGraph::shared_ptr +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const +{ gttic(getAffectedFactors); FastList candidates = getAffectedFactors(affectedKeys); gttoc(getAffectedFactors); @@ -183,33 +205,32 @@ ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys, const Fas gttic(affectedKeysSet); // for fast lookup below - FastSet affectedKeysSet; + FastSet affectedKeysSet; affectedKeysSet.insert(affectedKeys.begin(), affectedKeys.end()); gttoc(affectedKeysSet); gttic(check_candidates_and_linearize); - FactorGraph::shared_ptr linearized = boost::make_shared >(); + GaussianFactorGraph::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(key) == affectedKeysSet.end()) { inside = false; break; } - if(useCachedLinear && relinKeys.find(var) != relinKeys.end()) + if(useCachedLinear && relinKeys.find(key) != relinKeys.end()) useCachedLinear = false; } if(inside) { if(useCachedLinear) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS assert(linearFactors_[idx]); - assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->symbolic(ordering_)->keys()); + assert(linearFactors_[idx]->keys() == nonlinearFactors_[idx]->keys()); #endif linearized->push_back(linearFactors_[idx]); } else { - GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_, ordering_); + GaussianFactor::shared_ptr linearFactor = nonlinearFactors_[idx]->linearize(theta_); linearized->push_back(linearFactor); if(params_.cacheLinearizedFactors) { #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS @@ -235,20 +256,20 @@ GaussianFactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { GaussianFactorGraph cachedBoundary; BOOST_FOREACH(sharedClique orphan, orphans) { - // find the last variable that was eliminated - Index key = (*orphan)->frontals().back(); // retrieve the cached factor and add to boundary cachedBoundary.push_back(orphan->cachedFactor()); - if(debug) { cout << "Cached factor for variable " << key; orphan->cachedFactor()->print(""); } } return cachedBoundary; } -boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, - const FastSet& relinKeys, const FastVector& observedKeys, const FastSet& unusedIndices, - const boost::optional >& constrainKeys, ISAM2Result& result) { - +/* ************************************************************************* */ +boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKeys, const FastSet& relinKeys, + const FastVector& observedKeys, + const FastSet& unusedIndices, + const boost::optional >& constrainKeys, + ISAM2Result& result) +{ // TODO: new factors are linearized twice, the newFactors passed in are not used. const bool debug = ISDEBUG("ISAM2 recalculate"); @@ -275,10 +296,10 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark if(debug) { cout << "markedKeys: "; - BOOST_FOREACH(const Index key, markedKeys) { cout << key << " "; } + BOOST_FOREACH(const Key key, markedKeys) { cout << key << " "; } cout << endl; cout << "observedKeys: "; - BOOST_FOREACH(const Index key, observedKeys) { cout << key << " "; } + BOOST_FOREACH(const Key key, observedKeys) { cout << key << " "; } cout << endl; } @@ -287,7 +308,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. gttic(removetop); Cliques orphans; - BayesNet affectedBayesNet; + GaussianBayesNet affectedBayesNet; this->removeTop(markedKeys, affectedBayesNet, orphans); gttoc(removetop); @@ -307,17 +328,19 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark // ordering provides all keys in conditionals, there cannot be others because path to root included gttic(affectedKeys); - FastList affectedKeys = affectedBayesNet.ordering(); + FastList affectedKeys; + BOOST_FOREACH(const ConditionalType::shared_ptr& conditional, affectedBayesNet) + affectedKeys.insert(affectedKeys.end(), conditional->beginFrontals(), conditional->endFrontals()); gttoc(affectedKeys); - boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result - - if(affectedKeys.size() >= theta_.size() * batchThreshold) { + boost::shared_ptr > affectedKeysSet(new FastSet()); // Will return this result + if(affectedKeys.size() >= theta_.size() * batchThreshold) + { gttic(batch); gttic(add_keys); - BOOST_FOREACH(const Ordering::value_type& key_index, ordering_) { affectedKeysSet->insert(key_index.second); } + br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end())); gttoc(add_keys); gttic(reorder); @@ -341,6 +364,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& mark BOOST_FOREACH(Index var, observedKeys) { cmember[var] = 1; } } } + Ordering order = Ordering::COLAMDConstrained() Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); Permutation::shared_ptr colamdInverse(colamd->inverse()); gttoc(CCOLAMD); @@ -1163,5 +1187,3 @@ void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) { } /// namespace gtsam - -#endif diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 59df28db6..2ab434c61 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -19,8 +19,6 @@ #pragma once -#if 0 - #include #include #include @@ -344,10 +342,11 @@ struct GTSAM_EXPORT ISAM2Result { * Specialized Clique structure for ISAM2, incorporating caching and gradient contribution * TODO: more documentation */ -class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase { +class GTSAM_EXPORT ISAM2Clique : public BayesTreeCliqueBase +{ public: typedef ISAM2Clique This; - typedef BayesTreeCliqueBase Base; + typedef BayesTreeCliqueBase Base; typedef boost::shared_ptr shared_ptr; typedef boost::weak_ptr weak_ptr; typedef GaussianConditional ConditionalType; @@ -356,29 +355,8 @@ public: Base::FactorType::shared_ptr cachedFactor_; Vector gradientContribution_; - /** Construct from a conditional */ - ISAM2Clique(const sharedConditional& conditional) : Base(conditional) { - throw std::runtime_error("ISAM2Clique should always be constructed with the elimination result constructor"); } - - /** Construct from an elimination result */ - ISAM2Clique(const std::pair >& result) : - Base(result.first), cachedFactor_(result.second), - gradientContribution_(result.first->get_R().cols() + result.first->get_S().cols()) { - // Compute gradient contribution - const ConditionalType& conditional(*result.first); - // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons - gradientContribution_ << -conditional.get_R().transpose() * conditional.get_d(), - -conditional.get_S().transpose() * conditional.get_d(); - } - - /** Produce a deep copy, copying the cached factor and gradient contribution */ - shared_ptr clone() const { - shared_ptr copy(new ISAM2Clique(std::make_pair( - sharedConditional(new ConditionalType(*Base::conditional_)), - cachedFactor_ ? cachedFactor_->clone() : Base::FactorType::shared_ptr()))); - copy->gradientContribution_ = gradientContribution_; - return copy; - } + /// Overridden to also store the remaining factor and gradient contribution + void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult); /** Access the cached factor */ Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } @@ -386,35 +364,10 @@ public: /** Access the gradient contribution */ const Vector& gradientContribution() const { return gradientContribution_; } - bool equals(const This& other, double tol=1e-9) const { - return Base::equals(other) && - ((!cachedFactor_ && !other.cachedFactor_) - || (cachedFactor_ && other.cachedFactor_ - && cachedFactor_->equals(*other.cachedFactor_, tol))); - } + bool equals(const This& other, double tol=1e-9) const; /** print this node */ - void print(const std::string& s = "", - const IndexFormatter& formatter = DefaultIndexFormatter) const { - Base::print(s,formatter); - if(cachedFactor_) - cachedFactor_->print(s + "Cached: ", formatter); - else - std::cout << s << "Cached empty" << std::endl; - if(gradientContribution_.rows() != 0) - gtsam::print(gradientContribution_, "Gradient contribution: "); - } - - void permuteWithInverse(const Permutation& inversePermutation) { - if(cachedFactor_) cachedFactor_->permuteWithInverse(inversePermutation); - Base::permuteWithInverse(inversePermutation); - } - - bool reduceSeparatorWithInverse(const internal::Reduction& inverseReduction) { - bool changed = Base::reduceSeparatorWithInverse(inverseReduction); - if(changed) if(cachedFactor_) cachedFactor_->reduceWithInverse(inverseReduction); - return changed; - } + void print(const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const; private: @@ -438,7 +391,7 @@ private: * estimate of all variables. * */ -class ISAM2: public BayesTree { +class ISAM2: public BayesTree { protected: @@ -481,7 +434,7 @@ protected: * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable std::vector deltaReplacedMask_; + mutable FastMap deltaReplacedMask_; // TODO: Make sure accessed in the right way /** All original nonlinear factors are stored here to use during relinearization */ NonlinearFactorGraph nonlinearFactors_; @@ -489,10 +442,6 @@ protected: /** 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. */ - Ordering ordering_; - /** The current parameters */ ISAM2Params params_; @@ -506,7 +455,7 @@ protected: public: typedef ISAM2 This; ///< This class - typedef BayesTree Base; ///< The BayesTree base class + typedef BayesTree Base; ///< The BayesTree base class /** Create an empty ISAM2 instance */ GTSAM_EXPORT ISAM2(const ISAM2Params& params); @@ -615,9 +564,6 @@ public: /** Access the set of nonlinear factors */ GTSAM_EXPORT const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } - /** Access the current ordering */ - GTSAM_EXPORT const Ordering& getOrdering() const { return ordering_; } - /** Access the nonlinear variable index */ GTSAM_EXPORT const VariableIndex& getVariableIndex() const { return variableIndex_; } @@ -637,12 +583,12 @@ public: private: - GTSAM_EXPORT FastList getAffectedFactors(const FastList& keys) const; - GTSAM_EXPORT FactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; + GTSAM_EXPORT FastList getAffectedFactors(const FastList& keys) const; + GTSAM_EXPORT GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; GTSAM_EXPORT GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); - GTSAM_EXPORT boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, - const FastVector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); + GTSAM_EXPORT boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, + const FastVector& observedKeys, const FastSet& unusedIndices, const boost::optional >& constrainKeys, ISAM2Result& result); // void linear_update(const GaussianFactorGraph& newFactors); GTSAM_EXPORT void updateDelta(bool forceFullSolve = false) const; @@ -740,5 +686,3 @@ GTSAM_EXPORT void gradientAtZero(const ISAM2& bayesTree, VectorValues& g); #include #include - -#endif