From 42f4ff228b93f0de62efb8c00507a41237fb816d Mon Sep 17 00:00:00 2001 From: Michael Kaess Date: Mon, 18 Jan 2010 04:32:45 +0000 Subject: [PATCH] code cleanup, recovering estimate while dealing with incremental adding of factors, planar with new SLAM --- cpp/ISAM2-inl.h | 167 ++++++++++++-------------------------- cpp/ISAM2.h | 15 +++- cpp/testGaussianISAM2.cpp | 18 ++++ 3 files changed, 84 insertions(+), 116 deletions(-) diff --git a/cpp/ISAM2-inl.h b/cpp/ISAM2-inl.h index 3306ddec0..8622fe976 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 string& 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 @@ -47,7 +47,7 @@ namespace gtsam { // from GaussianFactorGraph.cpp, see _eliminateOne above GaussianBayesNet _eliminate(FactorGraph& graph, cachedFactors& cached, const Ordering& ordering) { GaussianBayesNet chordalBayesNet; // empty - BOOST_FOREACH(const Symbol& key, ordering) { + BOOST_FOREACH(string key, ordering) { GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key); chordalBayesNet.push_back(cg); } @@ -72,6 +72,43 @@ namespace gtsam { _eliminate_const(nlfg.linearize(config), cached, ordering); } + /* ************************************************************************* */ + // retrieve all factors that ONLY contain the affected variables + // (note that the remaining stuff is summarized in the cached factors) + template + FactorGraph ISAM2::relinearizeAffectedFactors(const list& affectedKeys) { + NonlinearFactorGraph nonlinearAffectedFactors; + typename FactorGraph >::iterator it; + for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) { + bool inside = true; + BOOST_FOREACH(string key, (*it)->keys()) { + if (find(affectedKeys.begin(), affectedKeys.end(), key) == affectedKeys.end()) + inside = false; + } + if (inside) + nonlinearAffectedFactors.push_back(*it); + } + return nonlinearAffectedFactors.linearize(config_); + } + + /* ************************************************************************* */ + template + FactorGraph ISAM2::getCachedBoundaryFactors(Cliques& orphans) { + // add intermediate (linearized) factors from cache that are passed into the affected area + FactorGraph cachedBoundary; + BOOST_FOREACH(sharedClique orphan, orphans) { + // find the last variable that is not part of the separator + string oneTooFar = orphan->separator_.front(); + list keys = orphan->keys(); + list::iterator it = find(keys.begin(), keys.end(), oneTooFar); + it--; + string key = *it; + // retrieve the cached factor and add to boundary + cachedBoundary.push_back(cached[key]); + } + return cachedBoundary; + } + /* ************************************************************************* */ template void ISAM2::update_internal(const NonlinearFactorGraph& newFactors, @@ -90,125 +127,31 @@ namespace gtsam { FactorGraph affectedFactors; boost::tie(affectedFactors, orphans) = this->removeTop(newFactorsLinearized); -#if 1 -#if 0 - // find the corresponding original nonlinear factors, and relinearize them - NonlinearFactorGraph nonlinearAffectedFactors; - set idxs; // avoid duplicates by putting index into set - BOOST_FOREACH(FactorGraph::sharedFactor fac, affectedFactors) { - // retrieve correspondent factor from nonlinearFactors_ - Ordering keys = fac->keys(); - BOOST_FOREACH(const Symbol& key, keys) { - list indices = nonlinearFactors_.factors(key); - BOOST_FOREACH(int idx, indices) { - // todo - only insert index if factor is subset of keys... not needed once we do relinearization - but then how to deal with overlap with orphans? - bool subset = true; - BOOST_FOREACH(const Symbol& k, nonlinearFactors_[idx]->keys()) { - if (find(keys.begin(), keys.end(), k)==keys.end()) subset = false; - } - if (subset) { - idxs.insert(idx); - } - } - } - } - BOOST_FOREACH(int idx, idxs) { - nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]); - } + // relinearize the affected factors ... + list affectedKeys = affectedFactors.keys(); + FactorGraph factors = relinearizeAffectedFactors(affectedKeys); - FactorGraph factors = nonlinearAffectedFactors.linearize(config_); - -#else - - NonlinearFactorGraph nonlinearAffectedFactors; - - // retrieve all factors that ONLY contain the affected variables - // (note that the remaining stuff is summarized in the cached factors) - list affectedKeys = affectedFactors.keys(); - typename FactorGraph >::iterator it; - for(it = nonlinearFactors_.begin(); it != nonlinearFactors_.end(); it++) { - bool inside = true; - BOOST_FOREACH(string key, (*it)->keys()) { - if (find(affectedKeys.begin(), affectedKeys.end(), key) == affectedKeys.end()) - inside = false; - } - if (inside) - nonlinearAffectedFactors.push_back(*it); - } - - FactorGraph factors = nonlinearAffectedFactors.linearize(config_); - - // recover intermediate factors from cache that are passed into the affected area - FactorGraph cachedBoundary; - BOOST_FOREACH(sharedClique orphan, orphans) { - // find the last variable that is not part of the separator - string oneTooFar = orphan->separator_.front(); - list keys = orphan->keys(); - list::iterator it = find(keys.begin(), keys.end(), oneTooFar); - it--; - const Symbol& key = *it; - // retrieve the cached factor and add to boundary - cachedBoundary.push_back(cached[key]); - } + // ... add the cached intermediate results from the boundary of the orphans ... + FactorGraph cachedBoundary = getCachedBoundaryFactors(orphans); factors.push_back(cachedBoundary); -#endif - - -#if 0 - printf("**************\n"); - nonlinearFactors_.linearize(config).print("all factors"); - printf("--------------\n"); - newFactorsLinearized.print("newFactorsLinearized"); - printf("--------------\n"); - factors.print("factors"); - printf("--------------\n"); - affectedFactors.print("affectedFactors"); - printf("--------------\n"); -#endif - - // add the new factors themselves + // ... and finally add the new linearized factors themselves factors.push_back(newFactorsLinearized); -#endif // create an ordering for the new and contaminated factors Ordering ordering; if (true) { ordering = factors.getOrdering(); } else { - list keys = factors.keys(); + list keys = factors.keys(); keys.sort(); // todo: correct sorting order? ordering = keys; } -#if 0 - ordering.print(); - - factors.print("factors BEFORE"); - printf("--------------\n"); -#endif - // eliminate into a Bayes net BayesNet bayesNet = _eliminate(factors, cached, ordering); -#if 1 - // check if relinearized agrees with correct solution - affectedFactors.push_back(newFactorsLinearized); - -#if 0 - affectedFactors.print("affectedFactors BEFORE"); - printf("--------------\n"); -#endif - - BayesNet bayesNetTest = eliminate(affectedFactors, ordering); - if (!bayesNet.equals(bayesNetTest)) { - printf("differ\n"); - bayesNet.print(); - bayesNetTest.print(); - exit(42); - } -#endif - + // remember the new factors for later relinearization nonlinearFactors_.push_back(newFactors); // insert conditionals back in, straight into the topless bayesTree @@ -216,29 +159,25 @@ namespace gtsam { for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) this->insert(*rit); - int count = 0; // add orphans to the bottom of the new tree BOOST_FOREACH(sharedClique orphan, orphans) { - Symbol key = orphan->separator_.front(); + string key = orphan->separator_.front(); sharedClique parent = (*this)[key]; parent->children_ += orphan; orphan->parent_ = parent; // set new parent! } + + // update solution + VectorConfig solution = optimize2(*this); + solution.print(); + } template void ISAM2::update(const NonlinearFactorGraph& newFactors, const Config& config) { -#if 0 - printf("8888888888888888\n"); - try { - this->print("BayesTree"); - } catch (char * c) {}; - printf("8888888888888888\n"); -#endif - Cliques orphans; this->update_internal(newFactors, config, orphans); } diff --git a/cpp/ISAM2.h b/cpp/ISAM2.h index 214f5963e..ed84e154e 100644 --- a/cpp/ISAM2.h +++ b/cpp/ISAM2.h @@ -31,8 +31,13 @@ namespace gtsam { protected: - // for keeping all original nonlinear data - Config config_; + // current linearization point + Config linPoint_; + + // most recent estimate + Config estimate_; + + // for keeping all original nonlinear factors NonlinearFactorGraph nonlinearFactors_; // cached intermediate results for restarting computation in the middle @@ -60,6 +65,12 @@ 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_;} + + private: + FactorGraph relinearizeAffectedFactors(const std::list& affectedKeys); + FactorGraph getCachedBoundaryFactors(Cliques& orphans); + }; // ISAM2 } /// namespace gtsam diff --git a/cpp/testGaussianISAM2.cpp b/cpp/testGaussianISAM2.cpp index e1cdf255d..e239765fc 100644 --- a/cpp/testGaussianISAM2.cpp +++ b/cpp/testGaussianISAM2.cpp @@ -21,6 +21,24 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +/* ************************************************************************* */ +TEST( ISAM2, solving ) +{ + ExampleNonlinearFactorGraph nlfg = createNonlinearFactorGraph(); + VectorConfig noisy = createNoisyConfig(); + Ordering ordering; + ordering += symbol('x', 1); + ordering += symbol('x', 2); + ordering += symbol('l', 1); + GaussianISAM2 btree(nlfg, ordering, noisy); + VectorConfig actualDelta = optimize2(btree); + VectorConfig delta = createCorrectDelta(); + CHECK(assert_equal(delta, actualDelta)); + VectorConfig actualSolution = noisy+actualDelta; + VectorConfig solution = createConfig(); + CHECK(assert_equal(solution, actualSolution)); +} + /* ************************************************************************* */ TEST( ISAM2, ISAM2_smoother ) {