From c0ccec465602e1cd9bd6d302add6e0dd55af4da5 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 7 Aug 2013 02:56:39 +0000 Subject: [PATCH 02/18] Working on ISAM2 --- gtsam/inference/BayesTreeCliqueBase-inst.h | 8 ++ gtsam/inference/BayesTreeCliqueBase.h | 5 + gtsam/inference/JunctionTree-inst.h | 4 +- gtsam/nonlinear/ISAM2.cpp | 112 ++++++++++++--------- gtsam/nonlinear/ISAM2.h | 84 +++------------- 5 files changed, 96 insertions(+), 117 deletions(-) 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 From 5b15b11261ce81eae6d3dc6cfd7f2de250fc1006 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 21:41:23 +0000 Subject: [PATCH 03/18] Added FastSet/Map exists function --- gtsam/base/FastMap.h | 3 +++ gtsam/base/FastSet.h | 3 +++ 2 files changed, 6 insertions(+) diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 7cea45e88..656eb74fd 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -58,6 +58,9 @@ public: return std::map(this->begin(), this->end()); } + /** Handy 'exists' function */ + bool exists(const KEY& e) const { return find(e) != end(); } + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index be2864f8f..fe4cc2c80 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -91,6 +91,9 @@ public: return std::set(this->begin(), this->end()); } + /** Handy 'exists' function */ + bool exists(const VALUE& e) const { return find(e) != end(); } + /** Print to implement Testable */ void print(const std::string& str = "") const { FastSetTestableHelper::print(*this, str); } From 731bfe49734aa3d573789c9f9b30d45b6d3911f6 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 21:41:28 +0000 Subject: [PATCH 04/18] Removed unneeded derived class copy constructors and assignment operators --- gtsam/linear/GaussianBayesTree.cpp | 11 ----------- gtsam/linear/GaussianBayesTree.h | 8 -------- gtsam/linear/GaussianEliminationTree.cpp | 12 ------------ gtsam/linear/GaussianEliminationTree.h | 8 -------- gtsam/linear/GaussianJunctionTree.cpp | 11 ----------- gtsam/linear/GaussianJunctionTree.h | 8 -------- gtsam/symbolic/SymbolicBayesTree.cpp | 11 ----------- gtsam/symbolic/SymbolicBayesTree.h | 8 -------- gtsam/symbolic/SymbolicEliminationTree.cpp | 12 ------------ gtsam/symbolic/SymbolicEliminationTree.h | 8 -------- gtsam/symbolic/SymbolicJunctionTree.cpp | 11 ----------- gtsam/symbolic/SymbolicJunctionTree.h | 8 -------- 12 files changed, 116 deletions(-) diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index 63dcefeb7..bb25c9a45 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -99,17 +99,6 @@ namespace gtsam { } } - /* ************************************************************************* */ - GaussianBayesTree::GaussianBayesTree(const GaussianBayesTree& other) : - Base(other) {} - - /* ************************************************************************* */ - GaussianBayesTree& GaussianBayesTree::operator=(const GaussianBayesTree& other) - { - (void) Base::operator=(other); - return *this; - } - /* ************************************************************************* */ bool GaussianBayesTree::equals(const This& other, double tol) const { diff --git a/gtsam/linear/GaussianBayesTree.h b/gtsam/linear/GaussianBayesTree.h index c30e9bf48..86d88e95b 100644 --- a/gtsam/linear/GaussianBayesTree.h +++ b/gtsam/linear/GaussianBayesTree.h @@ -59,14 +59,6 @@ namespace gtsam { /** Default constructor, creates an empty Bayes tree */ GaussianBayesTree() {} - /** Makes a deep copy of the tree structure, but only pointers to conditionals are - * copied, the conditionals and their matrices are not cloned. */ - GaussianBayesTree(const GaussianBayesTree& other); - - /** Makes a deep copy of the tree structure, but only pointers to conditionals are - * copied, the conditionals and their matrices are not cloned. */ - GaussianBayesTree& operator=(const GaussianBayesTree& other); - /** Check equality */ bool equals(const This& other, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianEliminationTree.cpp b/gtsam/linear/GaussianEliminationTree.cpp index 5933643bf..0daaf0707 100644 --- a/gtsam/linear/GaussianEliminationTree.cpp +++ b/gtsam/linear/GaussianEliminationTree.cpp @@ -35,18 +35,6 @@ namespace gtsam { const GaussianFactorGraph& factorGraph, const Ordering& order) : Base(factorGraph, order) {} - /* ************************************************************************* */ - GaussianEliminationTree::GaussianEliminationTree( - const This& other) : - Base(other) {} - - /* ************************************************************************* */ - GaussianEliminationTree& GaussianEliminationTree::operator=(const This& other) - { - (void) Base::operator=(other); - return *this; - } - /* ************************************************************************* */ bool GaussianEliminationTree::equals(const This& other, double tol) const { diff --git a/gtsam/linear/GaussianEliminationTree.h b/gtsam/linear/GaussianEliminationTree.h index a3d989416..1a4ba7868 100644 --- a/gtsam/linear/GaussianEliminationTree.h +++ b/gtsam/linear/GaussianEliminationTree.h @@ -51,14 +51,6 @@ namespace gtsam { GaussianEliminationTree(const GaussianFactorGraph& factorGraph, const Ordering& order); - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - GaussianEliminationTree(const This& other); - - /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - This& operator=(const This& other); - /** Test whether the tree is equal to another */ bool equals(const This& other, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianJunctionTree.cpp b/gtsam/linear/GaussianJunctionTree.cpp index ae5f116c2..182375925 100644 --- a/gtsam/linear/GaussianJunctionTree.cpp +++ b/gtsam/linear/GaussianJunctionTree.cpp @@ -30,15 +30,4 @@ namespace gtsam { const GaussianEliminationTree& eliminationTree) : Base(Base::FromEliminationTree(eliminationTree)) {} - /* ************************************************************************* */ - GaussianJunctionTree::GaussianJunctionTree(const This& other) : - Base(other) {} - - /* ************************************************************************* */ - GaussianJunctionTree& GaussianJunctionTree::operator=(const This& other) - { - (void) Base::operator=(other); - return *this; - } - } diff --git a/gtsam/linear/GaussianJunctionTree.h b/gtsam/linear/GaussianJunctionTree.h index 8ccb07f42..f944d417b 100644 --- a/gtsam/linear/GaussianJunctionTree.h +++ b/gtsam/linear/GaussianJunctionTree.h @@ -61,14 +61,6 @@ namespace gtsam { * @return The elimination tree */ GaussianJunctionTree(const GaussianEliminationTree& eliminationTree); - - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - GaussianJunctionTree(const This& other); - - /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - This& operator=(const This& other); }; } diff --git a/gtsam/symbolic/SymbolicBayesTree.cpp b/gtsam/symbolic/SymbolicBayesTree.cpp index f6665c39b..8797ba363 100644 --- a/gtsam/symbolic/SymbolicBayesTree.cpp +++ b/gtsam/symbolic/SymbolicBayesTree.cpp @@ -31,17 +31,6 @@ namespace gtsam { template class BayesTreeCliqueBase; template class BayesTree; - /* ************************************************************************* */ - SymbolicBayesTree::SymbolicBayesTree(const SymbolicBayesTree& other) : - Base(other) {} - - /* ************************************************************************* */ - SymbolicBayesTree& SymbolicBayesTree::operator=(const SymbolicBayesTree& other) - { - (void) Base::operator=(other); - return *this; - } - /* ************************************************************************* */\ bool SymbolicBayesTree::equals(const This& other, double tol /* = 1e-9 */) const { diff --git a/gtsam/symbolic/SymbolicBayesTree.h b/gtsam/symbolic/SymbolicBayesTree.h index e7ae6fbc9..858bc629f 100644 --- a/gtsam/symbolic/SymbolicBayesTree.h +++ b/gtsam/symbolic/SymbolicBayesTree.h @@ -58,14 +58,6 @@ namespace gtsam { /** Default constructor, creates an empty Bayes tree */ SymbolicBayesTree() {} - /** Makes a deep copy of the tree structure, but only pointers to conditionals are - * copied, the conditionals and their matrices are not cloned. */ - SymbolicBayesTree(const SymbolicBayesTree& other); - - /** Makes a deep copy of the tree structure, but only pointers to conditionals are - * copied, the conditionals and their matrices are not cloned. */ - SymbolicBayesTree& operator=(const SymbolicBayesTree& other); - /** check equality */ bool equals(const This& other, double tol = 1e-9) const; }; diff --git a/gtsam/symbolic/SymbolicEliminationTree.cpp b/gtsam/symbolic/SymbolicEliminationTree.cpp index 588983a79..bd11274bd 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.cpp +++ b/gtsam/symbolic/SymbolicEliminationTree.cpp @@ -35,18 +35,6 @@ namespace gtsam { const SymbolicFactorGraph& factorGraph, const Ordering& order) : Base(factorGraph, order) {} - /* ************************************************************************* */ - SymbolicEliminationTree::SymbolicEliminationTree( - const This& other) : - Base(other) {} - - /* ************************************************************************* */ - SymbolicEliminationTree& SymbolicEliminationTree::operator=(const This& other) - { - (void) Base::operator=(other); - return *this; - } - /* ************************************************************************* */ bool SymbolicEliminationTree::equals(const This& other, double tol) const { diff --git a/gtsam/symbolic/SymbolicEliminationTree.h b/gtsam/symbolic/SymbolicEliminationTree.h index a00bfde3d..5a44d451d 100644 --- a/gtsam/symbolic/SymbolicEliminationTree.h +++ b/gtsam/symbolic/SymbolicEliminationTree.h @@ -47,14 +47,6 @@ namespace gtsam { SymbolicEliminationTree(const SymbolicFactorGraph& factorGraph, const Ordering& order); - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - SymbolicEliminationTree(const This& other); - - /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - This& operator=(const This& other); - /** Test whether the tree is equal to another */ bool equals(const This& other, double tol = 1e-9) const; diff --git a/gtsam/symbolic/SymbolicJunctionTree.cpp b/gtsam/symbolic/SymbolicJunctionTree.cpp index 4b3aadf53..a8e9803fc 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.cpp +++ b/gtsam/symbolic/SymbolicJunctionTree.cpp @@ -30,15 +30,4 @@ namespace gtsam { const SymbolicEliminationTree& eliminationTree) : Base(Base::FromEliminationTree(eliminationTree)) {} - /* ************************************************************************* */ - SymbolicJunctionTree::SymbolicJunctionTree(const This& other) : - Base(other) {} - - /* ************************************************************************* */ - SymbolicJunctionTree& SymbolicJunctionTree::operator=(const This& other) - { - (void) Base::operator=(other); - return *this; - } - } diff --git a/gtsam/symbolic/SymbolicJunctionTree.h b/gtsam/symbolic/SymbolicJunctionTree.h index 3396c8698..709488cbf 100644 --- a/gtsam/symbolic/SymbolicJunctionTree.h +++ b/gtsam/symbolic/SymbolicJunctionTree.h @@ -61,14 +61,6 @@ namespace gtsam { * @return The elimination tree */ SymbolicJunctionTree(const SymbolicEliminationTree& eliminationTree); - - /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - SymbolicJunctionTree(const This& other); - - /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are - * copied, factors are not cloned. */ - This& operator=(const This& other); }; } From a98180f84f781d8ced914063f68603ea92b5054a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 8 Aug 2013 21:41:29 +0000 Subject: [PATCH 05/18] Working on converting ISAM2 --- gtsam/nonlinear/ISAM2-impl.cpp | 15 -- gtsam/nonlinear/ISAM2-impl.h | 15 +- gtsam/nonlinear/ISAM2.cpp | 344 ++++++++++++++------------------- gtsam/nonlinear/ISAM2.h | 6 + 4 files changed, 151 insertions(+), 229 deletions(-) diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 8982ca9bd..84c264563 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -16,8 +16,6 @@ * @author Richard Roberts */ -#if 0 - #include #include // for selective linearization thresholds #include @@ -130,17 +128,6 @@ void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Cli linearFactors.permuteWithInverse(unusedToEndInverse); } -/* ************************************************************************* */ -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors) { - FastSet indices; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, factors) { - BOOST_FOREACH(Key key, factor->keys()) { - indices.insert(ordering[key]); - } - } - return indices; -} - /* ************************************************************************* */ FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { @@ -495,5 +482,3 @@ size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThresho } } - -#endif diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index 435a75a48..fa136f1be 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -17,8 +17,6 @@ #pragma once -#if 0 - #include #include @@ -61,15 +59,6 @@ struct GTSAM_EXPORT ISAM2::Impl { VectorValues& RgProd, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, GaussianFactorGraph& linearFactors, FastSet& fixedVariables); - /** - * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol - * in each NonlinearFactor, obtains the index by calling ordering[symbol]. - * @param ordering The current ordering from which to obtain the variable indices - * @param factors The factors from which to extract the variables - * @return The set of variables indices from the factors - */ - static FastSet IndicesFromFactors(const Ordering& ordering, const NonlinearFactorGraph& factors); - /** * Find the set of variables to be relinearized according to relinearizeThreshold. * Any variables in the VectorValues delta whose vector magnitude is greater than @@ -111,7 +100,7 @@ struct GTSAM_EXPORT ISAM2::Impl { * * Alternatively could we trace up towards the root for each variable here? */ - static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const std::vector& markedMask); + static void FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const FastSet& markedMask); /** * Apply expmap to the given values, but only for indices appearing in @@ -156,5 +145,3 @@ struct GTSAM_EXPORT ISAM2::Impl { }; } - -#endif diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index fefb6d328..e5aba4731 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -25,10 +25,11 @@ namespace br { using namespace boost::range; using namespace boost::adaptors; } #include #include -#include -#include +#include +#include // We need the inst file because we'll make a special JT templated on ISAM2 #include #include +#include #include #include @@ -42,6 +43,33 @@ using namespace std; static const bool disableReordering = false; static const double batchThreshold = 0.65; +/* ************************************************************************* */ +// Special BayesTree class that uses ISAM2 cliques - this is the result of reeliminating ISAM2 +// subtrees. +class ISAM2BayesTree : public ISAM2::Base +{ +public: + typedef ISAM2::Base Base; + typedef ISAM2BayesTree This; + typedef boost::shared_ptr shared_ptr; + + ISAM2BayesTree() {} +}; + +/* ************************************************************************* */ +// Special JunctionTree class that produces ISAM2 BayesTree cliques, used for reeliminating ISAM2 +// subtrees. +class ISAM2JunctionTree : public JunctionTree +{ +public: + typedef JunctionTree Base; + typedef ISAM2JunctionTree This; + typedef boost::shared_ptr shared_ptr; + + ISAM2JunctionTree(const GaussianEliminationTree& eliminationTree) : + Base(Base::FromEliminationTree(eliminationTree)) {} +}; + /* ************************************************************************* */ std::string ISAM2DoglegParams::adaptationModeTranslator(const DoglegOptimizerImpl::TrustRegionAdaptationMode& adaptationMode) const { std::string s; @@ -337,72 +365,50 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe if(affectedKeys.size() >= theta_.size() * batchThreshold) { + // Do a batch step - reorder and relinearize all variables gttic(batch); gttic(add_keys); br::copy(variableIndex_ | br::map_keys, std::inserter(*affectedKeysSet, affectedKeysSet->end())); gttoc(add_keys); - gttic(reorder); - gttic(CCOLAMD); - // Do a batch step - reorder and relinearize all variables - vector cmember(theta_.size(), 0); - if(constrainKeys) { - if(!constrainKeys->empty()) { - typedef std::pair Index_Group; - if(theta_.size() > constrainKeys->size()) { // Only if some variables are unconstrained - BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) { - cmember[index_group.first] = index_group.second; } - } else { - int minGroup = *boost::range::min_element(boost::adaptors::values(*constrainKeys)); - BOOST_FOREACH(const Index_Group& index_group, *constrainKeys) { - cmember[index_group.first] = index_group.second - minGroup; } - } + gttic(ordering); + Ordering order; + if(constrainKeys) + { + order = Ordering::COLAMDConstrained(variableIndex_, *constrainKeys); + } + else + { + if(theta_.size() > observedKeys.size()) + { + // Only if some variables are unconstrained + FastMap constraintGroups; + BOOST_FOREACH(Key var, observedKeys) + constraintGroups[var] = 1; + order = Ordering::COLAMDConstrained(variableIndex_, constraintGroups); } - } else { - if(theta_.size() > observedKeys.size()) { // Only if some variables are unconstrained - BOOST_FOREACH(Index var, observedKeys) { cmember[var] = 1; } + else + { + order = Ordering::COLAMD(variableIndex_); } } - Ordering order = Ordering::COLAMDConstrained() - Permutation::shared_ptr colamd(inference::PermutationCOLAMD_(variableIndex_, cmember)); - Permutation::shared_ptr colamdInverse(colamd->inverse()); - gttoc(CCOLAMD); - - // Reorder - gttic(permute_global_variable_index); - variableIndex_.permuteInPlace(*colamd); - gttoc(permute_global_variable_index); - gttic(permute_delta); - delta_.permuteInPlace(*colamd); - deltaNewton_.permuteInPlace(*colamd); - RgProd_.permuteInPlace(*colamd); - gttoc(permute_delta); - gttic(permute_ordering); - ordering_.permuteInPlace(*colamd); - gttoc(permute_ordering); - gttoc(reorder); gttic(linearize); - GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_, ordering_); + GaussianFactorGraph linearized = *nonlinearFactors_.linearize(theta_); if(params_.cacheLinearizedFactors) linearFactors_ = linearized; gttoc(linearize); gttic(eliminate); - JunctionTree jt(linearized, variableIndex_); - sharedClique newRoot; - if(params_.factorization == ISAM2Params::CHOLESKY) - newRoot = jt.eliminate(EliminatePreferCholesky); - else if(params_.factorization == ISAM2Params::QR) - newRoot = jt.eliminate(EliminateQR); - else assert(false); - if(debug) newRoot->print("Eliminated: "); + ISAM2BayesTree bayesTree = *ISAM2JunctionTree(GaussianEliminationTree(linearized, variableIndex_, order)) + .eliminate(params_.getEliminationFunction()).first; gttoc(eliminate); gttic(insert); this->clear(); - this->insert(newRoot); + BOOST_FOREACH(const sharedNode& root, bayesTree.roots()) + this->insertRoot(root); gttoc(insert); result.variablesReeliminated = affectedKeysSet->size(); @@ -426,7 +432,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe gttic(incremental); // 2. Add the new factors \Factors' into the resulting factor graph - FastList affectedAndNewKeys; + FastList affectedAndNewKeys; affectedAndNewKeys.insert(affectedAndNewKeys.end(), affectedKeys.begin(), affectedKeys.end()); affectedAndNewKeys.insert(affectedAndNewKeys.end(), observedKeys.begin(), observedKeys.end()); gttic(relinearizeAffected); @@ -438,8 +444,8 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe // Reeliminated keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Index index, affectedAndNewKeys) { - result.detail->variableStatus[ordering_.key(index)].isReeliminated = true; + BOOST_FOREACH(Index key, affectedAndNewKeys) { + result.detail->variableStatus[key].isReeliminated = true; } } @@ -478,97 +484,48 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe gttic(PartialSolve); Impl::ReorderingMode reorderingMode; - reorderingMode.nFullSystemVars = ordering_.size(); + reorderingMode.nFullSystemVars = variableIndex_.size(); reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; if(constrainKeys) { reorderingMode.constrainedKeys = *constrainKeys; } else { - reorderingMode.constrainedKeys = FastMap(); - BOOST_FOREACH(Index var, observedKeys) { reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } + reorderingMode.constrainedKeys = FastMap(); + BOOST_FOREACH(Key var, observedKeys) + reorderingMode.constrainedKeys->insert(make_pair(var, 1)); } - FastSet affectedUsedKeys = *affectedKeysSet; // Remove unused keys from the set we pass to PartialSolve - BOOST_FOREACH(Index unused, unusedIndices) { + FastSet affectedUsedKeys = *affectedKeysSet; // Remove unused keys from the set we pass to PartialSolve + BOOST_FOREACH(Key unused, unusedIndices) affectedUsedKeys.erase(unused); - } // Remove unaffected keys from the constraints - FastMap::iterator iter = reorderingMode.constrainedKeys->begin(); - while(iter != reorderingMode.constrainedKeys->end()) { - if(affectedUsedKeys.find(iter->first) == affectedUsedKeys.end()) { + FastMap::iterator iter = reorderingMode.constrainedKeys->begin(); + while(iter != reorderingMode.constrainedKeys->end()) + if(affectedUsedKeys.find(iter->first) == affectedUsedKeys.end()) reorderingMode.constrainedKeys->erase(iter++); - } else { + else ++iter; - } - } Impl::PartialSolveResult partialSolveResult = Impl::PartialSolve(factors, affectedUsedKeys, reorderingMode, (params_.factorization == ISAM2Params::QR)); gttoc(PartialSolve); - // We now need to permute everything according this partial reordering: the - // delta vector, the global ordering, and the factors we're about to - // re-eliminate. The reordered variables are also mentioned in the - // orphans and the leftover cached factors. - gttic(permute_global_variable_index); - variableIndex_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - gttoc(permute_global_variable_index); - gttic(permute_delta); - delta_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - deltaNewton_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - RgProd_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - gttoc(permute_delta); - gttic(permute_ordering); - ordering_.permuteInPlace(partialSolveResult.reorderingSelector, partialSolveResult.reorderingPermutation); - gttoc(permute_ordering); - if(params_.cacheLinearizedFactors) { - gttic(permute_cached_linear); - //linearFactors_.permuteWithInverse(partialSolveResult.fullReorderingInverse); - FastList permuteLinearIndices = getAffectedFactors(affectedAndNewKeys); - BOOST_FOREACH(size_t idx, permuteLinearIndices) { - linearFactors_[idx]->reduceWithInverse(partialSolveResult.reorderingInverse); - } - gttoc(permute_cached_linear); - } - gttoc(reorder_and_eliminate); gttic(reassemble); - if(partialSolveResult.bayesTree) { - assert(!this->root_); - this->insert(partialSolveResult.bayesTree); - } + if(partialSolveResult.bayesTree) + BOOST_FOREACH(const sharedNode& root, *partialSolveResult.bayesTree.roots()) + this->insertRoot(root); gttoc(reassemble); - // 4. Insert the orphans back into the new Bayes tree. - gttic(orphans); - gttic(permute); - BOOST_FOREACH(sharedClique orphan, orphans) { - (void)orphan->reduceSeparatorWithInverse(partialSolveResult.reorderingInverse); - } - gttoc(permute); - gttic(insert); - // add orphans to the bottom of the new tree - BOOST_FOREACH(sharedClique orphan, orphans) { - // Because the affectedKeysSelector is sorted, the orphan separator keys - // will be sorted correctly according to the new elimination order after - // applying the permutation, so findParentClique, which looks for the - // lowest-ordered parent, will still work. - Index parentRepresentative = Base::findParentClique((*orphan)->parents()); - sharedClique parent = (*this)[parentRepresentative]; - parent->children_ += orphan; - orphan->parent_ = parent; // set new parent! - } - gttoc(insert); - gttoc(orphans); + // 4. The orphans have already been inserted during elimination gttoc(incremental); } // Root clique variables for detailed results - if(params_.enableDetailedResults) { - BOOST_FOREACH(Index index, this->root()->conditional()->frontals()) { - result.detail->variableStatus[ordering_.key(index)].inRootClique = true; - } - } + if(params_.enableDetailedResults) + BOOST_FOREACH(const sharedNode& root, this->roots()) + BOOST_FOREACH(Key var, *root) + result.detail->variableStatus[var].inRootClique = true; return affectedKeysSet; } @@ -628,7 +585,7 @@ ISAM2Result ISAM2::update( } // Remove removed factors from the variable index so we do not attempt to relinearize them - variableIndex_.remove(removeFactorIndices, *removeFactors.symbolic(ordering_)); + variableIndex_.remove(removeFactorIndices, removeFactors); // Compute unused keys and indices FastSet unusedKeys; @@ -638,7 +595,7 @@ ISAM2Result ISAM2::update( // i.e., keys that are empty now and do not appear in the new factors. FastSet removedAndEmpty; BOOST_FOREACH(Key key, removeFactors.keys()) { - if(variableIndex_[ordering_[key]].empty()) + if(variableIndex_[key].empty()) removedAndEmpty.insert(removedAndEmpty.end(), key); } FastSet newFactorSymbKeys = newFactors.keys(); @@ -647,14 +604,14 @@ ISAM2Result ISAM2::update( // Get indices for unused keys BOOST_FOREACH(Key key, unusedKeys) { - unusedIndices.insert(unusedIndices.end(), ordering_[key]); + unusedIndices.insert(unusedIndices.end(), key); } } gttoc(push_back_factors); gttic(add_new_variables); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_, ordering_); + Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_); // New keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } @@ -667,23 +624,23 @@ ISAM2Result ISAM2::update( gttic(gather_involved_keys); // 3. Mark linear update - FastSet markedKeys = Impl::IndicesFromFactors(ordering_, newFactors); // Get keys from new factors + FastSet markedKeys = newFactors.keys(); // Get keys from new factors // Also mark keys involved in removed factors { - FastSet markedRemoveKeys = Impl::IndicesFromFactors(ordering_, removeFactors); // Get keys involved in removed factors + FastSet markedRemoveKeys = removeFactors.keys(); // Get keys involved in removed factors markedKeys.insert(markedRemoveKeys.begin(), markedRemoveKeys.end()); // Add to the overall set of marked keys } // Also mark any provided extra re-eliminate keys if(extraReelimKeys) { BOOST_FOREACH(Key key, *extraReelimKeys) { - markedKeys.insert(ordering_.at(key)); + markedKeys.insert(key); } } // Observed keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Index index, markedKeys) { - result.detail->variableStatus[ordering_.key(index)].isObserved = true; + BOOST_FOREACH(Key key, markedKeys) { + result.detail->variableStatus[key].isObserved = true; } } // NOTE: we use assign instead of the iterator constructor here because this @@ -702,47 +659,50 @@ ISAM2Result ISAM2::update( gttic(gather_relinearize_keys); // 4. Mark keys in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}. if(params_.enablePartialRelinearizationCheck) - relinKeys = Impl::CheckRelinearizationPartial(root_, delta_, ordering_, params_.relinearizeThreshold); + relinKeys = Impl::CheckRelinearizationPartial(roots_, delta_, params_.relinearizeThreshold); else - relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, params_.relinearizeThreshold); - if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, ordering_, 0.0); // This is used for debugging + relinKeys = Impl::CheckRelinearizationFull(delta_, params_.relinearizeThreshold); + if(disableReordering) relinKeys = Impl::CheckRelinearizationFull(delta_, 0.0); // This is used for debugging // Remove from relinKeys any keys whose linearization points are fixed BOOST_FOREACH(Key key, fixedVariables_) { - relinKeys.erase(ordering_[key]); + relinKeys.erase(key); } if(noRelinKeys) { BOOST_FOREACH(Key key, *noRelinKeys) { - relinKeys.erase(ordering_[key]); + relinKeys.erase(key); } } // Above relin threshold keys for detailed results if(params_.enableDetailedResults) { - BOOST_FOREACH(Index index, relinKeys) { - result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold = true; - result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } } + BOOST_FOREACH(Key key, relinKeys) { + result.detail->variableStatus[key].isAboveRelinThreshold = true; + result.detail->variableStatus[key].isRelinearized = true; } } // Add the variables being relinearized to the marked keys - vector markedRelinMask(ordering_.size(), false); - BOOST_FOREACH(const Index j, relinKeys) { markedRelinMask[j] = true; } + FastSet markedRelinMask; + BOOST_FOREACH(const Key key, relinKeys) + markedRelinMask.insert(key); markedKeys.insert(relinKeys.begin(), relinKeys.end()); gttoc(gather_relinearize_keys); gttic(fluid_find_all); // 5. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors. - if (!relinKeys.empty() && this->root()) { - // add other cliques that have the marked ones in the separator - Impl::FindAll(this->root(), markedKeys, markedRelinMask); + if (!relinKeys.empty()) { + BOOST_FOREACH(const sharedClique& root, roots_) + // add other cliques that have the marked ones in the separator + Impl::FindAll(root, markedKeys, markedRelinMask); // Relin involved keys for detailed results if(params_.enableDetailedResults) { FastSet involvedRelinKeys; - Impl::FindAll(this->root(), involvedRelinKeys, markedRelinMask); - BOOST_FOREACH(Index index, involvedRelinKeys) { - if(!result.detail->variableStatus[ordering_.key(index)].isAboveRelinThreshold) { - result.detail->variableStatus[ordering_.key(index)].isRelinearizeInvolved = true; - result.detail->variableStatus[ordering_.key(index)].isRelinearized = true; } } + BOOST_FOREACH(const sharedClique& root, roots_) + Impl::FindAll(root, involvedRelinKeys, markedRelinMask); + BOOST_FOREACH(Key key, involvedRelinKeys) { + if(!result.detail->variableStatus[key].isAboveRelinThreshold) { + result.detail->variableStatus[key].isRelinearizeInvolved = true; + result.detail->variableStatus[key].isRelinearized = true; } } } } gttoc(fluid_find_all); @@ -750,7 +710,7 @@ ISAM2Result ISAM2::update( gttic(expmap); // 6. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}. if (!relinKeys.empty()) - Impl::ExpmapMasked(theta_, delta_, ordering_, markedRelinMask, delta_); + Impl::ExpmapMasked(theta_, delta_, markedRelinMask, delta_); gttoc(expmap); result.variablesRelinearized = markedKeys.size(); @@ -762,38 +722,29 @@ ISAM2Result ISAM2::update( // 7. Linearize new factors if(params_.cacheLinearizedFactors) { gttic(linearize); - FactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_, ordering_); + GaussianFactorGraph::shared_ptr linearFactors = newFactors.linearize(theta_); linearFactors_.push_back(*linearFactors); assert(nonlinearFactors_.size() == linearFactors_.size()); gttoc(linearize); gttic(augment_VI); // Augment the variable index with the new factors - variableIndex_.augment(*linearFactors); + variableIndex_.augment(*linearFactors); // TODO: move this to a better place now gttoc(augment_VI); } else { - variableIndex_.augment(*newFactors.symbolic(ordering_)); + variableIndex_.augment(newFactors); } gttoc(linearize_new); gttic(recalculate); // 8. Redo top of Bayes tree - // Convert constrained symbols to indices - boost::optional > constrainedIndices; - if(constrainedKeys) { - constrainedIndices = FastMap(); - typedef pair Key_Group; - BOOST_FOREACH(Key_Group key_group, *constrainedKeys) { - constrainedIndices->insert(make_pair(ordering_[key_group.first], key_group.second)); - } - } - boost::shared_ptr > replacedKeys; + boost::shared_ptr > replacedKeys; if(!markedKeys.empty() || !observedKeys.empty()) - replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedIndices, result); + replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); // Update replaced keys mask (accumulates until back-substitution takes place) if(replacedKeys) { - BOOST_FOREACH(const Index var, *replacedKeys) { + BOOST_FOREACH(const Key var, *replacedKeys) { deltaReplacedMask_[var] = true; } } gttoc(recalculate); @@ -803,7 +754,7 @@ ISAM2Result ISAM2::update( if(!unusedKeys.empty()) { gttic(remove_variables); Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, ordering_, Base::nodes_, linearFactors_, fixedVariables_); + deltaReplacedMask_, Base::nodes_, linearFactors_, fixedVariables_); gttoc(remove_variables); } result.cliques = this->nodes().size(); @@ -819,27 +770,24 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -void ISAM2::marginalizeLeaves(const FastList& leafKeys) +void ISAM2::marginalizeLeaves(const FastList& leafKeysList) { - // Convert set of keys into a set of indices - FastSet indices; - BOOST_FOREACH(Key key, leafKeys) { - indices.insert(ordering_[key]); - } + // Convert to ordered set + FastSet leafKeys(leafKeysList.begin(), leafKeysList.end()); // Keep track of marginal factors - map from clique to the marginal factors // that should be incorporated into it, passed up from it's children. multimap marginalFactors; // Remove each variable and its subtrees - BOOST_REVERSE_FOREACH(Index j, indices) { - if(nodes_[j]) { // If the index was not already removed by removing another subtree + BOOST_REVERSE_FOREACH(Key j, leafKeys) { + if(nodes_.exists(j)) { // If the index was not already removed by removing another subtree sharedClique clique = nodes_[j]; // See if we should remove the whole clique bool marginalizeEntireClique = true; - BOOST_FOREACH(Index frontal, clique->conditional()->frontals()) { - if(indices.find(frontal) == indices.end()) { + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + if(!leafKeys.exists(frontal)) { marginalizeEntireClique = false; break; } } @@ -857,8 +805,8 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) const Cliques removedCliques = this->removeSubtree(clique); // Remove the subtree and throw away the cliques BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { marginalFactors.erase(removedClique); - BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) { - if(indices.find(indexInClique) == indices.end()) + BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) { + if(!leafKeys.exists(frontal)) throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); } } } @@ -873,10 +821,10 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) GaussianFactorGraph graph; FastSet factorsInSubtreeRoot; Cliques subtreesToRemove; - BOOST_FOREACH(const sharedClique& child, clique->children()) { + BOOST_FOREACH(const sharedClique& child, clique->children) { // Remove subtree if child depends on any marginalized keys - BOOST_FOREACH(Index parentIndex, child->conditional()->parents()) { - if(indices.find(parentIndex) != indices.end()) { + BOOST_FOREACH(Key parent, child->conditional()->parents()) { + if(leafKeys.exists(parent)) { subtreesToRemove.push_back(child); graph.push_back(child->cachedFactor()); // Add child marginal break; @@ -889,28 +837,28 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) childrenRemoved.insert(childrenRemoved.end(), removedCliques.begin(), removedCliques.end()); BOOST_FOREACH(const sharedClique& removedClique, removedCliques) { marginalFactors.erase(removedClique); - BOOST_FOREACH(Index indexInClique, removedClique->conditional()->frontals()) { - if(indices.find(indexInClique) == indices.end()) + BOOST_FOREACH(Key frontal, removedClique->conditional()->frontals()) { + if(!leafKeys.exists(frontal)) throw runtime_error("Requesting to marginalize variables that are not leaves, the ISAM2 object is now in an inconsistent state so should no longer be used."); } } } // Gather remaining children after we removed marginalized subtrees - vector orphans(clique->children().begin(), clique->children().end()); + vector orphans(clique->children.begin(), clique->children.end()); // Add the factors that are pulled into the current clique by the marginalized variables. // These are the factors that involve *marginalized* frontal variables in this clique // but do not involve frontal variables of any of its children. FastSet factorsFromMarginalizedInClique; - BOOST_FOREACH(Index indexInClique, clique->conditional()->frontals()) { - if(indices.find(indexInClique) != indices.end()) - factorsFromMarginalizedInClique.insert(variableIndex_[indexInClique].begin(), variableIndex_[indexInClique].end()); } + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + if(leafKeys.exists(frontal)) + factorsFromMarginalizedInClique.insert(variableIndex_[frontal].begin(), variableIndex_[frontal].end()); } BOOST_FOREACH(const sharedClique& removedChild, childrenRemoved) { BOOST_FOREACH(Index indexInClique, removedChild->conditional()->frontals()) { BOOST_FOREACH(size_t factorInvolving, variableIndex_[indexInClique]) { factorsFromMarginalizedInClique.erase(factorInvolving); } } } BOOST_FOREACH(size_t i, factorsFromMarginalizedInClique) { - graph.push_back(nonlinearFactors_[i]->linearize(theta_, ordering_)); } + graph.push_back(nonlinearFactors_[i]->linearize(theta_)); } // Remove the current clique sharedClique parent = clique->parent(); @@ -919,33 +867,29 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeys) // Reeliminate the linear graph to get the marginal and discard the conditional const FastSet cliqueFrontals(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); FastSet cliqueFrontalsToEliminate; - std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), indices.begin(), indices.end(), + std::set_intersection(cliqueFrontals.begin(), cliqueFrontals.end(), leafKeys.begin(), leafKeys.end(), std::inserter(cliqueFrontalsToEliminate, cliqueFrontalsToEliminate.end())); vector cliqueFrontalsToEliminateV(cliqueFrontalsToEliminate.begin(), cliqueFrontalsToEliminate.end()); - pair eliminationResult1 = - graph.eliminate(cliqueFrontalsToEliminateV, - params_.factorization==ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky); + pair eliminationResult1 = + params_.getEliminationFunction()(graph, Ordering(cliqueFrontalsToEliminateV)); // Add the resulting marginal - BOOST_FOREACH(const GaussianFactor::shared_ptr& marginal, eliminationResult1.second) { - if(marginal) - marginalFactors.insert(make_pair(clique, marginal)); } + if(eliminationResult1.second) + marginalFactors.insert(make_pair(clique, eliminationResult1.second)); // Recover the conditional on the remaining subset of frontal variables - // of this clique being martially marginalized. - size_t nToEliminate = std::find(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j) - clique->conditional()->begin() + 1; + // of this clique being partially marginalized. + GaussianConditional::iterator jPosition = std::find( + clique->conditional()->beginFrontals(), clique->conditional()->endFrontals(), j); GaussianFactorGraph graph2; - graph2.push_back(clique->conditional()->toFactor()); + graph2.push_back(clique->conditional()); GaussianFactorGraph::EliminationResult eliminationResult2 = - params_.factorization == ISAM2Params::QR ? - EliminateQR(graph2, nToEliminate) : - EliminatePreferCholesky(graph2, nToEliminate); + params_.getEliminationFunction()(graph2, Ordering( + clique->conditional()->beginFrontals(), jPosition)); GaussianFactorGraph graph3; graph3.push_back(eliminationResult2.second); GaussianFactorGraph::EliminationResult eliminationResult3 = - params_.factorization == ISAM2Params::QR ? - EliminateQR(graph3, clique->conditional()->nrFrontals() - nToEliminate) : - EliminatePreferCholesky(graph3, clique->conditional()->nrFrontals() - nToEliminate); + params_.getEliminationFunction()(graph3, Ordering(jPosition, clique->conditional()->endFrontals())); sharedClique newClique = boost::make_shared(make_pair(eliminationResult3.first, clique->cachedFactor())); // Add the marginalized clique to the BayesTree diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 2ab434c61..05bdf3d7a 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -232,6 +232,12 @@ struct GTSAM_EXPORT ISAM2Params { Factorization factorizationTranslator(const std::string& str) const; std::string factorizationTranslator(const Factorization& value) const; + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + return factorization == CHOLESKY + ? (GaussianFactorGraph::Eliminate)EliminatePreferCholesky + : (GaussianFactorGraph::Eliminate)EliminateQR; + } }; From 356351db7540675ab72c35363be54498099e8a78 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 9 Aug 2013 21:35:39 +0000 Subject: [PATCH 06/18] Removed unused addClique function --- gtsam/inference/BayesTree-inst.h | 14 -------------- gtsam/inference/BayesTree.h | 3 --- 2 files changed, 17 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 6a303e081..8ff8c90d4 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -131,20 +131,6 @@ namespace gtsam { } } - /* ************************************************************************* */ - template - typename BayesTree::sharedClique BayesTree::addClique( - const sharedConditional& conditional, std::list& child_cliques) - { - sharedClique new_clique(new Clique(conditional)); - BOOST_FOREACH(Index j, conditional->frontals()) - nodes_[j] = new_clique; - new_clique->children.assign(child_cliques.begin(), child_cliques.end()); - BOOST_FOREACH(sharedClique& child, child_cliques) - child->parent_ = new_clique; - return new_clique; - } - /* ************************************************************************* */ // TODO: Clean up namespace { diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index e4c8edcf1..66dee364d 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -239,9 +239,6 @@ namespace gtsam { /** remove a clique: warning, can result in a forest */ void removeClique(sharedClique clique); - /** add a clique (bottom up) */ - sharedClique addClique(const sharedConditional& conditional, std::list& child_cliques); - /** Fill the nodes index for a subtree */ void fillNodesIndex(const sharedClique& subtree); From 1a4fe360eedf78dfaa8c140c14ea6a9f64bfc928 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 9 Aug 2013 21:35:40 +0000 Subject: [PATCH 07/18] Pulled GaussianBayesTree optimize algorithm into a template function --- gtsam/linear/GaussianBayesTree.cpp | 67 +---------------- gtsam/linear/linearAlgorithms-inst.h | 103 +++++++++++++++++++++++++++ 2 files changed, 105 insertions(+), 65 deletions(-) create mode 100644 gtsam/linear/linearAlgorithms-inst.h diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp index bb25c9a45..0ceecdfd2 100644 --- a/gtsam/linear/GaussianBayesTree.cpp +++ b/gtsam/linear/GaussianBayesTree.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -33,63 +34,6 @@ namespace gtsam { /* ************************************************************************* */ namespace internal { - /* ************************************************************************* */ - struct OptimizeData { - boost::optional parentData; - //VectorValues ancestorResults; - //VectorValues results; - }; - - /* ************************************************************************* */ - /** Pre-order visitor for back-substitution in a Bayes tree. The visitor function operator()() - * optimizes the clique given the solution for the parents, and returns the solution for the - * clique's frontal variables. In addition, it adds the solution to a global collected - * solution that will finally be returned to the user. The reason we pass the individual - * clique solutions between nodes is to avoid log(n) lookups over all variables, they instead - * then are only over a node's parent variables. */ - struct OptimizeClique - { - VectorValues collectedResult; - - OptimizeData operator()( - const GaussianBayesTreeClique::shared_ptr& clique, - OptimizeData& parentData) - { - OptimizeData myData; - myData.parentData = parentData; - // Take any ancestor results we'll need - //BOOST_FOREACH(Key parent, clique->conditional_->parents()) - // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]); - // Solve and store in our results - VectorValues result = clique->conditional()->solve(collectedResult/*myData.ancestorResults*/); - collectedResult.insert(result); - //myData.ancestorResults.insert(result); - return myData; - } - }; - - /* ************************************************************************* */ - //OptimizeData OptimizePreVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& parentData) - //{ - // // Create data - holds a pointer to our parent, a copy of parent solution, and our results - // OptimizeData myData; - // myData.parentData = parentData; - // // Take any ancestor results we'll need - // BOOST_FOREACH(Key parent, clique->conditional_->parents()) - // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]); - // // Solve and store in our results - // myData.results.insert(clique->conditional()->solve(myData.ancestorResults)); - // myData.ancestorResults.insert(myData.results); - // return myData; - //} - - /* ************************************************************************* */ - //void OptimizePostVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& myData) - //{ - // // Conglomerate our results to the parent - // myData.parentData->results.insert(myData.results); - //} - /* ************************************************************************* */ double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, double& parentSum) { @@ -108,14 +52,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValues GaussianBayesTree::optimize() const { - gttic(GaussianBayesTree_optimize); - //internal::OptimizeData rootData; // Will hold final solution - //treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor); - //return rootData.results; - internal::OptimizeData rootData; - internal::OptimizeClique preVisitor; - treeTraversal::DepthFirstForest(*this, rootData, preVisitor); - return preVisitor.collectedResult; + return internal::linearAlgorithms::optimizeBayesTree(*this); } /* ************************************************************************* */ diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h new file mode 100644 index 000000000..93276be59 --- /dev/null +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -0,0 +1,103 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file linearAlgorithms-inst.h + * @brief Templated algorithms that are used in multiple places in linear + * @author Richard Roberts + */ + +#include +#include + +#include +#include + +namespace gtsam +{ + namespace internal + { + namespace linearAlgorithms + { + /* ************************************************************************* */ + struct OptimizeData { + boost::optional parentData; + //VectorValues ancestorResults; + //VectorValues results; + }; + + /* ************************************************************************* */ + /** Pre-order visitor for back-substitution in a Bayes tree. The visitor function operator()() + * optimizes the clique given the solution for the parents, and returns the solution for the + * clique's frontal variables. In addition, it adds the solution to a global collected + * solution that will finally be returned to the user. The reason we pass the individual + * clique solutions between nodes is to avoid log(n) lookups over all variables, they instead + * then are only over a node's parent variables. */ + template + struct OptimizeClique + { + VectorValues collectedResult; + + OptimizeData operator()( + const boost::shared_ptr& clique, + OptimizeData& parentData) + { + OptimizeData myData; + myData.parentData = parentData; + // Take any ancestor results we'll need + //BOOST_FOREACH(Key parent, clique->conditional_->parents()) + // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]); + // Solve and store in our results + VectorValues result = clique->conditional()->solve(collectedResult/*myData.ancestorResults*/); + collectedResult.insert(result); + //myData.ancestorResults.insert(result); + return myData; + } + }; + + /* ************************************************************************* */ + //OptimizeData OptimizePreVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& parentData) + //{ + // // Create data - holds a pointer to our parent, a copy of parent solution, and our results + // OptimizeData myData; + // myData.parentData = parentData; + // // Take any ancestor results we'll need + // BOOST_FOREACH(Key parent, clique->conditional_->parents()) + // myData.ancestorResults.insert(parent, myData.parentData->ancestorResults[parent]); + // // Solve and store in our results + // myData.results.insert(clique->conditional()->solve(myData.ancestorResults)); + // myData.ancestorResults.insert(myData.results); + // return myData; + //} + + /* ************************************************************************* */ + //void OptimizePostVisitor(const GaussianBayesTreeClique::shared_ptr& clique, OptimizeData& myData) + //{ + // // Conglomerate our results to the parent + // myData.parentData->results.insert(myData.results); + //} + + /* ************************************************************************* */ + template + VectorValues optimizeBayesTree(const BAYESTREE& bayesTree) + { + gttic(linear_optimizeBayesTree); + //internal::OptimizeData rootData; // Will hold final solution + //treeTraversal::DepthFirstForest(*this, rootData, internal::OptimizePreVisitor, internal::OptimizePostVisitor); + //return rootData.results; + OptimizeData rootData; + OptimizeClique preVisitor; + treeTraversal::DepthFirstForest(bayesTree, rootData, preVisitor); + return preVisitor.collectedResult; + } + } + } +} \ No newline at end of file From 9a10242e96e257a0f88ce4d231af6e6d8bb20d8c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 9 Aug 2013 21:35:42 +0000 Subject: [PATCH 08/18] Fixed warnings --- gtsam/inference/VariableIndex.h | 5 +---- gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/HessianFactor.h | 2 +- gtsam/linear/JacobianFactor.h | 2 +- tests/testGraph.cpp | 4 ++-- 5 files changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/inference/VariableIndex.h b/gtsam/inference/VariableIndex.h index 4733ca59c..14fbc2072 100644 --- a/gtsam/inference/VariableIndex.h +++ b/gtsam/inference/VariableIndex.h @@ -139,10 +139,7 @@ public: template void remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors); - /** Remove unused empty variables at the end of the ordering (in debug mode - * verifies they are empty). - * @param nToRemove The number of unused variables at the end to remove - */ + /** Remove unused empty variables (in debug mode verifies they are empty). */ template void removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey); diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index c8282e8b8..1c9589ce2 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -58,7 +58,7 @@ namespace gtsam { virtual double error(const VectorValues& c) const = 0; /** 0.5*(A*x-b)'*D*(A*x-b) */ /** Return the dimension of the variable pointed to by the given key iterator */ - virtual size_t getDim(const_iterator variable) const = 0; + virtual DenseIndex getDim(const_iterator variable) const = 0; /** * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$ diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 866e79f7b..e380c5c38 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -221,7 +221,7 @@ namespace gtsam { * @param variable An iterator pointing to the slot in this factor. You can * use, for example, begin() + 2 to get the 3rd variable in this factor. */ - virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } + virtual DenseIndex getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } /** Return the number of columns and rows of the Hessian matrix, including the information vector. */ size_t rows() const { return info_.rows(); } diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index f1d66db77..e3412c79d 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -227,7 +227,7 @@ namespace gtsam { /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables? */ - virtual size_t getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } + virtual DenseIndex getDim(const_iterator variable) const { return Ab_(variable - begin()).cols(); } /** * return the number of rows in the corresponding linear system diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index ecd21b659..842f2bd67 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -70,7 +70,7 @@ TEST( Graph, predecessorMap2Graph ) p_map.insert(3, 2); boost::tie(graph, root, key2vertex) = predecessorMap2Graph, SVertex, Key>(p_map); - LONGS_EQUAL(3, boost::num_vertices(graph)); + LONGS_EQUAL(3, (long)boost::num_vertices(graph)); CHECK(root == key2vertex[2]); } @@ -101,7 +101,7 @@ TEST( Graph, composePoses ) expected.insert(3, p3); expected.insert(4, p4); - LONGS_EQUAL(4, actual->size()); + LONGS_EQUAL(4, (long)actual->size()); CHECK(assert_equal(expected, *actual)); } From 222bd888efab28024983f9706cacf9a71b72b43c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 9 Aug 2013 21:35:44 +0000 Subject: [PATCH 09/18] Added VectorValues update function and comment formatting --- gtsam/linear/VectorValues.cpp | 18 ++++++++++++++++++ gtsam/linear/VectorValues.h | 29 +++++++++++++++++++++-------- 2 files changed, 39 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 66c001831..ceb58e551 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -53,6 +53,24 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + void VectorValues::update(const VectorValues& values) + { + iterator hint = begin(); + BOOST_FOREACH(const KeyValuePair& key_value, values) + { + // Use this trick to find the value using a hint, since we are inserting from another sorted map + size_t oldSize = values_.size(); + hint = values_.insert(hint, key_value); + if(values_.size() > oldSize) { + values_.erase(hint); + throw std::out_of_range("Requested to update a VectorValues with another VectorValues that contains keys not present in the first."); + } else { + hint->second = key_value.second; + } + } + } + /* ************************************************************************* */ void VectorValues::insert(const VectorValues& values) { diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 31ad6525b..39e332263 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -157,24 +157,31 @@ namespace gtsam { return item->second; } - /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does not exist, identical to at(Key). */ + /** Read/write access to the vector value with key \c j, throws std::out_of_range if \c j does + * not exist, identical to at(Key). */ Vector& operator[](Key j) { return at(j); } - /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does not exist, identical to at(Key). */ + /** Access the vector value with key \c j (const version), throws std::out_of_range if \c j does + * not exist, identical to at(Key). */ const Vector& operator[](Key j) const { return at(j); } - /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c j is already used. + /** For all key/value pairs in \c values, replace values with corresponding keys in this class + * with those in \c values. Throws std::out_of_range if any keys in \c values are not present + * in this class. */ + void update(const VectorValues& values); + + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. * @param value The vector to be inserted. - * @param j The index with which the value will be associated. - */ + * @param j The index with which the value will be associated. */ void insert(Key j, const Vector& value) { insert(std::pair(j, value)); // Note only passing a reference to the Vector } - /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c j is already used. + /** Insert a vector \c value with key \c j. Throws an invalid_argument exception if the key \c + * j is already used. * @param value The vector to be inserted. - * @param j The index with which the value will be associated. - */ + * @param j The index with which the value will be associated. */ void insert(std::pair key_value) { // Note that here we accept a pair with a reference to the Vector, but the Vector is copied as // it is inserted into the values_ map. @@ -195,6 +202,12 @@ namespace gtsam { std::pair tryInsert(Key j, const Vector& value) { return values_.insert(std::make_pair(j, value)); } + /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */ + void erase(Key var) { + if(values_.erase(var) == 0) + throw std::invalid_argument("Requested variable '" + DefaultKeyFormatter(var) + "', is not in this VectorValues."); + } + /** Set all values to zero vectors. */ void setZero(); From 6843ee822710000fb77194dfa70843da381a164d Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 9 Aug 2013 21:35:45 +0000 Subject: [PATCH 10/18] Change in DoglegOptimizerImpl function arguments to make ISAM2 compatibility easier --- gtsam/nonlinear/DoglegOptimizer.cpp | 12 +++++++++--- gtsam/nonlinear/DoglegOptimizerImpl.h | 15 ++++----------- 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 1e76886bd..03da76eb6 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -64,14 +64,20 @@ void DoglegOptimizer::iterate(void) { if ( params_.isMultifrontal() ) { GaussianBayesTree bt = *linear->eliminateMultifrontal(*params_.ordering, params_.getEliminationFunction()); - result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, state_.error, dlVerbose); + VectorValues dx_u = bt.optimizeGradientSearch(); + VectorValues dx_n = bt.optimize(); + result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, + dx_u, dx_n, bt, graph_, state_.values, state_.error, dlVerbose); } else if ( params_.isSequential() ) { GaussianBayesNet bn = *linear->eliminateSequential(*params_.ordering, params_.getEliminationFunction()); - result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bn, graph_, state_.values, state_.error, dlVerbose); + VectorValues dx_u = bn.optimizeGradientSearch(); + VectorValues dx_n = bn.optimize(); + result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, + dx_u, dx_n, bn, graph_, state_.values, state_.error, dlVerbose); } else if ( params_.isCG() ) { - throw runtime_error("todo: "); + throw runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver"); } else { throw runtime_error("Optimization parameter is invalid: DoglegParams::elimination"); diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index 3344da43a..c0b52f88b 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -100,8 +100,8 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { */ template static IterationResult Iterate( - double Delta, TrustRegionAdaptationMode mode, const M& Rd, - const F& f, const VALUES& x0, const double f_error, const bool verbose=false); + double Delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n, + const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose=false); /** * Compute the dogleg point given a trust region radius \f$ \Delta \f$. The @@ -143,16 +143,9 @@ struct GTSAM_EXPORT DoglegOptimizerImpl { /* ************************************************************************* */ template typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( - double Delta, TrustRegionAdaptationMode mode, const M& Rd, - const F& f, const VALUES& x0, const double f_error, const bool verbose) + double Delta, TrustRegionAdaptationMode mode, const VectorValues& dx_u, const VectorValues& dx_n, + const M& Rd, const F& f, const VALUES& x0, const double f_error, const bool verbose) { - // Compute steepest descent and Newton's method points - gttic(optimizeGradientSearch); - VectorValues dx_u = GaussianFactorGraph(Rd).optimizeGradientSearch(); - gttoc(optimizeGradientSearch); - gttic(optimize); - VectorValues dx_n = Rd.optimize(); - gttoc(optimize); gttic(M_error); const double M_error = Rd.error(VectorValues::Zero(dx_u)); gttoc(M_error); From 789f2bee97fe62b8c074e5b0235d192e2675a4a9 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 9 Aug 2013 21:35:47 +0000 Subject: [PATCH 11/18] ISAM2 compiling and fixed several issues but still some unit tests failing --- gtsam/inference/BayesTree-inst.h | 11 +- gtsam/nonlinear/ISAM2-impl.cpp | 397 +++++++--------------- gtsam/nonlinear/ISAM2-impl.h | 45 +-- gtsam/nonlinear/ISAM2-inl.h | 93 +++--- gtsam/nonlinear/ISAM2.cpp | 210 ++++++------ gtsam/nonlinear/ISAM2.h | 83 ++--- tests/testGaussianISAM2.cpp | 543 +++++++++---------------------- 7 files changed, 482 insertions(+), 900 deletions(-) diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 8ff8c90d4..2e18723c2 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -234,7 +234,10 @@ namespace gtsam { template void BayesTree::fillNodesIndex(const sharedClique& subtree) { // Add each frontal variable of this root node - BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; } + BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { + bool inserted = nodes_.insert(std::make_pair(j, subtree)).second; + assert(inserted); (void)inserted; + } // Fill index for each child typedef typename BayesTree::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& child, subtree->children) { @@ -336,7 +339,7 @@ namespace gtsam { // Factor the shortcuts to be conditioned on the full root // Get the set of variables to eliminate, which is C1\B. gttic(Full_root_factoring); - shared_ptr p_C1_B; { + boost::shared_ptr p_C1_B; { std::vector C1_minus_B; { FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); BOOST_FOREACH(const Index j, *B->conditional()) { @@ -348,7 +351,7 @@ namespace gtsam { boost::tie(p_C1_B, temp_remaining) = FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function); } - shared_ptr p_C2_B; { + boost::shared_ptr p_C2_B; { std::vector C2_minus_B; { FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); BOOST_FOREACH(const Index j, *B->conditional()) { @@ -422,7 +425,7 @@ namespace gtsam { void BayesTree::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans) { // base case is NULL, if so we do nothing and return empties above - if (clique!=NULL) { + if (clique) { // remove the clique from orphans in case it has been added earlier orphans.remove(clique); diff --git a/gtsam/nonlinear/ISAM2-impl.cpp b/gtsam/nonlinear/ISAM2-impl.cpp index 84c264563..b95b6f6fd 100644 --- a/gtsam/nonlinear/ISAM2-impl.cpp +++ b/gtsam/nonlinear/ISAM2-impl.cpp @@ -30,126 +30,60 @@ namespace gtsam { /* ************************************************************************* */ void ISAM2::Impl::AddVariables( const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, vector& replacedKeys, - Ordering& ordering, const KeyFormatter& keyFormatter) { + VectorValues& deltaNewton, VectorValues& RgProd, + const KeyFormatter& keyFormatter) +{ const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); if(debug) newTheta.print("The new variables are: "); - // Add the new keys onto the ordering, add zeros to the delta for the new variables - std::vector dims(newTheta.dims(*newTheta.orderingArbitrary())); - if(debug) cout << "New variables have total dimensionality " << accumulate(dims.begin(), dims.end(), 0) << endl; - const size_t originalnVars = delta.size(); - delta.append(dims); - deltaNewton.append(dims); - RgProd.append(dims); - for(Index j = originalnVars; j < delta.size(); ++j) { - delta[j].setZero(); - deltaNewton[j].setZero(); - RgProd[j].setZero(); - } - { - Index nextVar = originalnVars; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { - ordering.insert(key_value.key, nextVar); - if(debug) cout << "Adding variable " << keyFormatter(key_value.key) << " with order " << nextVar << endl; - ++ nextVar; - } - assert(ordering.size() == delta.size()); - } - replacedKeys.resize(ordering.size(), false); + // Add zeros into the VectorValues + delta.insert(newTheta.zeroVectors()); + deltaNewton.insert(newTheta.zeroVectors()); + RgProd.insert(newTheta.zeroVectors()); } /* ************************************************************************* */ -void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, +void ISAM2::Impl::RemoveVariables(const FastSet& unusedKeys, const std::vector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, VectorValues& RgProd, - std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, - GaussianFactorGraph& linearFactors, FastSet& fixedVariables) { - - // Get indices of unused keys - vector unusedIndices; unusedIndices.reserve(unusedKeys.size()); - BOOST_FOREACH(Key key, unusedKeys) { unusedIndices.push_back(ordering[key]); } - - // Create a permutation that shifts the unused variables to the end - Permutation unusedToEnd = Permutation::PushToBack(unusedIndices, delta.size()); - Permutation unusedToEndInverse = *unusedToEnd.inverse(); - - // Use the permutation to remove unused variables while shifting all the others to take up the space - variableIndex.permuteInPlace(unusedToEnd); - variableIndex.removeUnusedAtEnd(unusedIndices.size()); - { - // Create a vector of variable dimensions with the unused ones removed - // by applying the unusedToEnd permutation to the original vector of - // variable dimensions. We only allocate space in the shifted dims - // vector for the used variables, so that the unused ones are dropped - // when the permutation is applied. - vector originalDims = delta.dims(); - vector dims(delta.size() - unusedIndices.size()); - unusedToEnd.applyToCollection(dims, originalDims); - - // Copy from the old data structures to new ones, only iterating up to - // the number of used variables, and applying the unusedToEnd permutation - // in order to remove the unused variables. - VectorValues newDelta(dims); - VectorValues newDeltaNewton(dims); - VectorValues newDeltaGradSearch(dims); - std::vector newReplacedKeys(replacedKeys.size() - unusedIndices.size()); - Base::Nodes newNodes(replacedKeys.size() - unusedIndices.size()); - - for(size_t j = 0; j < dims.size(); ++j) { - newDelta[j] = delta[unusedToEnd[j]]; - newDeltaNewton[j] = deltaNewton[unusedToEnd[j]]; - newDeltaGradSearch[j] = RgProd[unusedToEnd[j]]; - newReplacedKeys[j] = replacedKeys[unusedToEnd[j]]; - newNodes[j] = nodes[unusedToEnd[j]]; - } - - // Swap the new data structures with the outputs of this function - delta.swap(newDelta); - deltaNewton.swap(newDeltaNewton); - RgProd.swap(newDeltaGradSearch); - replacedKeys.swap(newReplacedKeys); - nodes.swap(newNodes); - } - - // Reorder and remove from ordering, solution, and fixed keys - ordering.permuteInPlace(unusedToEnd); - BOOST_REVERSE_FOREACH(Key key, unusedKeys) { - Ordering::value_type removed = ordering.pop_back(); - assert(removed.first == key); - theta.erase(key); - fixedVariables.erase(key); - } - - // Finally, permute references to variables - if(root) - root->permuteWithInverse(unusedToEndInverse); - linearFactors.permuteWithInverse(unusedToEndInverse); + FastSet& replacedKeys, Base::Nodes& nodes, + FastSet& fixedVariables) +{ + variableIndex.removeUnusedVariables(unusedKeys.begin(), unusedKeys.end()); + BOOST_FOREACH(Key key, unusedKeys) { + delta.erase(key); + deltaNewton.erase(key); + RgProd.erase(key); + replacedKeys.erase(key); + nodes.erase(key); + theta.erase(key); + fixedVariables.erase(key); + } } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { +FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) +{ FastSet relinKeys; - if(relinearizeThreshold.type() == typeid(double)) { - double threshold = boost::get(relinearizeThreshold); - for(Index var=0; var(); - if(maxDelta >= threshold) { - relinKeys.insert(var); - } + if(const double* threshold = boost::get(relinearizeThreshold)) + { + BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) { + double maxDelta = key_delta.second.lpNorm(); + if(maxDelta >= *threshold) + relinKeys.insert(key_delta.first); } - } else if(relinearizeThreshold.type() == typeid(FastMap)) { - const FastMap& thresholds = boost::get >(relinearizeThreshold); - BOOST_FOREACH(const Ordering::value_type& key_index, ordering) { - const Vector& threshold = thresholds.find(Symbol(key_index.first).chr())->second; - Index j = key_index.second; - if(threshold.rows() != delta[j].rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality"); - if((delta[j].array().abs() > threshold.array()).any()) - relinKeys.insert(j); + } + else if(const FastMap* thresholds = boost::get*>(relinearizeThreshold)) + { + BOOST_FOREACH(const VectorValues::KeyValuePair& key_delta, delta) { + const Vector& threshold = thresholds->find(Symbol(key_delta.first).chr())->second; + if(threshold.rows() != key_delta.second.rows()) + throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(key_delta.first).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); + if((key_delta.second.array().abs() > threshold.array()).any()) + relinKeys.insert(key_delta.first); } } @@ -157,11 +91,12 @@ FastSet ISAM2::Impl::CheckRelinearizationFull(const VectorValues& delta, } /* ************************************************************************* */ -void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) { - +void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double threshold, + const VectorValues& delta, const ISAM2Clique::shared_ptr& clique) +{ // Check the current clique for relinearization bool relinearize = false; - BOOST_FOREACH(Index var, clique->conditional()->keys()) { + BOOST_FOREACH(Key var, *clique->conditional()) { double maxDelta = delta[var].lpNorm(); if(maxDelta >= threshold) { relinKeys.insert(var); @@ -171,31 +106,31 @@ void CheckRelinearizationRecursiveDouble(FastSet& relinKeys, double thres // If this node was relinearized, also check its children if(relinearize) { - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { CheckRelinearizationRecursiveDouble(relinKeys, threshold, delta, child); } } } /* ************************************************************************* */ -void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, const VectorValues& delta, const Ordering& ordering, const ISAM2Clique::shared_ptr& clique) { - +void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMap& thresholds, + const VectorValues& delta, + const ISAM2Clique::shared_ptr& clique) +{ // Check the current clique for relinearization bool relinearize = false; - BOOST_FOREACH(Index var, clique->conditional()->keys()) { - - // Lookup the key associated with this index - Key key = ordering.key(var); - + BOOST_FOREACH(Key var, *clique->conditional()) { // Find the threshold for this variable type - const Vector& threshold = thresholds.find(Symbol(key).chr())->second; + const Vector& threshold = thresholds.find(Symbol(var).chr())->second; + + const Vector& deltaVar = delta[var]; // Verify the threshold vector matches the actual variable size - if(threshold.rows() != delta[var].rows()) - throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality"); + if(threshold.rows() != deltaVar.rows()) + throw std::invalid_argument("Relinearization threshold vector dimensionality for '" + std::string(1, Symbol(var).chr()) + "' passed into iSAM2 parameters does not match actual variable dimensionality."); // Check for relinearization - if((delta[var].array().abs() > threshold.array()).any()) { + if((deltaVar.array().abs() > threshold.array()).any()) { relinKeys.insert(var); relinearize = true; } @@ -203,52 +138,54 @@ void CheckRelinearizationRecursiveMap(FastSet& relinKeys, const FastMapchildren()) { - CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, ordering, child); + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + CheckRelinearizationRecursiveMap(relinKeys, thresholds, delta, child); } } } /* ************************************************************************* */ -FastSet ISAM2::Impl::CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { - +FastSet ISAM2::Impl::CheckRelinearizationPartial(const std::vector& roots, + const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) +{ FastSet relinKeys; - - if(root) { - if(relinearizeThreshold.type() == typeid(double)) { + BOOST_FOREACH(const ISAM2::sharedClique& root, roots) { + if(relinearizeThreshold.type() == typeid(double)) CheckRelinearizationRecursiveDouble(relinKeys, boost::get(relinearizeThreshold), delta, root); - } else if(relinearizeThreshold.type() == typeid(FastMap)) { - CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, ordering, root); - } + else if(relinearizeThreshold.type() == typeid(FastMap)) + CheckRelinearizationRecursiveMap(relinKeys, boost::get >(relinearizeThreshold), delta, root); } - return relinKeys; } /* ************************************************************************* */ -void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { +void ISAM2::Impl::FindAll(ISAM2Clique::shared_ptr clique, FastSet& keys, const FastSet& markedMask) +{ static const bool debug = false; // does the separator contain any of the variables? bool found = false; - BOOST_FOREACH(const Index& key, (*clique)->parents()) { - if (markedMask[key]) + BOOST_FOREACH(Key key, clique->conditional()->parents()) { + if (markedMask.exists(key)) { found = true; + break; + } } if (found) { // then add this clique - keys.insert((*clique)->beginFrontals(), (*clique)->endFrontals()); + keys.insert(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals()); if(debug) clique->print("Key(s) marked in clique "); - if(debug) cout << "so marking key " << (*clique)->keys().front() << endl; + if(debug) cout << "so marking key " << clique->conditional()->front() << endl; } - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { FindAll(child, keys, markedMask); } } /* ************************************************************************* */ -void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const Ordering& ordering, - const vector& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) { +void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, + const FastSet& mask, boost::optional invalidateIfDebug, const KeyFormatter& keyFormatter) +{ // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions // if we try to re-use them. @@ -256,23 +193,16 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const invalidateIfDebug = boost::none; #endif - assert(values.size() == ordering.size()); - assert(delta.size() == ordering.size()); + assert(values.size() == delta.size()); Values::iterator key_value; - Ordering::const_iterator key_index; - for(key_value = values.begin(), key_index = ordering.begin(); - key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) { - assert(key_value->key == key_index->first); - const Index var = key_index->second; - if(ISDEBUG("ISAM2 update verbose")) { - if(mask[var]) - cout << "expmap " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - else - cout << " " << keyFormatter(key_value->key) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - } + VectorValues::const_iterator key_delta; + for(key_value = values.begin(), key_delta = delta.begin(); key_value != values.end(); ++key_value, ++key_delta) + { + assert(key_value->key == key_delta->first); + Key var = key_value->key; assert(delta[var].size() == (int)key_value->value.dim()); assert(delta[var].unaryExpr(ptr_fun(isfinite)).all()); - if(mask[var]) { + if(mask.exists(var)) { Value* retracted = key_value->value.retract_(delta[var]); key_value->value = *retracted; retracted->deallocate_(); @@ -282,134 +212,35 @@ void ISAM2::Impl::ExpmapMasked(Values& values, const VectorValues& delta, const } } -/* ************************************************************************* */ -ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, - const FastSet& keys, const ReorderingMode& reorderingMode, bool useQR) { - - const bool debug = ISDEBUG("ISAM2 recalculate"); - - PartialSolveResult result; - - gttic(select_affected_variables); -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - // Debug check that all variables involved in the factors to be re-eliminated - // are in affectedKeys, since we will use it to select a subset of variables. - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - BOOST_FOREACH(Index key, factor->keys()) { - assert(find(keys.begin(), keys.end(), key) != keys.end()); - } - } -#endif - Permutation affectedKeysSelector(keys.size()); // Create a permutation that pulls the affected keys to the front - Permutation affectedKeysSelectorInverse(keys.size() > 0 ? *keys.rbegin()+1 : 0 /*ordering_.nVars()*/); // And its inverse -#ifndef NDEBUG - // If debugging, fill with invalid values that will trip asserts if dereferenced - std::fill(affectedKeysSelectorInverse.begin(), affectedKeysSelectorInverse.end(), numeric_limits::max()); -#endif - { Index position=0; BOOST_FOREACH(Index var, keys) { - affectedKeysSelector[position] = var; - affectedKeysSelectorInverse[var] = position; - ++ position; } } - if(debug) affectedKeysSelector.print("affectedKeysSelector: "); - if(debug) affectedKeysSelectorInverse.print("affectedKeysSelectorInverse: "); - factors.permuteWithInverse(affectedKeysSelectorInverse); - if(debug) factors.print("Factors to reorder/re-eliminate: "); - gttoc(select_affected_variables); - gttic(variable_index); - VariableIndex affectedFactorsIndex(factors); // Create a variable index for the factors to be re-eliminated - if(debug) affectedFactorsIndex.print("affectedFactorsIndex: "); - gttoc(variable_index); - gttic(ccolamd); - vector cmember(affectedKeysSelector.size(), 0); - if(reorderingMode.constrain == ReorderingMode::CONSTRAIN_LAST) { - assert(reorderingMode.constrainedKeys); - if(!reorderingMode.constrainedKeys->empty()) { - typedef std::pair Index_Group; - if(keys.size() > reorderingMode.constrainedKeys->size()) { // Only if some variables are unconstrained - BOOST_FOREACH(const Index_Group& index_group, *reorderingMode.constrainedKeys) { - cmember[affectedKeysSelectorInverse[index_group.first]] = index_group.second; } - } else { - int minGroup = *boost::range::min_element(boost::adaptors::values(*reorderingMode.constrainedKeys)); - BOOST_FOREACH(const Index_Group& index_group, *reorderingMode.constrainedKeys) { - cmember[affectedKeysSelectorInverse[index_group.first]] = index_group.second - minGroup; } - } - } - } - Permutation::shared_ptr affectedColamd(inference::PermutationCOLAMD_(affectedFactorsIndex, cmember)); - gttoc(ccolamd); - gttic(ccolamd_permutations); - Permutation::shared_ptr affectedColamdInverse(affectedColamd->inverse()); - if(debug) affectedColamd->print("affectedColamd: "); - if(debug) affectedColamdInverse->print("affectedColamdInverse: "); - result.reorderingSelector = affectedKeysSelector; - result.reorderingPermutation = *affectedColamd; - result.reorderingInverse = internal::Reduction::CreateFromPartialPermutation(affectedKeysSelector, *affectedColamdInverse); - gttoc(ccolamd_permutations); - - gttic(permute_affected_variable_index); - affectedFactorsIndex.permuteInPlace(*affectedColamd); - gttoc(permute_affected_variable_index); - - gttic(permute_affected_factors); - factors.permuteWithInverse(*affectedColamdInverse); - gttoc(permute_affected_factors); - - if(debug) factors.print("Colamd-ordered affected factors: "); - -#ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - VariableIndex fromScratchIndex(factors); - assert(assert_equal(fromScratchIndex, affectedFactorsIndex)); -#endif - - // eliminate into a Bayes net - gttic(eliminate); - JunctionTree jt(factors, affectedFactorsIndex); - if(!useQR) - result.bayesTree = jt.eliminate(EliminatePreferCholesky); - else - result.bayesTree = jt.eliminate(EliminateQR); - gttoc(eliminate); - - gttic(permute_eliminated); - if(result.bayesTree) result.bayesTree->permuteWithInverse(affectedKeysSelector); - if(debug && result.bayesTree) { - 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); - gttoc(permute_eliminated); - - return result; -} - /* ************************************************************************* */ namespace internal { inline static void optimizeInPlace(const boost::shared_ptr& clique, VectorValues& result) { // parents are assumed to already be solved and available in result - clique->conditional()->solveInPlace(result); + result.update(clique->conditional()->solve(result)); // starting from the root, call optimize on each conditional - BOOST_FOREACH(const boost::shared_ptr& child, clique->children_) + BOOST_FOREACH(const boost::shared_ptr& child, clique->children) optimizeInPlace(child, result); } } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold) { +size_t ISAM2::Impl::UpdateDelta(const std::vector& roots, FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold) { size_t lastBacksubVariableCount; if (wildfireThreshold <= 0.0) { // Threshold is zero or less, so do a full recalculation - internal::optimizeInPlace(root, delta); + BOOST_FOREACH(const ISAM2::sharedClique& root, roots) + internal::optimizeInPlace(root, delta); lastBacksubVariableCount = delta.size(); } else { // Optimize with wildfire - lastBacksubVariableCount = optimizeWildfireNonRecursive(root, wildfireThreshold, replacedKeys, delta); // modifies delta_ + lastBacksubVariableCount = 0; + BOOST_FOREACH(const ISAM2::sharedClique& root, roots) + lastBacksubVariableCount += optimizeWildfireNonRecursive( + root, wildfireThreshold, replacedKeys, delta); // modifies delta #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS for(size_t j=0; j& root, std: } // Clear replacedKeys - replacedKeys.assign(replacedKeys.size(), false); + replacedKeys.clear(); return lastBacksubVariableCount; } /* ************************************************************************* */ namespace internal { -void updateDoglegDeltas(const boost::shared_ptr& clique, const std::vector& replacedKeys, - const VectorValues& grad, VectorValues& deltaNewton, VectorValues& RgProd, size_t& varsUpdated) { +void updateDoglegDeltas(const boost::shared_ptr& clique, const FastSet& replacedKeys, + const VectorValues& grad, VectorValues& RgProd, size_t& varsUpdated) { // Check if any frontal or separator keys were recalculated, if so, we need // update deltas and recurse to children, but if not, we do not need to // recurse further because of the running separator property. bool anyReplaced = false; - BOOST_FOREACH(Index j, *clique->conditional()) { - if(replacedKeys[j]) { + BOOST_FOREACH(Key j, *clique->conditional()) { + if(replacedKeys.exists(j)) { anyReplaced = true; break; } @@ -442,41 +273,49 @@ void updateDoglegDeltas(const boost::shared_ptr& clique, const std: if(anyReplaced) { // Update the current variable // Get VectorValues slice corresponding to current variables - Vector gR = internal::extractVectorValuesSlices(grad, (*clique)->beginFrontals(), (*clique)->endFrontals()); - Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents()); + Vector gR = grad.vector(std::vector(clique->conditional()->beginFrontals(), clique->conditional()->endFrontals())); + Vector gS = grad.vector(std::vector(clique->conditional()->beginParents(), clique->conditional()->endParents())); // Compute R*g and S*g for this clique - Vector RSgProd = (*clique)->get_R() * gR + (*clique)->get_S() * gS; + Vector RSgProd = clique->conditional()->get_R() * gR + clique->conditional()->get_S() * gS; // Write into RgProd vector - internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals()); + DenseIndex vectorPosition = 0; + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + Vector& RgProdValue = RgProd[frontal]; + RgProdValue = RSgProd.segment(vectorPosition, RgProdValue.size()); + vectorPosition += RgProdValue.size(); + } // Now solve the part of the Newton's method point for this clique (back-substitution) //(*clique)->solveInPlace(deltaNewton); - varsUpdated += (*clique)->nrFrontals(); + varsUpdated += clique->conditional()->nrFrontals(); // Recurse to children - BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children()) { - updateDoglegDeltas(child, replacedKeys, grad, deltaNewton, RgProd, varsUpdated); } + BOOST_FOREACH(const ISAM2Clique::shared_ptr& child, clique->children) { + updateDoglegDeltas(child, replacedKeys, grad, RgProd, varsUpdated); } } } } /* ************************************************************************* */ -size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, +size_t ISAM2::Impl::UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet& replacedKeys, VectorValues& deltaNewton, VectorValues& RgProd) { // Get gradient - VectorValues grad = *allocateVectorValues(isam); + VectorValues grad; gradientAtZero(isam, grad); // Update variables size_t varsUpdated = 0; - internal::updateDoglegDeltas(isam.root(), replacedKeys, grad, deltaNewton, RgProd, varsUpdated); - optimizeWildfireNonRecursive(isam.root(), wildfireThreshold, replacedKeys, deltaNewton); + BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) + { + internal::updateDoglegDeltas(root, replacedKeys, grad, RgProd, varsUpdated); + optimizeWildfireNonRecursive(root, wildfireThreshold, replacedKeys, deltaNewton); + } - replacedKeys.assign(replacedKeys.size(), false); + replacedKeys.clear(); return varsUpdated; } diff --git a/gtsam/nonlinear/ISAM2-impl.h b/gtsam/nonlinear/ISAM2-impl.h index fa136f1be..2463f55e7 100644 --- a/gtsam/nonlinear/ISAM2-impl.h +++ b/gtsam/nonlinear/ISAM2-impl.h @@ -26,9 +26,6 @@ struct GTSAM_EXPORT ISAM2::Impl { struct GTSAM_EXPORT PartialSolveResult { ISAM2::sharedClique bayesTree; - Permutation reorderingSelector; - Permutation reorderingPermutation; - internal::Reduction reorderingInverse; }; struct GTSAM_EXPORT ReorderingMode { @@ -48,16 +45,16 @@ struct GTSAM_EXPORT ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void AddVariables(const Values& newTheta, Values& theta, VectorValues& delta, - VectorValues& deltaNewton, VectorValues& RgProd, std::vector& replacedKeys, - Ordering& ordering, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + VectorValues& deltaNewton, VectorValues& RgProd, + const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Remove variables from the ISAM2 system. */ - static void RemoveVariables(const FastSet& unusedKeys, const ISAM2Clique::shared_ptr& root, + static void RemoveVariables(const FastSet& unusedKeys, const std::vector& roots, Values& theta, VariableIndex& variableIndex, VectorValues& delta, VectorValues& deltaNewton, - VectorValues& RgProd, std::vector& replacedKeys, Ordering& ordering, Base::Nodes& nodes, - GaussianFactorGraph& linearFactors, FastSet& fixedVariables); + VectorValues& RgProd, FastSet& replacedKeys, Base::Nodes& nodes, + FastSet& fixedVariables); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -68,8 +65,8 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationFull(const VectorValues& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + static FastSet CheckRelinearizationFull(const VectorValues& delta, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** * Find the set of variables to be relinearized according to relinearizeThreshold. @@ -82,8 +79,8 @@ struct GTSAM_EXPORT ISAM2::Impl { * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearizationPartial(const ISAM2Clique::shared_ptr& root, const VectorValues& delta, const Ordering& ordering, - const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); + static FastSet CheckRelinearizationPartial(const std::vector& roots, + const VectorValues& delta, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); /** * Recursively search this clique and its children for marked keys appearing @@ -116,30 +113,14 @@ struct GTSAM_EXPORT ISAM2::Impl { * @param keyFormatter Formatter for printing nonlinear keys during debugging */ static void ExpmapMasked(Values& values, const VectorValues& delta, - const Ordering& ordering, const std::vector& mask, + const FastSet& mask, boost::optional invalidateIfDebug = boost::none, const KeyFormatter& keyFormatter = DefaultKeyFormatter); - /** - * Reorder and eliminate factors. These factors form a subset of the full - * problem, so along with the BayesTree we get a partial reordering of the - * problem that needs to be applied to the other data in ISAM2, which is the - * VariableIndex, the delta, the ordering, and the orphans (including cached - * factors). - * \param factors The factors to eliminate, representing part of the full - * problem. This is permuted during use and so is cleared when this function - * returns in order to invalidate it. - * \param keys The set of indices used in \c factors. - * \param useQR Whether to use QR (if true), or Cholesky (if false). - * \return The eliminated BayesTree and the permutation to be applied to the - * rest of the ISAM2 data. - */ - static PartialSolveResult PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, - const ReorderingMode& reorderingMode, bool useQR); + static size_t UpdateDelta(const std::vector& roots, + FastSet& replacedKeys, VectorValues& delta, double wildfireThreshold); - static size_t UpdateDelta(const boost::shared_ptr& root, std::vector& replacedKeys, VectorValues& delta, double wildfireThreshold); - - static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, std::vector& replacedKeys, + static size_t UpdateDoglegDeltas(const ISAM2& isam, double wildfireThreshold, FastSet& replacedKeys, VectorValues& deltaNewton, VectorValues& RgProd); }; diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index 4a9cf105a..71904a56a 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -28,8 +28,7 @@ namespace gtsam { /* ************************************************************************* */ template VALUE ISAM2::calculateEstimate(Key key) const { - const Index index = getOrdering()[key]; - const Vector& delta = getDelta()[index]; + const Vector& delta = getDelta()[key]; return theta_.at(key).retract(delta); } @@ -37,24 +36,25 @@ VALUE ISAM2::calculateEstimate(Key key) const { namespace internal { template void optimizeWildfire(const boost::shared_ptr& clique, double threshold, - std::vector& changed, const std::vector& replaced, VectorValues& delta, int& count) { + FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) +{ // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the // cliques in the children need to be processed // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced[(*clique)->frontals().front()]; + bool cliqueReplaced = replaced.exists((*clique)->frontals().front()); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - assert(cliqueReplaced == replaced[frontal]); + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + assert(cliqueReplaced == replaced.exists(frontal)); } #endif // If not redone, then has one of the separator variables changed significantly? bool recalculate = cliqueReplaced; if(!recalculate) { - BOOST_FOREACH(Index parent, (*clique)->parents()) { - if(changed[parent]) { + BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + if(changed.exists(parent)) { recalculate = true; break; } @@ -66,22 +66,22 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, if(recalculate) { // Temporary copy of the original values, to check how much they change - std::vector originalValues((*clique)->nrFrontals()); + std::vector originalValues(clique->conditional()->nrFrontals()); GaussianConditional::const_iterator it; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - originalValues[it - (*clique)->beginFrontals()] = delta[*it]; + for(it = clique->conditional()->beginFrontals(); it!=clique->conditional()->endFrontals(); it++) { + originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; } // Back-substitute - (*clique)->solveInPlace(delta); - count += (*clique)->nrFrontals(); + delta.update(clique->conditional()->solve(delta)); + count += clique->conditional()->nrFrontals(); // Whether the values changed above a threshold, or always true if the // clique was replaced. bool valuesChanged = cliqueReplaced; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { if(!valuesChanged) { - const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); + const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); const Vector& newValue(delta[*it]); if((oldValue - newValue).lpNorm() >= threshold) { valuesChanged = true; @@ -94,18 +94,18 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, // If the values were above the threshold or this clique was replaced if(valuesChanged) { // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - changed[frontal] = true; + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + changed.insert(frontal); } } else { // Replace with the old values - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { + delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; } } // Recurse to children - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children_) { + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, clique->children) { optimizeWildfire(child, threshold, changed, replaced, delta, count); } } @@ -113,24 +113,25 @@ void optimizeWildfire(const boost::shared_ptr& clique, double threshold, template bool optimizeWildfireNode(const boost::shared_ptr& clique, double threshold, - std::vector& changed, const std::vector& replaced, VectorValues& delta, int& count) { + FastSet& changed, const FastSet& replaced, VectorValues& delta, size_t& count) +{ // if none of the variables in this clique (frontal and separator!) changed // significantly, then by the running intersection property, none of the // cliques in the children need to be processed // Are any clique variables part of the tree that has been redone? - bool cliqueReplaced = replaced[(*clique)->frontals().front()]; + bool cliqueReplaced = replaced.exists(clique->conditional()->frontals().front()); #ifdef GTSAM_EXTRA_CONSISTENCY_CHECKS - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - assert(cliqueReplaced == replaced[frontal]); + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + assert(cliqueReplaced == replaced.exists(frontal)); } #endif // If not redone, then has one of the separator variables changed significantly? bool recalculate = cliqueReplaced; if(!recalculate) { - BOOST_FOREACH(Index parent, (*clique)->parents()) { - if(changed[parent]) { + BOOST_FOREACH(Key parent, clique->conditional()->parents()) { + if(changed.exists(parent)) { recalculate = true; break; } @@ -139,25 +140,25 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // Solve clique if it was replaced, or if any parents were changed above the // threshold or themselves replaced. - if(recalculate) { - + if(recalculate) + { // Temporary copy of the original values, to check how much they change - std::vector originalValues((*clique)->nrFrontals()); + std::vector originalValues(clique->conditional()->nrFrontals()); GaussianConditional::const_iterator it; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - originalValues[it - (*clique)->beginFrontals()] = delta[*it]; + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { + originalValues[it - clique->conditional()->beginFrontals()] = delta[*it]; } // Back-substitute - (*clique)->solveInPlace(delta); - count += (*clique)->nrFrontals(); + delta.update(clique->conditional()->solve(delta)); + count += clique->conditional()->nrFrontals(); // Whether the values changed above a threshold, or always true if the // clique was replaced. bool valuesChanged = cliqueReplaced; - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { if(!valuesChanged) { - const Vector& oldValue(originalValues[it - (*clique)->beginFrontals()]); + const Vector& oldValue(originalValues[it - clique->conditional()->beginFrontals()]); const Vector& newValue(delta[*it]); if((oldValue - newValue).lpNorm() >= threshold) { valuesChanged = true; @@ -170,16 +171,15 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh // If the values were above the threshold or this clique was replaced if(valuesChanged) { // Set changed flag for each frontal variable and leave the new values - BOOST_FOREACH(Index frontal, (*clique)->frontals()) { - changed[frontal] = true; + BOOST_FOREACH(Key frontal, clique->conditional()->frontals()) { + changed.insert(frontal); } } else { // Replace with the old values - for(it = (*clique)->beginFrontals(); it!=(*clique)->endFrontals(); it++) { - delta[*it] = originalValues[it - (*clique)->beginFrontals()]; + for(it = clique->conditional()->beginFrontals(); it != clique->conditional()->endFrontals(); it++) { + delta[*it] = originalValues[it - clique->conditional()->beginFrontals()]; } } - } return recalculate; @@ -189,8 +189,8 @@ bool optimizeWildfireNode(const boost::shared_ptr& clique, double thresh /* ************************************************************************* */ template -int optimizeWildfire(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValues& delta) { - std::vector changed(keys.size(), false); +size_t optimizeWildfire(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) { + FastSet changed; int count = 0; // starting from the root, call optimize on each conditional if(root) @@ -200,9 +200,10 @@ int optimizeWildfire(const boost::shared_ptr& root, double threshold, co /* ************************************************************************* */ template -int optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const std::vector& keys, VectorValues& delta) { - std::vector changed(keys.size(), false); - int count = 0; +size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, double threshold, const FastSet& keys, VectorValues& delta) +{ + FastSet changed; + size_t count = 0; if (root) { std::stack > travStack; @@ -213,7 +214,7 @@ int optimizeWildfireNonRecursive(const boost::shared_ptr& root, double t travStack.pop(); bool recalculate = internal::optimizeWildfireNode(currentNode, threshold, changed, keys, delta, count); if (recalculate) { - BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, currentNode->children_) { + BOOST_FOREACH(const typename CLIQUE::shared_ptr& child, currentNode->children) { travStack.push(child); } } diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index e5aba4731..7047d7990 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -26,7 +26,9 @@ namespace br { using namespace boost::range; using namespace boost::adaptors; } #include #include #include +#include #include // We need the inst file because we'll make a special JT templated on ISAM2 +#include #include #include #include @@ -36,9 +38,13 @@ namespace br { using namespace boost::range; using namespace boost::adaptors; } #include #include +using namespace std; + namespace gtsam { -using namespace std; +// Instantiate base classes +template class BayesTreeCliqueBase; +template class BayesTree; static const bool disableReordering = false; static const double batchThreshold = 0.65; @@ -113,14 +119,14 @@ std::string ISAM2Params::factorizationTranslator(const ISAM2Params::Factorizatio } /* ************************************************************************* */ -void ISAM2Clique::setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult) +void ISAM2Clique::setEliminationResult(const 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(), + gradientContribution_ << -conditional_->get_R().transpose() * conditional_->get_d(), -conditional_->get_S().transpose() * conditional_->get_d(); } @@ -159,37 +165,11 @@ ISAM2::ISAM2(): } /* ************************************************************************* */ -ISAM2::ISAM2(const ISAM2& other) { - *this = other; -} - -/* ************************************************************************* */ -ISAM2& ISAM2::operator=(const ISAM2& rhs) -{ - // Copy BayesTree - this->Base::operator=(rhs); - - // Copy our variables - theta_ = rhs.theta_; - variableIndex_ = rhs.variableIndex_; - delta_ = rhs.delta_; - deltaNewton_ = rhs.deltaNewton_; - RgProd_ = rhs.RgProd_; - deltaDoglegUptodate_ = rhs.deltaDoglegUptodate_; - deltaUptodate_ = rhs.deltaUptodate_; - deltaReplacedMask_ = rhs.deltaReplacedMask_; - nonlinearFactors_ = rhs.nonlinearFactors_; - linearFactors_ = rhs.linearFactors_; - params_ = rhs.params_; - doglegDelta_ = rhs.doglegDelta_; - lastAffectedVariableCount = rhs.lastAffectedVariableCount; - lastAffectedFactorCount = rhs.lastAffectedFactorCount; - lastAffectedCliqueCount = rhs.lastAffectedCliqueCount; - lastAffectedMarkedCount = rhs.lastAffectedMarkedCount; - lastBacksubVariableCount = rhs.lastBacksubVariableCount; - lastNnzTop = rhs.lastNnzTop; - - return *this; +bool ISAM2::equals(const ISAM2& other, double tol) const { + return Base::equals(other, tol) + && theta_.equals(other.theta_, tol) && variableIndex_.equals(other.variableIndex_, tol) + && nonlinearFactors_.equals(other.nonlinearFactors_, tol) + && fixedVariables_ == other.fixedVariables_; } /* ************************************************************************* */ @@ -337,12 +317,9 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe gttic(removetop); Cliques orphans; GaussianBayesNet affectedBayesNet; - this->removeTop(markedKeys, affectedBayesNet, orphans); + this->removeTop(vector(markedKeys.begin(), markedKeys.end()), affectedBayesNet, orphans); gttoc(removetop); - if(debug) affectedBayesNet.print("Removed top: "); - if(debug) orphans.print("Orphans: "); - // FactorGraph factors(affectedBayesNet); // bug was here: we cannot reuse the original factors, because then the cached factors get messed up // [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries, @@ -401,14 +378,14 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe gttoc(linearize); gttic(eliminate); - ISAM2BayesTree bayesTree = *ISAM2JunctionTree(GaussianEliminationTree(linearized, variableIndex_, order)) + ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree(linearized, variableIndex_, order)) .eliminate(params_.getEliminationFunction()).first; gttoc(eliminate); gttic(insert); this->clear(); - BOOST_FOREACH(const sharedNode& root, bayesTree.roots()) - this->insertRoot(root); + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(insert); result.variablesReeliminated = affectedKeysSet->size(); @@ -469,6 +446,13 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe factors.push_back(cachedBoundary); gttoc(cached); + gttic(orphans); + // Add the orphaned subtrees + BOOST_FOREACH(const sharedClique& orphan, orphans) + factors += boost::make_shared >(orphan); + gttoc(orphans); + + // END OF COPIED CODE // 3. Re-order and eliminate the factor graph into a Bayes net (Algorithm [alg:eliminate]), and re-assemble into a new Bayes tree (Algorithm [alg:BayesTree]) @@ -482,38 +466,41 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe affectedKeysSet->insert(affectedKeys.begin(), affectedKeys.end()); gttoc(list_to_set); - gttic(PartialSolve); - Impl::ReorderingMode reorderingMode; - reorderingMode.nFullSystemVars = variableIndex_.size(); - reorderingMode.algorithm = Impl::ReorderingMode::COLAMD; - reorderingMode.constrain = Impl::ReorderingMode::CONSTRAIN_LAST; + VariableIndex affectedFactorsVarIndex(factors); + + gttic(ordering_constraints); + // Create ordering constraints + FastMap constraintGroups; if(constrainKeys) { - reorderingMode.constrainedKeys = *constrainKeys; + constraintGroups = *constrainKeys; } else { - reorderingMode.constrainedKeys = FastMap(); + constraintGroups = FastMap(); + const int group = observedKeys.size() < affectedFactorsVarIndex.size() + ? 1 : 0; BOOST_FOREACH(Key var, observedKeys) - reorderingMode.constrainedKeys->insert(make_pair(var, 1)); + constraintGroups.insert(make_pair(var, group)); } - FastSet affectedUsedKeys = *affectedKeysSet; // Remove unused keys from the set we pass to PartialSolve - BOOST_FOREACH(Key unused, unusedIndices) - affectedUsedKeys.erase(unused); + // Remove unaffected keys from the constraints - FastMap::iterator iter = reorderingMode.constrainedKeys->begin(); - while(iter != reorderingMode.constrainedKeys->end()) - if(affectedUsedKeys.find(iter->first) == affectedUsedKeys.end()) - reorderingMode.constrainedKeys->erase(iter++); - else - ++iter; - Impl::PartialSolveResult partialSolveResult = - Impl::PartialSolve(factors, affectedUsedKeys, reorderingMode, (params_.factorization == ISAM2Params::QR)); + for(FastMap::iterator iter = constraintGroups.begin(); iter != constraintGroups.end(); ++iter) { + if(unusedIndices.exists(iter->first) || !affectedKeysSet->exists(iter->first)) + constraintGroups.erase(iter); + } + gttoc(ordering_constraints); + + // Generate ordering + gttic(Ordering); + Ordering ordering = Ordering::COLAMDConstrained(affectedFactorsVarIndex, constraintGroups); + + ISAM2BayesTree::shared_ptr bayesTree = ISAM2JunctionTree(GaussianEliminationTree( + factors, affectedFactorsVarIndex, ordering)).eliminate(params_.getEliminationFunction()).first; gttoc(PartialSolve); gttoc(reorder_and_eliminate); gttic(reassemble); - if(partialSolveResult.bayesTree) - BOOST_FOREACH(const sharedNode& root, *partialSolveResult.bayesTree.roots()) - this->insertRoot(root); + this->roots_.insert(this->roots_.end(), bayesTree->roots().begin(), bayesTree->roots().end()); + this->nodes_.insert(bayesTree->nodes().begin(), bayesTree->nodes().end()); gttoc(reassemble); // 4. The orphans have already been inserted during elimination @@ -524,7 +511,7 @@ boost::shared_ptr > ISAM2::recalculate(const FastSet& markedKe // Root clique variables for detailed results if(params_.enableDetailedResults) BOOST_FOREACH(const sharedNode& root, this->roots()) - BOOST_FOREACH(Key var, *root) + BOOST_FOREACH(Key var, *root->conditional()) result.detail->variableStatus[var].inRootClique = true; return affectedKeysSet; @@ -585,7 +572,7 @@ ISAM2Result ISAM2::update( } // Remove removed factors from the variable index so we do not attempt to relinearize them - variableIndex_.remove(removeFactorIndices, removeFactors); + variableIndex_.remove(removeFactorIndices.begin(), removeFactorIndices.end(), removeFactors); // Compute unused keys and indices FastSet unusedKeys; @@ -611,7 +598,7 @@ ISAM2Result ISAM2::update( gttic(add_new_variables); // 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}. - Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_, deltaReplacedMask_); + Impl::AddVariables(newTheta, theta_, delta_, deltaNewton_, RgProd_); // New keys for detailed results if(params_.enableDetailedResults) { BOOST_FOREACH(Key key, newTheta.keys()) { result.detail->variableStatus[key].isNew = true; } } @@ -743,9 +730,8 @@ ISAM2Result ISAM2::update( replacedKeys = recalculate(markedKeys, relinKeys, observedKeys, unusedIndices, constrainedKeys, result); // Update replaced keys mask (accumulates until back-substitution takes place) - if(replacedKeys) { - BOOST_FOREACH(const Key var, *replacedKeys) { - deltaReplacedMask_[var] = true; } } + if(replacedKeys) + deltaReplacedMask_.insert(replacedKeys->begin(), replacedKeys->end()); gttoc(recalculate); // After the top of the tree has been redone and may have index gaps from @@ -753,8 +739,8 @@ ISAM2Result ISAM2::update( // in all data structures. if(!unusedKeys.empty()) { gttic(remove_variables); - Impl::RemoveVariables(unusedKeys, root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, Base::nodes_, linearFactors_, fixedVariables_); + Impl::RemoveVariables(unusedKeys, roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, Base::nodes_, fixedVariables_); gttoc(remove_variables); } result.cliques = this->nodes().size(); @@ -890,7 +876,8 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList) graph3.push_back(eliminationResult2.second); GaussianFactorGraph::EliminationResult eliminationResult3 = params_.getEliminationFunction()(graph3, Ordering(jPosition, clique->conditional()->endFrontals())); - sharedClique newClique = boost::make_shared(make_pair(eliminationResult3.first, clique->cachedFactor())); + sharedClique newClique = boost::make_shared(); + newClique->setEliminationResult(make_pair(eliminationResult3.first, clique->cachedFactor())); // Add the marginalized clique to the BayesTree this->addClique(newClique, parent); @@ -911,32 +898,32 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList) if(clique_factor.second) factorsToAdd.push_back(clique_factor.second); nonlinearFactors_.push_back(boost::make_shared( - clique_factor.second, ordering_)); + clique_factor.second)); if(params_.cacheLinearizedFactors) linearFactors_.push_back(clique_factor.second); - BOOST_FOREACH(Index factorIndex, *clique_factor.second) { - fixedVariables_.insert(ordering_.key(factorIndex)); } + BOOST_FOREACH(Key factorKey, *clique_factor.second) { + fixedVariables_.insert(factorKey); } } variableIndex_.augment(factorsToAdd); // Augment the variable index // Remove the factors to remove that have been summarized in the newly-added marginal factors FastSet factorIndicesToRemove; - BOOST_FOREACH(Index j, indices) { + BOOST_FOREACH(Key j, leafKeys) { factorIndicesToRemove.insert(variableIndex_[j].begin(), variableIndex_[j].end()); } vector removedFactorIndices; - SymbolicFactorGraph removedFactors; + NonlinearFactorGraph removedFactors; BOOST_FOREACH(size_t i, factorIndicesToRemove) { removedFactorIndices.push_back(i); - removedFactors.push_back(nonlinearFactors_[i]->symbolic(ordering_)); + removedFactors.push_back(nonlinearFactors_[i]); nonlinearFactors_.remove(i); if(params_.cacheLinearizedFactors) linearFactors_.remove(i); } - variableIndex_.remove(removedFactorIndices, removedFactors); + variableIndex_.remove(removedFactorIndices.begin(), removedFactorIndices.end(), removedFactors); // Remove the marginalized variables - Impl::RemoveVariables(FastSet(leafKeys.begin(), leafKeys.end()), root_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, - deltaReplacedMask_, ordering_, nodes_, linearFactors_, fixedVariables_); + Impl::RemoveVariables(FastSet(leafKeys.begin(), leafKeys.end()), roots_, theta_, variableIndex_, delta_, deltaNewton_, RgProd_, + deltaReplacedMask_, nodes_, fixedVariables_); } /* ************************************************************************* */ @@ -948,7 +935,7 @@ void ISAM2::updateDelta(bool forceFullSolve) const { boost::get(params_.optimizationParams); const double effectiveWildfireThreshold = forceFullSolve ? 0.0 : gaussNewtonParams.wildfireThreshold; gttic(Wildfire_update); - lastBacksubVariableCount = Impl::UpdateDelta(this->root(), deltaReplacedMask_, delta_, effectiveWildfireThreshold); + lastBacksubVariableCount = Impl::UpdateDelta(roots_, deltaReplacedMask_, delta_, effectiveWildfireThreshold); gttoc(Wildfire_update); } else if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) { @@ -958,8 +945,11 @@ void ISAM2::updateDelta(bool forceFullSolve) const { // Do one Dogleg iteration gttic(Dogleg_Iterate); + VectorValues dx_u = gtsam::optimizeGradientSearch(*this); + VectorValues dx_n = gtsam::optimize(*this); DoglegOptimizerImpl::IterationResult doglegResult(DoglegOptimizerImpl::Iterate( - *doglegDelta_, doglegParams.adaptationMode, *this, nonlinearFactors_, theta_, ordering_, nonlinearFactors_.error(theta_), doglegParams.verbose)); + *doglegDelta_, doglegParams.adaptationMode, dx_u, dx_n, *this, nonlinearFactors_, + theta_, nonlinearFactors_.error(theta_), doglegParams.verbose)); gttoc(Dogleg_Iterate); gttic(Copy_dx_d); @@ -974,8 +964,6 @@ void ISAM2::updateDelta(bool forceFullSolve) const { /* ************************************************************************* */ Values ISAM2::calculateEstimate() const { - // We use ExpmapMasked here instead of regular expmap because the former - // handles Permuted gttic(Copy_Values); Values ret(theta_); gttoc(Copy_Values); @@ -983,31 +971,25 @@ Values ISAM2::calculateEstimate() const { const VectorValues& delta(getDelta()); gttoc(getDelta); gttic(Expmap); - vector mask(ordering_.size(), true); - Impl::ExpmapMasked(ret, delta, ordering_, mask); + ret = ret.retract(delta); gttoc(Expmap); return ret; } /* ************************************************************************* */ const Value& ISAM2::calculateEstimate(Key key) const { - const Index index = getOrdering()[key]; - const Vector& delta = getDelta()[index]; + const Vector& delta = getDelta()[key]; return *theta_.at(key).retract_(delta); } /* ************************************************************************* */ Values ISAM2::calculateBestEstimate() const { - VectorValues delta(theta_.dims(ordering_)); - internal::optimizeInPlace(this->root(), delta); - return theta_.retract(delta, ordering_); + return theta_.retract(internal::linearAlgorithms::optimizeBayesTree(*this)); } /* ************************************************************************* */ -Matrix ISAM2::marginalCovariance(Index key) const { - return marginalFactor(ordering_[key], - params_.factorization == ISAM2Params::QR ? EliminateQR : EliminatePreferCholesky) - ->information().inverse(); +Matrix ISAM2::marginalCovariance(Key key) const { + return marginalFactor(key, params_.getEliminationFunction())->information().inverse(); } /* ************************************************************************* */ @@ -1018,11 +1000,14 @@ const VectorValues& ISAM2::getDelta() const { } /* ************************************************************************* */ +double ISAM2::error(const VectorValues& x) const +{ + return GaussianFactorGraph(*this).error(x); +} +/* ************************************************************************* */ VectorValues optimize(const ISAM2& isam) { - gttic(allocateVectorValues); - VectorValues delta = *allocateVectorValues(isam); - gttoc(allocateVectorValues); + VectorValues delta; optimizeInPlace(isam, delta); return delta; } @@ -1052,12 +1037,8 @@ void optimizeInPlace(const ISAM2& isam, VectorValues& delta) { /* ************************************************************************* */ VectorValues optimizeGradientSearch(const ISAM2& isam) { - gttic(Allocate_VectorValues); - VectorValues grad = *allocateVectorValues(isam); - gttoc(Allocate_VectorValues); - + VectorValues grad; optimizeGradientSearchInPlace(isam, grad); - return grad; } @@ -1086,35 +1067,38 @@ void optimizeGradientSearchInPlace(const ISAM2& isam, VectorValues& grad) { gttic(Compute_minimizing_step_size); // Compute minimizing step size - double RgNormSq = isam.RgProd_.asVector().squaredNorm(); + double RgNormSq = isam.RgProd_.vector().squaredNorm(); double step = -gradientSqNorm / RgNormSq; gttoc(Compute_minimizing_step_size); gttic(Compute_point); // Compute steepest descent point - scal(step, grad); + grad *= step; gttoc(Compute_point); } /* ************************************************************************* */ VectorValues gradient(const ISAM2& bayesTree, const VectorValues& x0) { - return gradient(FactorGraph(bayesTree), x0); + return GaussianFactorGraph(bayesTree).gradient(x0); } /* ************************************************************************* */ static void gradientAtZeroTreeAdder(const boost::shared_ptr& root, VectorValues& g) { // Loop through variables in each clique, adding contributions - int variablePosition = 0; + DenseIndex variablePosition = 0; for(GaussianConditional::const_iterator jit = root->conditional()->begin(); jit != root->conditional()->end(); ++jit) { - const int dim = root->conditional()->dim(jit); - g[*jit] += root->gradientContribution().segment(variablePosition, dim); + const DenseIndex dim = root->conditional()->getDim(jit); + pair pos_ins = + g.tryInsert(*jit, root->gradientContribution().segment(variablePosition, dim)); + if(!pos_ins.second) + pos_ins.first->second += root->gradientContribution().segment(variablePosition, dim); variablePosition += dim; } // Recursively add contributions from children typedef boost::shared_ptr sharedClique; - BOOST_FOREACH(const sharedClique& child, root->children()) { + BOOST_FOREACH(const sharedClique& child, root->children) { gradientAtZeroTreeAdder(child, g); } } @@ -1125,8 +1109,8 @@ void gradientAtZero(const ISAM2& bayesTree, VectorValues& g) { g.setZero(); // Sum up contributions for each clique - if(bayesTree.root()) - gradientAtZeroTreeAdder(bayesTree.root(), g); + BOOST_FOREACH(const ISAM2::sharedClique& root, bayesTree.roots()) + gradientAtZeroTreeAdder(root, g); } } diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 05bdf3d7a..9de4ccf7f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -362,7 +362,7 @@ public: Vector gradientContribution_; /// Overridden to also store the remaining factor and gradient contribution - void setEliminationResult(const typename FactorGraphType::EliminationResult& eliminationResult); + void setEliminationResult(const FactorGraphType::EliminationResult& eliminationResult); /** Access the cached factor */ Base::FactorType::shared_ptr& cachedFactor() { return cachedFactor_; } @@ -397,7 +397,7 @@ private: * estimate of all variables. * */ -class ISAM2: public BayesTree { +class GTSAM_EXPORT ISAM2: public BayesTree { protected: @@ -440,7 +440,7 @@ protected: * This is \c mutable because it is used internally to not update delta_ * until it is needed. */ - mutable FastMap deltaReplacedMask_; // TODO: Make sure accessed in the right way + mutable FastSet deltaReplacedMask_; // TODO: Make sure accessed in the right way /** All original nonlinear factors are stored here to use during relinearization */ NonlinearFactorGraph nonlinearFactors_; @@ -462,23 +462,19 @@ public: typedef ISAM2 This; ///< This class typedef BayesTree Base; ///< The BayesTree base class - - /** Create an empty ISAM2 instance */ - GTSAM_EXPORT ISAM2(const ISAM2Params& params); - - /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ - GTSAM_EXPORT ISAM2(); - - /** Copy constructor */ - GTSAM_EXPORT ISAM2(const ISAM2& other); - - /** Assignment operator */ - GTSAM_EXPORT ISAM2& operator=(const ISAM2& rhs); - typedef Base::Clique Clique; ///< A clique typedef Base::sharedClique sharedClique; ///< Shared pointer to a clique typedef Base::Cliques Cliques; ///< List of Clique typedef from base class + /** Create an empty ISAM2 instance */ + ISAM2(const ISAM2Params& params); + + /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ + ISAM2(); + + /** Compare equality */ + bool equals(const ISAM2& other, double tol = 1e-9) const; + /** * Add new factors, updating the solution and relinearizing as needed. * @@ -507,7 +503,7 @@ public: * of the size of the linear delta. This allows the provided keys to be reordered. * @return An ISAM2Result struct containing information about the update */ - GTSAM_EXPORT ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), + ISAM2Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), const Values& newTheta = Values(), const FastVector& removeFactorIndices = FastVector(), const boost::optional >& constrainedKeys = boost::none, const boost::optional >& noRelinKeys = boost::none, @@ -522,16 +518,24 @@ public: * fixed variables will include any involved with the marginalized variables * in the original factors, and possibly additional ones due to fill-in. */ - GTSAM_EXPORT void marginalizeLeaves(const FastList& leafKeys); + void marginalizeLeaves(const FastList& leafKeys); /** Access the current linearization point */ const Values& getLinearizationPoint() const { return theta_; } + /// Compute the current solution. This is the "standard" function for computing the solution that + /// uses: + /// - Partial relinearization and backsubstitution using the thresholds provided in ISAM2Params. + /// - Dogleg trust-region step, if enabled in ISAM2Params. + /// - Equivalent to getLinearizationPoint().retract(getDelta()) + /// The solution returned is in general not the same as that returned by getLinearizationPoint(). + Values optimize() const; + /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ - GTSAM_EXPORT Values calculateEstimate() const; + Values calculateEstimate() const; /** Compute an estimate for a single variable using its incomplete linear delta computed * during the last update. This is faster than calling the no-argument version of @@ -549,10 +553,10 @@ public: * @param key * @return */ - GTSAM_EXPORT const Value& calculateEstimate(Key key) const; + const Value& calculateEstimate(Key key) const; /** Return marginal on any variable as a covariance matrix */ - GTSAM_EXPORT Matrix marginalCovariance(Index key) const; + Matrix marginalCovariance(Index key) const; /// @name Public members for non-typical usage /// @{ @@ -562,16 +566,19 @@ public: /** Compute an estimate using a complete delta computed by a full back-substitution. */ - GTSAM_EXPORT Values calculateBestEstimate() const; + Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ - GTSAM_EXPORT const VectorValues& getDelta() const; + const VectorValues& getDelta() const; + + /** Compute the linear error */ + double error(const VectorValues& x) const; /** Access the set of nonlinear factors */ - GTSAM_EXPORT const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } + const NonlinearFactorGraph& getFactorsUnsafe() const { return nonlinearFactors_; } /** Access the nonlinear variable index */ - GTSAM_EXPORT const VariableIndex& getVariableIndex() const { return variableIndex_; } + const VariableIndex& getVariableIndex() const { return variableIndex_; } size_t lastAffectedVariableCount; size_t lastAffectedFactorCount; @@ -580,26 +587,26 @@ public: mutable size_t lastBacksubVariableCount; size_t lastNnzTop; - GTSAM_EXPORT const ISAM2Params& params() const { return params_; } + const ISAM2Params& params() const { return params_; } /** prints out clique statistics */ - GTSAM_EXPORT void printStats() const { getCliqueData().getStats().print(); } + void printStats() const { getCliqueData().getStats().print(); } /// @} private: - 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); + FastList getAffectedFactors(const FastList& keys) const; + GaussianFactorGraph::shared_ptr relinearizeAffectedFactors(const FastList& affectedKeys, const FastSet& relinKeys) const; + GaussianFactorGraph getCachedBoundaryFactors(Cliques& orphans); - GTSAM_EXPORT boost::shared_ptr > recalculate(const FastSet& markedKeys, const FastSet& relinKeys, + 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; + void updateDelta(bool forceFullSolve = false) const; - GTSAM_EXPORT friend void optimizeInPlace(const ISAM2&, VectorValues&); - GTSAM_EXPORT friend void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&); + friend GTSAM_EXPORT void optimizeInPlace(const ISAM2&, VectorValues&); + friend GTSAM_EXPORT void optimizeGradientSearchInPlace(const ISAM2&, VectorValues&); }; // ISAM2 @@ -621,12 +628,12 @@ GTSAM_EXPORT void optimizeInPlace(const ISAM2& isam, VectorValues& delta); /// variables are contained in the subtree. /// @return The number of variables that were solved for template -int optimizeWildfire(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, VectorValues& delta); +size_t optimizeWildfire(const boost::shared_ptr& root, + double threshold, const FastSet& replaced, VectorValues& delta); template -int optimizeWildfireNonRecursive(const boost::shared_ptr& root, - double threshold, const std::vector& replaced, VectorValues& delta); +size_t optimizeWildfireNonRecursive(const boost::shared_ptr& root, + double threshold, const FastSet& replaced, VectorValues& delta); /** * Optimize along the gradient direction, with a closed-form computation to diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 9a3c84bb8..3c8b5ce71 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -6,11 +6,10 @@ #include -#if 0 - #include #include #include +#include #include #include #include @@ -27,9 +26,10 @@ #include #include -#include // for operator += -#include +#include using namespace boost::assign; +#include +namespace br { using namespace boost::adaptors; using namespace boost::range; } using namespace std; using namespace gtsam; @@ -37,6 +37,8 @@ using boost::shared_ptr; const double tol = 1e-4; +#define TEST TEST_UNSAFE + // SETDEBUG("ISAM2 update", true); // SETDEBUG("ISAM2 update verbose", true); // SETDEBUG("ISAM2 recalculate", true); @@ -48,7 +50,8 @@ SharedDiagonal brNoise = noiseModel::Diagonal::Sigmas(Vector_(2, M_PI/100.0, 0.1 ISAM2 createSlamlikeISAM2( boost::optional init_values = boost::none, boost::optional full_graph = boost::none, - const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)) { + const ISAM2Params& params = ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true), + size_t maxPoses = 10) { // These variables will be reused and accumulate factors and values ISAM2 isam(params); @@ -61,7 +64,7 @@ ISAM2 createSlamlikeISAM2( // Add a prior at time 0 and update isam { NonlinearFactorGraph newfactors; - newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); + newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -71,10 +74,13 @@ ISAM2 createSlamlikeISAM2( isam.update(newfactors, init); } + if(i > maxPoses) + goto done; + // Add odometry from time 0 to time 5 for( ; i<5; ++i) { NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -82,14 +88,17 @@ ISAM2 createSlamlikeISAM2( fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); isam.update(newfactors, init); + + if(i > maxPoses) + goto done; } // Add odometry from time 5 to 6 and landmark measurement at time 5 { NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); - newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); Values init; @@ -104,10 +113,13 @@ ISAM2 createSlamlikeISAM2( ++ i; } + if(i > maxPoses) + goto done; + // Add odometry from time 6 to time 10 for( ; i<10; ++i) { NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -115,14 +127,17 @@ ISAM2 createSlamlikeISAM2( fullinit.insert((i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); isam.update(newfactors, init); + + if(i > maxPoses) + goto done; } // Add odometry from time 10 to 11 and landmark measurement at time 10 { NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); Values init; @@ -133,6 +148,8 @@ ISAM2 createSlamlikeISAM2( ++ i; } +done: + if (full_graph) *full_graph = fullgraph; @@ -142,185 +159,6 @@ ISAM2 createSlamlikeISAM2( return isam; } -/* ************************************************************************* */ -TEST(ISAM2, ImplAddVariables) { - - // Create initial state - Values theta; - theta.insert(0, Pose2(.1, .2, .3)); - theta.insert(100, Point2(.4, .5)); - Values newTheta; - newTheta.insert(1, Pose2(.6, .7, .8)); - - VectorValues delta; - delta.insert(0, Vector_(3, .1, .2, .3)); - delta.insert(1, Vector_(2, .4, .5)); - - VectorValues deltaNewton; - deltaNewton.insert(0, Vector_(3, .1, .2, .3)); - deltaNewton.insert(1, Vector_(2, .4, .5)); - - VectorValues deltaRg; - deltaRg.insert(0, Vector_(3, .1, .2, .3)); - deltaRg.insert(1, Vector_(2, .4, .5)); - - vector replacedKeys(2, false); - - Ordering ordering; ordering += 100, 0; - - // Verify initial state - LONGS_EQUAL(0, ordering[100]); - LONGS_EQUAL(1, ordering[0]); - EXPECT(assert_equal(delta[0], delta[ordering[100]])); - EXPECT(assert_equal(delta[1], delta[ordering[0]])); - - // Create expected state - Values thetaExpected; - thetaExpected.insert(0, Pose2(.1, .2, .3)); - thetaExpected.insert(100, Point2(.4, .5)); - thetaExpected.insert(1, Pose2(.6, .7, .8)); - - VectorValues deltaExpected; - deltaExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaExpected.insert(1, Vector_(2, .4, .5)); - deltaExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - VectorValues deltaNewtonExpected; - deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaNewtonExpected.insert(1, Vector_(2, .4, .5)); - deltaNewtonExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - VectorValues deltaRgExpected; - deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaRgExpected.insert(1, Vector_(2, .4, .5)); - deltaRgExpected.insert(2, Vector_(3, 0.0, 0.0, 0.0)); - - vector replacedKeysExpected(3, false); - - Ordering orderingExpected; orderingExpected += 100, 0, 1; - - // Expand initial state - ISAM2::Impl::AddVariables(newTheta, theta, delta, deltaNewton, deltaRg, replacedKeys, ordering); - - EXPECT(assert_equal(thetaExpected, theta)); - EXPECT(assert_equal(deltaExpected, delta)); - EXPECT(assert_equal(deltaNewtonExpected, deltaNewton)); - EXPECT(assert_equal(deltaRgExpected, deltaRg)); - EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); - EXPECT(assert_equal(orderingExpected, ordering)); -} - -/* ************************************************************************* */ -TEST(ISAM2, ImplRemoveVariables) { - - // Create initial state - Values theta; - theta.insert(0, Pose2(.1, .2, .3)); - theta.insert(1, Pose2(.6, .7, .8)); - theta.insert(100, Point2(.4, .5)); - - SymbolicFactorGraph sfg; - sfg.push_back(boost::make_shared(Index(0), Index(2))); - sfg.push_back(boost::make_shared(Index(0), Index(1))); - VariableIndex variableIndex(sfg); - - VectorValues delta; - delta.insert(0, Vector_(3, .1, .2, .3)); - delta.insert(1, Vector_(3, .4, .5, .6)); - delta.insert(2, Vector_(2, .7, .8)); - - VectorValues deltaNewton; - deltaNewton.insert(0, Vector_(3, .1, .2, .3)); - deltaNewton.insert(1, Vector_(3, .4, .5, .6)); - deltaNewton.insert(2, Vector_(2, .7, .8)); - - VectorValues deltaRg; - deltaRg.insert(0, Vector_(3, .1, .2, .3)); - deltaRg.insert(1, Vector_(3, .4, .5, .6)); - deltaRg.insert(2, Vector_(2, .7, .8)); - - vector replacedKeys(3, false); - replacedKeys[0] = true; - replacedKeys[1] = false; - replacedKeys[2] = true; - - Ordering ordering; ordering += 100, 1, 0; - - ISAM2::Nodes nodes(3); - - // Verify initial state - LONGS_EQUAL(0, ordering[100]); - LONGS_EQUAL(1, ordering[1]); - LONGS_EQUAL(2, ordering[0]); - - // Create expected state - Values thetaExpected; - thetaExpected.insert(0, Pose2(.1, .2, .3)); - thetaExpected.insert(100, Point2(.4, .5)); - - SymbolicFactorGraph sfgRemoved; - sfgRemoved.push_back(boost::make_shared(Index(0), Index(1))); - sfgRemoved.push_back(SymbolicFactorGraph::sharedFactor()); // Add empty factor to keep factor indices consistent - VariableIndex variableIndexExpected(sfgRemoved); - - VectorValues deltaExpected; - deltaExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaExpected.insert(1, Vector_(2, .7, .8)); - - VectorValues deltaNewtonExpected; - deltaNewtonExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaNewtonExpected.insert(1, Vector_(2, .7, .8)); - - VectorValues deltaRgExpected; - deltaRgExpected.insert(0, Vector_(3, .1, .2, .3)); - deltaRgExpected.insert(1, Vector_(2, .7, .8)); - - vector replacedKeysExpected(2, true); - - Ordering orderingExpected; orderingExpected += 100, 0; - - ISAM2::Nodes nodesExpected(2); - - // Reduce initial state - FastSet unusedKeys; - unusedKeys.insert(1); - vector removedFactorsI; removedFactorsI.push_back(1); - SymbolicFactorGraph removedFactors; removedFactors.push_back(sfg[1]); - variableIndex.remove(removedFactorsI, removedFactors); - GaussianFactorGraph linearFactors; - FastSet fixedVariables; - ISAM2::Impl::RemoveVariables(unusedKeys, ISAM2::sharedClique(), theta, variableIndex, delta, deltaNewton, deltaRg, - replacedKeys, ordering, nodes, linearFactors, fixedVariables); - - EXPECT(assert_equal(thetaExpected, theta)); - EXPECT(assert_equal(variableIndexExpected, variableIndex)); - EXPECT(assert_equal(deltaExpected, delta)); - EXPECT(assert_equal(deltaNewtonExpected, deltaNewton)); - EXPECT(assert_equal(deltaRgExpected, deltaRg)); - EXPECT(assert_container_equality(replacedKeysExpected, replacedKeys)); - EXPECT(assert_equal(orderingExpected, ordering)); -} - -/* ************************************************************************* */ -//TEST(ISAM2, IndicesFromFactors) { -// -// using namespace gtsam::planarSLAM; -// typedef GaussianISAM2::Impl Impl; -// -// Ordering ordering; ordering += (0), (0), (1); -// NonlinearFactorGraph graph; -// graph.add(PriorFactor((0), Pose2(), noiseModel::Unit::Create(Pose2::dimension)); -// graph.addRange((0), (0), 1.0, noiseModel::Unit::Create(1)); -// -// FastSet expected; -// expected.insert(0); -// expected.insert(1); -// -// FastSet actual = Impl::IndicesFromFactors(ordering, graph); -// -// EXPECT(assert_equal(expected, actual)); -//} - /* ************************************************************************* */ //TEST(ISAM2, CheckRelinearization) { // @@ -355,37 +193,29 @@ TEST(ISAM2, ImplRemoveVariables) { //} /* ************************************************************************* */ -TEST(ISAM2, optimize2) { - - // Create initialization - Values theta; - theta.insert((0), Pose2(0.01, 0.01, 0.01)); - - // Create conditional - Vector d(3); d << -0.1, -0.1, -0.31831; - Matrix R(3,3); R << - 10, 0.0, 0.0, - 0.0, 10, 0.0, - 0.0, 0.0, 31.8309886; - GaussianConditional::shared_ptr conditional(new GaussianConditional(0, d, R, Vector::Ones(3))); - - // Create ordering - Ordering ordering; ordering += (0); - - // Expected vector - VectorValues expected(1, 3); - conditional->solveInPlace(expected); - - // Clique - ISAM2::sharedClique clique( - ISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); - VectorValues actual(theta.dims(ordering)); - internal::optimizeInPlace(clique, actual); - -// expected.print("expected: "); -// actual.print("actual: "); - EXPECT(assert_equal(expected, actual)); -} +struct ConsistencyVisitor +{ + bool consistent; + const ISAM2& isam; + ConsistencyVisitor(const ISAM2& isam) : + consistent(true), isam(isam) {} + int operator()(const ISAM2::sharedClique& node, int& parentData) + { + if(std::find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end()) + { + if(node->parent_.expired()) + consistent = false; + if(std::find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end()) + consistent = false; + } + BOOST_FOREACH(Key j, node->conditional()->frontals()) + { + if(isam.nodes().at(j).get() != node.get()) + consistent = false; + } + return 0; + } +}; /* ************************************************************************* */ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { @@ -394,31 +224,35 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c const std::string name_ = test.getName(); Values actual = isam.calculateEstimate(); - Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; - GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); -// linearized.print("Expected linearized: "); - GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); -// gbn.print("Expected bayesnet: "); - VectorValues delta = optimize(gbn); - Values expected = fullinit.retract(delta, ordering); + Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize()); bool isamEqual = assert_equal(expected, actual); + // Check information + ISAM2 expectedisam(isam.params()); + expectedisam.update(fullgraph, fullinit); + bool isamTreeEqual = assert_equal(GaussianFactorGraph(expectedisam).augmentedHessian(), GaussianFactorGraph(isam).augmentedHessian()); + + // Check consistency + ConsistencyVisitor visitor(isam); + int data; // Unused + treeTraversal::DepthFirstForest(isam, data, visitor); + bool consistent = visitor.consistent; + // The following two checks make sure that the cached gradients are maintained and used correctly // Check gradient at each node bool nodeGradientsOk = true; typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { + BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); + GaussianFactorGraph jfg; + jfg += clique->conditional(); + VectorValues expectedGradient = jfg.gradientAtZero(); // Compare with actual gradients - int variablePosition = 0; + DenseIndex variablePosition = 0; for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); + const DenseIndex dim = clique->conditional()->getDim(jit); Vector actual = clique->gradientContribution().segment(variablePosition, dim); bool gradOk = assert_equal(expectedGradient[*jit], actual); EXPECT(gradOk); @@ -431,17 +265,30 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c } // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); + VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); + VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); + VectorValues actualGradient; gradientAtZero(isam, actualGradient); bool expectedGradOk = assert_equal(expectedGradient2, expectedGradient); EXPECT(expectedGradOk); bool totalGradOk = assert_equal(expectedGradient, actualGradient); EXPECT(totalGradOk); - return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual; + return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent; +} + +/* ************************************************************************* */ +TEST(ISAM2, simple) +{ + for(size_t i = 0; i < 10; ++i) { + // These variables will be reused and accumulate factors and values + Values fullinit; + NonlinearFactorGraph fullgraph; + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.0), 0.0, 0, false), i); + + // Compare solutions + CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); + } } /* ************************************************************************* */ @@ -505,8 +352,8 @@ TEST(ISAM2, clone) { // Modify original isam NonlinearFactorGraph factors; - factors.add(BetweenFactor(0, 10, - isam.calculateEstimate(0).between(isam.calculateEstimate(10)), noiseModel::Unit::Create(3))); + factors += BetweenFactor(0, 10, + isam.calculateEstimate(0).between(isam.calculateEstimate(10)), noiseModel::Unit::Create(3)); isam.update(factors); CHECK(assert_equal(createSlamlikeISAM2(), clone2)); @@ -526,66 +373,6 @@ TEST(ISAM2, clone) { CHECK(assert_equal(ISAM2(), clone1)); } -/* ************************************************************************* */ -TEST(ISAM2, permute_cached) { - typedef boost::shared_ptr sharedISAM2Clique; - - // Construct expected permuted BayesTree (variable 2 has been changed to 1) - BayesTree expected; - expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (3, Matrix_(1,1,1.0)) - (4, Matrix_(1,1,2.0)), - 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) - HessianFactor::shared_ptr())))); // Cached: empty - expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (2, Matrix_(1,1,1.0)) - (3, Matrix_(1,1,2.0)), - 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) - boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) - expected.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (0, Matrix_(1,1,1.0)) - (2, Matrix_(1,1,2.0)), - 1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2) - boost::make_shared(1, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(1) - // Change variable 2 to 1 - expected.root()->children().front()->conditional()->keys()[0] = 1; - expected.root()->children().front()->children().front()->conditional()->keys()[1] = 1; - - // Construct unpermuted BayesTree - BayesTree actual; - actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (3, Matrix_(1,1,1.0)) - (4, Matrix_(1,1,2.0)), - 2, Vector_(1,1.0), Vector_(1,1.0)), // p(3,4) - HessianFactor::shared_ptr())))); // Cached: empty - actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (2, Matrix_(1,1,1.0)) - (3, Matrix_(1,1,2.0)), - 1, Vector_(1,1.0), Vector_(1,1.0)), // p(2|3) - boost::make_shared(3, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(3) - actual.insert(sharedISAM2Clique(new ISAM2Clique(make_pair( - boost::make_shared(pair_list_of - (0, Matrix_(1,1,1.0)) - (2, Matrix_(1,1,2.0)), - 1, Vector_(1,1.0), Vector_(1,1.0)), // p(0|2) - boost::make_shared(2, Matrix_(1,1,1.0), Vector_(1,1.0), 0.0))))); // Cached: p(2) - - // Create permutation that changes variable 2 -> 0 - Permutation permutation = Permutation::Identity(5); - permutation[2] = 1; - - // Permute BayesTree - actual.root()->permuteWithInverse(permutation); - - // Check - EXPECT(assert_equal(expected, actual)); -} - /* ************************************************************************* */ TEST(ISAM2, removeFactors) { @@ -650,8 +437,8 @@ TEST(ISAM2, swapFactors) fullgraph.remove(swap_idx); NonlinearFactorGraph swapfactors; -// swapfactors.add(BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); // original factor - swapfactors.add(BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise)); +// swapfactors += BearingRange(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise; // original factor + swapfactors += BearingRangeFactor(10, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 5.0, brNoise); fullgraph.push_back(swapfactors); isam.update(swapfactors, Values(), toRemove); } @@ -662,28 +449,26 @@ TEST(ISAM2, swapFactors) // Check gradient at each node typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { + BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); + GaussianFactorGraph jfg; + jfg += clique->conditional(); + VectorValues expectedGradient = jfg.gradientAtZero(); // Compare with actual gradients - int variablePosition = 0; + DenseIndex variablePosition = 0; for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); + const DenseIndex dim = clique->conditional()->getDim(jit); Vector actual = clique->gradientContribution().segment(variablePosition, dim); EXPECT(assert_equal(expectedGradient[*jit], actual)); variablePosition += dim; } - EXPECT_LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); + EXPECT_LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition); } // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); + VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); + VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); + VectorValues actualGradient; gradientAtZero(isam, actualGradient); EXPECT(assert_equal(expectedGradient2, expectedGradient)); EXPECT(assert_equal(expectedGradient, actualGradient)); @@ -708,7 +493,7 @@ TEST(ISAM2, constrained_ordering) // Add a prior at time 0 and update isam { NonlinearFactorGraph newfactors; - newfactors.add(PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise)); + newfactors += PriorFactor(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -723,7 +508,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 0 to time 5 for( ; i<5; ++i) { NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -739,9 +524,9 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 5 to 6 and landmark measurement at time 5 { NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise)); - newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise)); + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0), 5.0, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); Values init; @@ -759,7 +544,7 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 6 to time 10 for( ; i<10; ++i) { NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); Values init; @@ -772,9 +557,9 @@ TEST(ISAM2, constrained_ordering) // Add odometry from time 10 to 11 and landmark measurement at time 10 { NonlinearFactorGraph newfactors; - newfactors.add(BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise)); - newfactors.add(BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); - newfactors.add(BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise)); + newfactors += BetweenFactor(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); + newfactors += BearingRangeFactor(i, 100, Rot2::fromAngle(M_PI/4.0 + M_PI/16.0), 4.5, brNoise); + newfactors += BearingRangeFactor(i, 101, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); Values init; @@ -788,33 +573,28 @@ TEST(ISAM2, constrained_ordering) // Compare solutions EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); - // Check that x3 and x4 are last, but either can come before the other - EXPECT(isam.getOrdering()[(3)] == 12 && isam.getOrdering()[(4)] == 13); - // Check gradient at each node typedef ISAM2::sharedClique sharedClique; - BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { + BOOST_FOREACH(const sharedClique& clique, isam.nodes() | br::map_values) { // Compute expected gradient - FactorGraph jfg; - jfg.push_back(JacobianFactor::shared_ptr(new JacobianFactor(*clique->conditional()))); - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(jfg, expectedGradient); + GaussianFactorGraph jfg; + jfg += clique->conditional(); + VectorValues expectedGradient = jfg.gradientAtZero(); // Compare with actual gradients - int variablePosition = 0; + DenseIndex variablePosition = 0; for(GaussianConditional::const_iterator jit = clique->conditional()->begin(); jit != clique->conditional()->end(); ++jit) { - const int dim = clique->conditional()->dim(jit); + const DenseIndex dim = clique->conditional()->getDim(jit); Vector actual = clique->gradientContribution().segment(variablePosition, dim); EXPECT(assert_equal(expectedGradient[*jit], actual)); variablePosition += dim; } - LONGS_EQUAL(clique->gradientContribution().rows(), variablePosition); + LONGS_EQUAL((long)clique->gradientContribution().rows(), (long)variablePosition); } // Check gradient - VectorValues expectedGradient(*allocateVectorValues(isam)); - gradientAtZero(FactorGraph(isam), expectedGradient); - VectorValues expectedGradient2(gradient(FactorGraph(isam), VectorValues::Zero(expectedGradient))); - VectorValues actualGradient(*allocateVectorValues(isam)); + VectorValues expectedGradient = GaussianFactorGraph(isam).gradientAtZero(); + VectorValues expectedGradient2 = GaussianFactorGraph(isam).gradient(VectorValues::Zero(expectedGradient)); + VectorValues actualGradient; gradientAtZero(isam, actualGradient); EXPECT(assert_equal(expectedGradient2, expectedGradient)); EXPECT(assert_equal(expectedGradient, actualGradient)); @@ -838,16 +618,12 @@ namespace { bool checkMarginalizeLeaves(ISAM2& isam, const FastList& leafKeys) { Matrix expectedAugmentedHessian, expected3AugmentedHessian; vector toKeep; - const Index lastVar = isam.getOrdering().size() - 1; - for(Index i=0; i<=lastVar; ++i) - if(find(leafKeys.begin(), leafKeys.end(), isam.getOrdering().key(i)) == leafKeys.end()) - toKeep.push_back(i); + BOOST_FOREACH(Key j, isam.getDelta() | br::map_keys) + if(find(leafKeys.begin(), leafKeys.end(), j) == leafKeys.end()) + toKeep.push_back(j); // Calculate expected marginal from iSAM2 tree - GaussianFactorGraph isamAsGraph(isam); - GaussianSequentialSolver solver(isamAsGraph); - GaussianFactorGraph marginalgfg = *solver.jointFactorGraph(toKeep); - expectedAugmentedHessian = marginalgfg.augmentedHessian(); + expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep)->augmentedHessian(); //// Calculate expected marginal from cached linear factors //assert(isam.params().cacheLinearizedFactors); @@ -855,10 +631,8 @@ namespace { //expected2AugmentedHessian = solver2.jointFactorGraph(toKeep)->augmentedHessian(); // Calculate expected marginal from original nonlinear factors - GaussianSequentialSolver solver3( - *isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint(), isam.getOrdering()), - isam.params().factorization == ISAM2Params::QR); - expected3AugmentedHessian = solver3.jointFactorGraph(toKeep)->augmentedHessian(); + expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint()) + ->marginal(toKeep)->augmentedHessian(); // Do marginalization isam.marginalizeLeaves(leafKeys); @@ -868,7 +642,7 @@ namespace { Matrix actualAugmentedHessian = actualMarginalGraph.augmentedHessian(); //Matrix actual2AugmentedHessian = linearFactors_.augmentedHessian(); Matrix actual3AugmentedHessian = isam.getFactorsUnsafe().linearize( - isam.getLinearizationPoint(), isam.getOrdering())->augmentedHessian(); + isam.getLinearizationPoint())->augmentedHessian(); assert(actualAugmentedHessian.unaryExpr(std::ptr_fun(&std::isfinite)).all()); // Check full marginalization @@ -893,11 +667,11 @@ TEST(ISAM2, marginalizeLeaves1) ISAM2 isam; NonlinearFactorGraph factors; - factors.add(PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1))); + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); - factors.add(BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1))); + factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); Values values; values.insert(0, LieVector(0.0)); @@ -911,8 +685,7 @@ TEST(ISAM2, marginalizeLeaves1) isam.update(factors, values, FastVector(), constrainedKeys); - FastList leafKeys; - leafKeys.push_back(isam.getOrdering().key(0)); + FastList leafKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -922,12 +695,12 @@ TEST(ISAM2, marginalizeLeaves2) ISAM2 isam; NonlinearFactorGraph factors; - factors.add(PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1))); + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); - factors.add(BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1))); + factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); Values values; values.insert(0, LieVector(0.0)); @@ -943,8 +716,7 @@ TEST(ISAM2, marginalizeLeaves2) isam.update(factors, values, FastVector(), constrainedKeys); - FastList leafKeys; - leafKeys.push_back(isam.getOrdering().key(0)); + FastList leafKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -954,17 +726,17 @@ TEST(ISAM2, marginalizeLeaves3) ISAM2 isam; NonlinearFactorGraph factors; - factors.add(PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1))); + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); - factors.add(BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1))); + factors += BetweenFactor(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); - factors.add(BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1))); + factors += BetweenFactor(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); - factors.add(BetweenFactor(3, 4, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(4, 5, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(3, 5, LieVector(0.0), noiseModel::Unit::Create(1))); + factors += BetweenFactor(3, 4, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(4, 5, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(3, 5, LieVector(0.0), noiseModel::Unit::Create(1)); Values values; values.insert(0, LieVector(0.0)); @@ -984,8 +756,7 @@ TEST(ISAM2, marginalizeLeaves3) isam.update(factors, values, FastVector(), constrainedKeys); - FastList leafKeys; - leafKeys.push_back(isam.getOrdering().key(0)); + FastList leafKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -995,9 +766,9 @@ TEST(ISAM2, marginalizeLeaves4) ISAM2 isam; NonlinearFactorGraph factors; - factors.add(PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1))); - factors.add(BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1))); + factors += PriorFactor(0, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); + factors += BetweenFactor(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); Values values; values.insert(0, LieVector(0.0)); @@ -1011,8 +782,7 @@ TEST(ISAM2, marginalizeLeaves4) isam.update(factors, values, FastVector(), constrainedKeys); - FastList leafKeys; - leafKeys.push_back(isam.getOrdering().key(1)); + FastList leafKeys = list_of(1); EXPECT(checkMarginalizeLeaves(isam, leafKeys)); } @@ -1023,8 +793,7 @@ TEST(ISAM2, marginalizeLeaves5) ISAM2 isam = createSlamlikeISAM2(); // Marginalize - FastList marginalizeKeys; - marginalizeKeys.push_back(isam.getOrdering().key(0)); + FastList marginalizeKeys = list_of(0); EXPECT(checkMarginalizeLeaves(isam, marginalizeKeys)); } @@ -1040,8 +809,6 @@ TEST(ISAM2, marginalCovariance) EXPECT(assert_equal(expected, actual)); } -#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From a056ba2095e6aaabf60230b919a8427bfa142b21 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Fri, 9 Aug 2013 21:52:24 +0000 Subject: [PATCH 12/18] Fixed check for zero-row jacobians in Scatter --- gtsam/linear/HessianFactor.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index f73b2b531..1fa94922b 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -70,7 +70,10 @@ Scatter::Scatter(const GaussianFactorGraph& gfg, boost::optionalbegin(); variable != factor->end(); ++variable) { - this->insert(make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); + // TODO: Fix this hack to cope with zero-row Jacobians that come from BayesTreeOrphanWrappers + const JacobianFactor* asJacobian = dynamic_cast(factor.get()); + if(!asJacobian || asJacobian->rows() > 0) + this->insert(make_pair(*variable, SlotEntry(none, factor->getDim(variable)))); } } } From 67431ba8ad29aba20cc63f309905a09ee11fa407 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 10 Aug 2013 15:51:52 +0000 Subject: [PATCH 13/18] Fix marginalizeLeaves bug --- gtsam/nonlinear/ISAM2.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 7047d7990..82bc67769 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -871,11 +871,11 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList) graph2.push_back(clique->conditional()); GaussianFactorGraph::EliminationResult eliminationResult2 = params_.getEliminationFunction()(graph2, Ordering( - clique->conditional()->beginFrontals(), jPosition)); + clique->conditional()->beginFrontals(), jPosition + 1)); GaussianFactorGraph graph3; graph3.push_back(eliminationResult2.second); GaussianFactorGraph::EliminationResult eliminationResult3 = - params_.getEliminationFunction()(graph3, Ordering(jPosition, clique->conditional()->endFrontals())); + params_.getEliminationFunction()(graph3, Ordering(jPosition + 1, clique->conditional()->endFrontals())); sharedClique newClique = boost::make_shared(); newClique->setEliminationResult(make_pair(eliminationResult3.first, clique->cachedFactor())); From 63e89656d2a20eb22509695b9c33cd1b9766713e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sat, 10 Aug 2013 15:52:00 +0000 Subject: [PATCH 14/18] Temporary debugging for marginalizeLeaves --- gtsam/nonlinear/ISAM2.cpp | 1 + gtsam/nonlinear/ISAM2.h | 2 +- tests/testGaussianISAM2.cpp | 15 +++++++-------- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index 82bc67769..f86378856 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -781,6 +781,7 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList) if(marginalizeEntireClique) { // Remove the whole clique and its subtree, and keep the marginal factor. GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); + marginalFactor->print("marginalFactor: "); // We do not need the marginal factors associated with this clique // because their information is already incorporated in the new // marginal factor. So, now associate this marginal factor with the diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 9de4ccf7f..9b119401a 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -399,7 +399,7 @@ private: */ class GTSAM_EXPORT ISAM2: public BayesTree { -protected: +public: /** The current linearization point */ Values theta_; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 3c8b5ce71..30f8d08bf 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -623,16 +623,15 @@ namespace { toKeep.push_back(j); // Calculate expected marginal from iSAM2 tree - expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep)->augmentedHessian(); + expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep, EliminateQR)->augmentedHessian(); - //// Calculate expected marginal from cached linear factors - //assert(isam.params().cacheLinearizedFactors); - //GaussianSequentialSolver solver2(isam.linearFactors_, isam.params().factorization == ISAM2Params::QR); - //expected2AugmentedHessian = solver2.jointFactorGraph(toKeep)->augmentedHessian(); + // Calculate expected marginal from cached linear factors + assert(isam.params().cacheLinearizedFactors); + Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian(); // Calculate expected marginal from original nonlinear factors expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint()) - ->marginal(toKeep)->augmentedHessian(); + ->marginal(toKeep, EliminateQR)->augmentedHessian(); // Do marginalization isam.marginalizeLeaves(leafKeys); @@ -648,7 +647,7 @@ namespace { // Check full marginalization //cout << "treeEqual" << endl; bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); - //bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); + bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); //cout << "nonlinEqual" << endl; bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6); @@ -656,7 +655,7 @@ namespace { //cout << "nonlinCorrect" << endl; actual3AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); - bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; + bool ok = treeEqual && linEqual && nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; return ok; } } From 211542fc0efd595adcc7049688496ac374e3e9ed Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 11 Aug 2013 00:39:44 +0000 Subject: [PATCH 15/18] Removed print statement --- gtsam/nonlinear/ISAM2.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/nonlinear/ISAM2.cpp b/gtsam/nonlinear/ISAM2.cpp index f86378856..82bc67769 100644 --- a/gtsam/nonlinear/ISAM2.cpp +++ b/gtsam/nonlinear/ISAM2.cpp @@ -781,7 +781,6 @@ void ISAM2::marginalizeLeaves(const FastList& leafKeysList) if(marginalizeEntireClique) { // Remove the whole clique and its subtree, and keep the marginal factor. GaussianFactor::shared_ptr marginalFactor = clique->cachedFactor(); - marginalFactor->print("marginalFactor: "); // We do not need the marginal factors associated with this clique // because their information is already incorporated in the new // marginal factor. So, now associate this marginal factor with the From 3228895c26f4eb57dfd8e27ab8ce7099d1f4c62c Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 11 Aug 2013 00:40:04 +0000 Subject: [PATCH 16/18] Fixed ISAM(2) unit tests --- tests/testGaussianISAM.cpp | 8 +++----- tests/testGaussianISAM2.cpp | 23 ++++++++++++++++------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index c1b5f4ffa..81c68c106 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -53,18 +53,16 @@ TEST( ISAM, iSAM_smoother ) } // Create expected Bayes Tree by solving smoother with "natural" ordering - GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering); - GaussianISAM expected; - expected.insertRoot(bayesTree.roots().front()); + GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering); // Verify sigmas in the bayes tree - BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, bayesTree.nodes() | br::map_values) { + BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, expected.nodes() | br::map_values) { GaussianConditional::shared_ptr conditional = clique->conditional(); EXPECT(!conditional->get_model()); } // Check whether BayesTree is correct - EXPECT(assert_equal(expected, actual)); + EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian())); // obtain solution VectorValues e; // expected solution diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index 30f8d08bf..c0014e765 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -93,6 +93,9 @@ ISAM2 createSlamlikeISAM2( goto done; } + if(i > maxPoses) + goto done; + // Add odometry from time 5 to 6 and landmark measurement at time 5 { NonlinearFactorGraph newfactors; @@ -132,6 +135,9 @@ ISAM2 createSlamlikeISAM2( goto done; } + if(i > maxPoses) + goto done; + // Add odometry from time 10 to 11 and landmark measurement at time 10 { NonlinearFactorGraph newfactors; @@ -229,9 +235,12 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c bool isamEqual = assert_equal(expected, actual); // Check information - ISAM2 expectedisam(isam.params()); - expectedisam.update(fullgraph, fullinit); - bool isamTreeEqual = assert_equal(GaussianFactorGraph(expectedisam).augmentedHessian(), GaussianFactorGraph(isam).augmentedHessian()); + GaussianFactorGraph isamGraph(isam); + isamGraph += isam.roots().front()->cachedFactor_; + Matrix expectedHessian = fullgraph.linearize(isam.getLinearizationPoint())->augmentedHessian(); + Matrix actualHessian = isamGraph.augmentedHessian(); + expectedHessian.bottomRightCorner(1,1) = actualHessian.bottomRightCorner(1,1); + bool isamTreeEqual = assert_equal(expectedHessian, actualHessian); // Check consistency ConsistencyVisitor visitor(isam); @@ -284,10 +293,10 @@ TEST(ISAM2, simple) // These variables will be reused and accumulate factors and values Values fullinit; NonlinearFactorGraph fullgraph; - ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.0), 0.0, 0, false), i); + ISAM2 isam = createSlamlikeISAM2(fullinit, fullgraph, ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false), i); // Compare solutions - CHECK(isam_check(fullgraph, fullinit, isam, *this, result_)); + EXPECT(isam_check(fullgraph, fullinit, isam, *this, result_)); } } @@ -647,13 +656,13 @@ namespace { // Check full marginalization //cout << "treeEqual" << endl; bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); - bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); + actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); //cout << "nonlinEqual" << endl; bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6); //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6); //cout << "nonlinCorrect" << endl; - actual3AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); + bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); bool ok = treeEqual && linEqual && nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; return ok; From 4e2f2c3822f807281e76c8f009da90ebb016cede Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 11 Aug 2013 00:40:12 +0000 Subject: [PATCH 17/18] Changed public back to protected --- gtsam/nonlinear/ISAM2.h | 2 +- tests/testGaussianISAM2.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index 9b119401a..9de4ccf7f 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -399,7 +399,7 @@ private: */ class GTSAM_EXPORT ISAM2: public BayesTree { -public: +protected: /** The current linearization point */ Values theta_; diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index c0014e765..6fd6cba92 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -635,8 +635,8 @@ namespace { expectedAugmentedHessian = GaussianFactorGraph(isam).marginal(toKeep, EliminateQR)->augmentedHessian(); // Calculate expected marginal from cached linear factors - assert(isam.params().cacheLinearizedFactors); - Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian(); + //assert(isam.params().cacheLinearizedFactors); + //Matrix expected2AugmentedHessian = isam.linearFactors_.marginal(toKeep, EliminateQR)->augmentedHessian(); // Calculate expected marginal from original nonlinear factors expected3AugmentedHessian = isam.getFactorsUnsafe().linearize(isam.getLinearizationPoint()) @@ -656,15 +656,15 @@ namespace { // Check full marginalization //cout << "treeEqual" << endl; bool treeEqual = assert_equal(expectedAugmentedHessian, actualAugmentedHessian, 1e-6); - actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); + //actualAugmentedHessian.bottomRightCorner(1,1) = expected2AugmentedHessian.bottomRightCorner(1,1); bool linEqual = assert_equal(expected2AugmentedHessian, actualAugmentedHessian, 1e-6); //cout << "nonlinEqual" << endl; - bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); + actualAugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool nonlinEqual = assert_equal(expected3AugmentedHessian, actualAugmentedHessian, 1e-6); //bool linCorrect = assert_equal(expected3AugmentedHessian, expected2AugmentedHessian, 1e-6); //actual2AugmentedHessian.bottomRightCorner(1,1) = expected3AugmentedHessian.bottomRightCorner(1,1); bool afterLinCorrect = assert_equal(expected3AugmentedHessian, actual2AugmentedHessian, 1e-6); //cout << "nonlinCorrect" << endl; bool afterNonlinCorrect = assert_equal(expected3AugmentedHessian, actual3AugmentedHessian, 1e-6); - bool ok = treeEqual && linEqual && nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; + bool ok = treeEqual && /*linEqual &&*/ nonlinEqual && /*linCorrect &&*/ /*afterLinCorrect &&*/ afterNonlinCorrect; return ok; } } From 094fffeea79c11f425285d3910acc130ca7071eb Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 11 Aug 2013 00:56:50 +0000 Subject: [PATCH 18/18] Fixed compile errors on Linux --- gtsam/base/FastMap.h | 2 +- gtsam/base/FastSet.h | 2 +- gtsam/inference/BayesTree-inst.h | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 656eb74fd..5015a77d8 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -59,7 +59,7 @@ public: } /** Handy 'exists' function */ - bool exists(const KEY& e) const { return find(e) != end(); } + bool exists(const KEY& e) const { return this->find(e) != this->end(); } private: /** Serialization function */ diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index fe4cc2c80..4c8227c22 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -92,7 +92,7 @@ public: } /** Handy 'exists' function */ - bool exists(const VALUE& e) const { return find(e) != end(); } + bool exists(const VALUE& e) const { return this->find(e) != this->end(); } /** Print to implement Testable */ void print(const std::string& str = "") const { FastSetTestableHelper::print(*this, str); } diff --git a/gtsam/inference/BayesTree-inst.h b/gtsam/inference/BayesTree-inst.h index 2e18723c2..4bbed8bf0 100644 --- a/gtsam/inference/BayesTree-inst.h +++ b/gtsam/inference/BayesTree-inst.h @@ -339,7 +339,7 @@ namespace gtsam { // Factor the shortcuts to be conditioned on the full root // Get the set of variables to eliminate, which is C1\B. gttic(Full_root_factoring); - boost::shared_ptr p_C1_B; { + boost::shared_ptr p_C1_B; { std::vector C1_minus_B; { FastSet C1_minus_B_set(C1->conditional()->beginParents(), C1->conditional()->endParents()); BOOST_FOREACH(const Index j, *B->conditional()) { @@ -351,7 +351,7 @@ namespace gtsam { boost::tie(p_C1_B, temp_remaining) = FactorGraphType(p_C1_Bred).eliminatePartialMultifrontal(Ordering(C1_minus_B), function); } - boost::shared_ptr p_C2_B; { + boost::shared_ptr p_C2_B; { std::vector C2_minus_B; { FastSet C2_minus_B_set(C2->conditional()->beginParents(), C2->conditional()->endParents()); BOOST_FOREACH(const Index j, *B->conditional()) {