From 18414b1286e6ed48d6586cf942d6ce98c761c626 Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Mon, 18 Jan 2010 08:05:33 +0000 Subject: [PATCH] VectorConfig/Config confusion resolved; planarSLAM integrated --- cpp/GaussianISAM2.cpp | 27 +++++++++++++++++++++++++-- cpp/GaussianISAM2.h | 12 ++++++++++++ cpp/ISAM2-inl.h | 23 ++++++++++++----------- cpp/ISAM2.h | 10 +++++----- 4 files changed, 54 insertions(+), 18 deletions(-) diff --git a/cpp/GaussianISAM2.cpp b/cpp/GaussianISAM2.cpp index 9cb77c186..951777416 100644 --- a/cpp/GaussianISAM2.cpp +++ b/cpp/GaussianISAM2.cpp @@ -11,7 +11,9 @@ using namespace gtsam; // Explicitly instantiate so we don't have to include everywhere #include "ISAM2-inl.h" + //template class ISAM2; +//template class ISAM2; namespace gtsam { @@ -25,8 +27,6 @@ void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) result.insert(cg->key(), x); // store result in partial solution } BOOST_FOREACH(GaussianISAM2::sharedClique child, clique->children_) { -// list::const_iterator child; -// for (child = clique->children_.begin(); child != clique->children_.end(); child++) { optimize2(child, result); } } @@ -39,4 +39,27 @@ VectorConfig optimize2(const GaussianISAM2& bayesTree) { return result; } +#if 0 +/* ************************************************************************* */ +void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result) { + // parents are assumed to already be solved and available in result + GaussianISAM2_P::Clique::const_reverse_iterator it; + for (it = clique->rbegin(); it!=clique->rend(); it++) { + GaussianConditional::shared_ptr cg = *it; + result.insert(cg->key(), cg->solve(result)); // store result in partial solution + } + BOOST_FOREACH(GaussianISAM2_P::sharedClique child, clique->children_) { + optimize2(child, result); + } +} +#endif + +/* ************************************************************************* */ +VectorConfig optimize2(const GaussianISAM2_P& bayesTree) { + VectorConfig result; + // starting from the root, call optimize on each conditional + optimize2(bayesTree.root(), result); + return result; +} + } /// namespace gtsam diff --git a/cpp/GaussianISAM2.h b/cpp/GaussianISAM2.h index 76ee7a780..9fff986f5 100644 --- a/cpp/GaussianISAM2.h +++ b/cpp/GaussianISAM2.h @@ -11,6 +11,7 @@ #include "ISAM2.h" #include "GaussianConditional.h" #include "GaussianFactor.h" +#include "planarSLAM.h" namespace gtsam { @@ -22,4 +23,15 @@ namespace gtsam { // optimize the BayesTree, starting from the root VectorConfig optimize2(const GaussianISAM2& bayesTree); + + // todo: copy'n'paste to avoid template hell + + typedef ISAM2 GaussianISAM2_P; + + // recursively optimize this conditional and all subtrees +// void optimize2(const GaussianISAM2_P::sharedClique& clique, VectorConfig& result); + + // optimize the BayesTree, starting from the root + VectorConfig optimize2(const GaussianISAM2_P& bayesTree); + }/// namespace gtsam diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index added3f52..6b16f29e9 100644 --- a/cpp/ISAM2-inl.h +++ b/cpp/ISAM2-inl.h @@ -23,7 +23,7 @@ namespace gtsam { using namespace std; // from inference-inl.h - need to additionally return the newly created factor for caching - boost::shared_ptr _eliminateOne(FactorGraph& graph, cachedFactors& cached, const Symbol& key) { + boost::shared_ptr _eliminateOne(FactorGraph& graph, CachedFactors& cached, const Symbol& key) { // combine the factors of all nodes connected to the variable to be eliminated // if no factors are connected to key, returns an empty factor @@ -45,7 +45,7 @@ namespace gtsam { } // from GaussianFactorGraph.cpp, see _eliminateOne above - GaussianBayesNet _eliminate(FactorGraph& graph, cachedFactors& cached, const Ordering& ordering) { + GaussianBayesNet _eliminate(FactorGraph& graph, CachedFactors& cached, const Ordering& ordering) { GaussianBayesNet chordalBayesNet; // empty BOOST_FOREACH(const Symbol& key, ordering) { GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key); @@ -54,7 +54,7 @@ namespace gtsam { return chordalBayesNet; } - GaussianBayesNet _eliminate_const(const FactorGraph& graph, cachedFactors& cached, const Ordering& ordering) { + GaussianBayesNet _eliminate_const(const FactorGraph& graph, CachedFactors& cached, const Ordering& ordering) { // make a copy that can be modified locally FactorGraph graph_ignored = graph; return _eliminate(graph_ignored, cached, ordering); @@ -67,9 +67,9 @@ namespace gtsam { /** Create a Bayes Tree from a nonlinear factor graph */ template ISAM2::ISAM2(const NonlinearFactorGraph& nlfg, const Ordering& ordering, const Config& config) - : BayesTree(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), linPoint_(config), estimate_(config) { + : BayesTree(nlfg.linearize(config).eliminate(ordering)), nonlinearFactors_(nlfg), linPoint_(config) { // todo: repeats calculation above, just to set "cached" - _eliminate_const(nlfg.linearize(config), cached, ordering); + _eliminate_const(nlfg.linearize(config), cached_, ordering); } /* ************************************************************************* */ @@ -108,7 +108,7 @@ namespace gtsam { it--; const Symbol& key = *it; // retrieve the cached factor and add to boundary - cachedBoundary.push_back(cached[key]); + cachedBoundary.push_back(cached_[key]); } return cachedBoundary; @@ -119,13 +119,16 @@ namespace gtsam { void ISAM2::update_internal(const NonlinearFactorGraph& newFactors, const Config& config, Cliques& orphans) { +#if 0 // todo - temporarily disabled -------------------------------------------------------------------------------------------------- // copy variables into config_, but don't overwrite existing entries (current linearization point!) for (typename Config::const_iterator it = config.begin(); it!=config.end(); it++) { if (!linPoint_.contains(it->first)) { linPoint_.insert(it->first, it->second); - estimate_.insert(it->first, it->second); } } +#else + linPoint_ = config; +#endif FactorGraph newFactorsLinearized = newFactors.linearize(linPoint_); @@ -155,7 +158,7 @@ namespace gtsam { } // eliminate into a Bayes net - BayesNet bayesNet = _eliminate(factors, cached, ordering); + BayesNet bayesNet = _eliminate(factors, cached_, ordering); // remember the new factors for later relinearization nonlinearFactors_.push_back(newFactors); @@ -176,9 +179,7 @@ namespace gtsam { } // update solution - todo: potentially expensive - VectorConfig delta = optimize2(*this); -// delta.print(); - estimate_ += delta; + delta_ = optimize2(*this); } diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h index ed84e154e..f01f55ff8 100644 --- a/cpp/ISAM2.h +++ b/cpp/ISAM2.h @@ -24,7 +24,7 @@ namespace gtsam { - typedef std::map cachedFactors; + typedef std::map CachedFactors; template class ISAM2: public BayesTree { @@ -34,14 +34,14 @@ namespace gtsam { // current linearization point Config linPoint_; - // most recent estimate - Config estimate_; + // most recent solution + VectorConfig delta_; // for keeping all original nonlinear factors NonlinearFactorGraph nonlinearFactors_; // cached intermediate results for restarting computation in the middle - cachedFactors cached; + CachedFactors cached_; public: @@ -65,7 +65,7 @@ namespace gtsam { void update_internal(const NonlinearFactorGraph& newFactors, const Config& config, Cliques& orphans); void update(const NonlinearFactorGraph& newFactors, const Config& config); - const Config estimate() {return estimate_;} + const Config estimate() {return expmap(linPoint_, delta_);} private: FactorGraph relinearizeAffectedFactors(const std::list& affectedKeys);