12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update+relin combined for speed; new backsub/threshold confirmed to yield correct result

release/4.3a0
Michael Kaess 2010-09-09 23:54:39 +00:00
parent aca6602a32
commit b825655ba6
3 changed files with 438 additions and 379 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

184
inference/ISAM2-inl.h
View File

@ -83,7 +83,7 @@ void tictoc_print() {
timing.print(); timing.print();
} }
#else #else
void tictoc_print() {} void tictoc_print() {}
double tic(string id) {return 0.;} double tic(string id) {return 0.;}
double toc(string id) {return 0.;} double toc(string id) {return 0.;}
#endif #endif
@ -91,10 +91,10 @@ double toc(string id) {return 0.;}
namespace gtsam { namespace gtsam {
using namespace std; using namespace std;
// from inference-inl.h - need to additionally return the newly created factor for caching // from inference-inl.h - need to additionally return the newly created factor for caching
boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Symbol& key) { boost::shared_ptr<GaussianConditional> _eliminateOne(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Symbol& key) {
// combine the factors of all nodes connected to the variable to be eliminated // combine the factors of all nodes connected to the variable to be eliminated
// if no factors are connected to key, returns an empty factor // if no factors are connected to key, returns an empty factor
@ -117,42 +117,42 @@ namespace gtsam {
// return the conditional Gaussian // return the conditional Gaussian
return conditional; return conditional;
} }
// from GaussianFactorGraph.cpp, see _eliminateOne above // from GaussianFactorGraph.cpp, see _eliminateOne above
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) { GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
GaussianBayesNet chordalBayesNet; // empty GaussianBayesNet chordalBayesNet; // empty
BOOST_FOREACH(const Symbol& key, ordering) { BOOST_FOREACH(const Symbol& key, ordering) {
GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key); GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
chordalBayesNet.push_back(cg); chordalBayesNet.push_back(cg);
} }
return chordalBayesNet; return chordalBayesNet;
} }
// special const version used in constructor below // special const version used in constructor below
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) { GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
// make a copy that can be modified locally // make a copy that can be modified locally
FactorGraph<GaussianFactor> graph_ignored = graph; FactorGraph<GaussianFactor> graph_ignored = graph;
return _eliminate(graph_ignored, cached, ordering); return _eliminate(graph_ignored, cached, ordering);
} }
/** Create an empty Bayes Tree */ /** Create an empty Bayes Tree */
template<class Conditional, class Config> template<class Conditional, class Config>
ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>() {} ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>() {}
/** Create a Bayes Tree from a nonlinear factor graph */ /** Create a Bayes Tree from a nonlinear factor graph */
template<class Conditional, class Config> template<class Conditional, class Config>
ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config) ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config)
: BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)), : BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)),
theta_(config), delta_(VectorConfig()), nonlinearFactors_(nlfg) { theta_(config), delta_(VectorConfig()), nonlinearFactors_(nlfg) {
// todo: repeats calculation above, just to set "cached" // todo: repeats calculation above, just to set "cached"
// De-referencing shared pointer can be quite expensive because creates temporary // De-referencing shared pointer can be quite expensive because creates temporary
_eliminate_const(*nlfg.linearize(config), cached_, ordering); _eliminate_const(*nlfg.linearize(config), cached_, ordering);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Config>
list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const { list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const {
FactorGraph<NonlinearFactor<Config> > allAffected; FactorGraph<NonlinearFactor<Config> > allAffected;
list<size_t> indices; list<size_t> indices;
BOOST_FOREACH(const Symbol& key, keys) { BOOST_FOREACH(const Symbol& key, keys) {
@ -162,14 +162,14 @@ namespace gtsam {
indices.sort(); indices.sort();
indices.unique(); indices.unique();
return indices; return indices;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// retrieve all factors that ONLY contain the affected variables // retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors) // (note that the remaining stuff is summarized in the cached factors)
template<class Conditional, class Config> template<class Conditional, class Config>
boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors
(const set<Symbol>& affectedKeys) const { (const set<Symbol>& affectedKeys) const {
list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list... list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list...
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end()); affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
@ -191,12 +191,12 @@ namespace gtsam {
// TODO: temporary might be expensive, return shared pointer ? // TODO: temporary might be expensive, return shared pointer ?
return nonlinearAffectedFactors.linearize(theta_); return nonlinearAffectedFactors.linearize(theta_);
} }
/* ************************************************************************* */ /* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area // find intermediate (linearized) factors from cache that are passed into the affected area
template<class Conditional, class Config> template<class Conditional, class Config>
FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) { FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) {
FactorGraph<GaussianFactor> cachedBoundary; FactorGraph<GaussianFactor> cachedBoundary;
BOOST_FOREACH(sharedClique orphan, orphans) { BOOST_FOREACH(sharedClique orphan, orphans) {
@ -207,15 +207,15 @@ namespace gtsam {
} }
return cachedBoundary; return cachedBoundary;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Config>
void ISAM2<Conditional, Config>::linear_update(const FactorGraph<GaussianFactor>& newFactors) { void ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors) {
// Input: BayesTree(this), newFactors // Input: BayesTree(this), newFactors
//#define PRINT_STATS // figures for paper, disable for timing //#define PRINT_STATS // figures for paper, disable for timing
#ifdef PRINT_STATS #ifdef PRINT_STATS
static int counter = 0; static int counter = 0;
int maxClique = 0; int maxClique = 0;
@ -236,10 +236,11 @@ namespace gtsam {
// 1. Remove top of Bayes tree and convert to a factor graph: // 1. Remove top of Bayes tree and convert to a factor graph:
// (a) For each affected variable, remove the corresponding clique and all parents up to the root. // (a) For each affected variable, remove the corresponding clique and all parents up to the root.
// (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques. // (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques.
const list<Symbol> markedKeys = newFactors.keys(); tic("re-removetop");
Cliques orphans; Cliques orphans;
BayesNet<GaussianConditional> affectedBayesNet; BayesNet<GaussianConditional> affectedBayesNet;
this->removeTop(markedKeys, affectedBayesNet, orphans); this->removeTop(markedKeys, affectedBayesNet, orphans);
toc("re-removetop");
// FactorGraph<GaussianFactor> factors(affectedBayesNet); // FactorGraph<GaussianFactor> factors(affectedBayesNet);
// bug was here: we cannot reuse the original factors, because then the cached factors get messed up // bug was here: we cannot reuse the original factors, because then the cached factors get messed up
@ -252,7 +253,7 @@ namespace gtsam {
// BEGIN OF COPIED CODE // BEGIN OF COPIED CODE
tic("linear_lookup1"); tic("re-lookup");
// ordering provides all keys in conditionals, there cannot be others because path to root included // ordering provides all keys in conditionals, there cannot be others because path to root included
set<Symbol> affectedKeys; set<Symbol> affectedKeys;
list<Symbol> tmp = affectedBayesNet.ordering(); list<Symbol> tmp = affectedBayesNet.ordering();
@ -271,17 +272,23 @@ namespace gtsam {
<< " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl; << " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
#endif #endif
toc("linear_lookup1"); toc("re-lookup");
tic("re-cached");
// add the cached intermediate results from the boundary of the orphans ... // add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans); FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
factors.push_back(cachedBoundary); factors.push_back(cachedBoundary);
toc("re-cached");
// END OF COPIED CODE // END OF COPIED CODE
// 2. Add the new factors \Factors' into the resulting factor graph // 2. Add the new factors \Factors' into the resulting factor graph
factors.push_back(newFactors); tic("re-newfactors");
if (newFactors) {
factors.push_back(*newFactors);
}
toc("re-newfactors");
// 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]) // 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])
@ -290,13 +297,14 @@ namespace gtsam {
set<Symbol> markedKeysSet; set<Symbol> markedKeysSet;
markedKeysSet.insert(markedKeys.begin(), markedKeys.end()); markedKeysSet.insert(markedKeys.begin(), markedKeys.end());
Ordering ordering = factors.getConstrainedOrdering(markedKeysSet); // intelligent ordering Ordering ordering = factors.getConstrainedOrdering(markedKeysSet); // intelligent ordering
// Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance // Ordering ordering = factors.getOrdering(); // original ordering, yields in bad performance
// eliminate into a Bayes net // eliminate into a Bayes net
tic("linear_eliminate"); tic("eliminate");
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering); BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
toc("linear_eliminate"); toc("eliminate");
tic("re-assemble");
// Create Index from ordering // Create Index from ordering
IndexTable<Symbol> index(ordering); IndexTable<Symbol> index(ordering);
@ -307,9 +315,11 @@ namespace gtsam {
// Save number of affectedCliques // Save number of affectedCliques
lastAffectedCliqueCount = this->size(); lastAffectedCliqueCount = this->size();
tic("re-assemble");
// 4. Insert the orphans back into the new Bayes tree. // 4. Insert the orphans back into the new Bayes tree.
tic("re-orphans");
// add orphans to the bottom of the new tree // add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) { BOOST_FOREACH(sharedClique orphan, orphans) {
Symbol parentRepresentative = findParentClique(orphan->separator_, index); Symbol parentRepresentative = findParentClique(orphan->separator_, index);
@ -317,14 +327,22 @@ namespace gtsam {
parent->children_ += orphan; parent->children_ += orphan;
orphan->parent_ = parent; // set new parent! orphan->parent_ = parent; // set new parent!
} }
toc("re-orphans");
// Output: BayesTree(this) // Output: BayesTree(this)
} }
/* ************************************************************************* */ /* ************************************************************************* */
// find all variables that are directly connected by a measurement to one of the marked variables template<class Conditional, class Config>
template<class Conditional, class Config> void ISAM2<Conditional, Config>::linear_update(const FactorGraph<GaussianFactor>& newFactors) {
void ISAM2<Conditional, Config>::find_all(sharedClique clique, list<Symbol>& keys, const list<Symbol>& marked) { const list<Symbol> markedKeys = newFactors.keys();
recalculate(markedKeys, &newFactors);
}
/* ************************************************************************* */
// find all variables that are directly connected by a measurement to one of the marked variables
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::find_all(sharedClique clique, list<Symbol>& keys, const list<Symbol>& marked) {
// does the separator contain any of the variables? // does the separator contain any of the variables?
bool found = false; bool found = false;
BOOST_FOREACH(const Symbol& key, clique->separator_) { BOOST_FOREACH(const Symbol& key, clique->separator_) {
@ -338,11 +356,11 @@ namespace gtsam {
BOOST_FOREACH(const sharedClique& child, clique->children_) { BOOST_FOREACH(const sharedClique& child, clique->children_) {
find_all(child, keys, marked); find_all(child, keys, marked);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class Conditional, class Config> template<class Conditional, class Config>
void ISAM2<Conditional, Config>::fluid_relinearization(double relinearize_threshold) { list<Symbol> ISAM2<Conditional, Config>::fluid_relinearization(double relinearize_threshold) {
// Input: nonlinear factors factors_, linearization point theta_, Bayes tree (this), delta_ // Input: nonlinear factors factors_, linearization point theta_, Bayes tree (this), delta_
@ -378,13 +396,17 @@ namespace gtsam {
// add the cached marginal factors from its children, and re-eliminate. // add the cached marginal factors from its children, and re-eliminate.
// todo: for simplicity, currently simply remove the top and recreate it using the original ordering // todo: for simplicity, currently simply remove the top and recreate it using the original ordering
//recalculate(affectedSymbols);
return affectedSymbols;
#if 0
tic("nonlin-mess"); tic("nonlin-mess");
Cliques orphans; Cliques orphans;
BayesNet<GaussianConditional> affectedBayesNet; BayesNet<GaussianConditional> affectedBayesNet;
this->removeTop(affectedSymbols, affectedBayesNet, orphans); this->removeTop(affectedSymbols, affectedBayesNet, orphans);
// remember original ordering // remember original ordering
// todo Ordering original_ordering = affectedBayesNet.ordering(); // does not yield original ordering... // todo Ordering original_ordering = affectedBayesNet.ordering(); // does not yield original ordering...
FactorGraph<GaussianFactor> tmp_factors(affectedBayesNet); // so instead we recalculate an acceptable ordering here FactorGraph<GaussianFactor> tmp_factors(affectedBayesNet); // so instead we recalculate an acceptable ordering here
Ordering original_ordering = tmp_factors.getOrdering(); // todo - remove multiple lines up to here Ordering original_ordering = tmp_factors.getOrdering(); // todo - remove multiple lines up to here
@ -404,7 +426,7 @@ namespace gtsam {
lastNonlinearAffectedVariableCount = affectedKeys.size(); lastNonlinearAffectedVariableCount = affectedKeys.size();
lastNonlinearAffectedFactorCount = factors->size(); lastNonlinearAffectedFactorCount = factors->size();
// cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl; // cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl;
// add the cached intermediate results from the boundary of the orphans ... // add the cached intermediate results from the boundary of the orphans ...
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans); FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
@ -430,14 +452,19 @@ namespace gtsam {
parent->children_ += orphan; parent->children_ += orphan;
orphan->parent_ = parent; // set new parent! orphan->parent_ = parent; // set new parent!
} }
#endif
// Output: updated Bayes tree (this), updated linearization point theta_ // Output: updated Bayes tree (this), updated linearization point theta_
} }
}
/* ************************************************************************* */ list<Symbol> empty;
template<class Conditional, class Config> return empty;
void ISAM2<Conditional, Config>::update(
}
/* ************************************************************************* */
template<class Conditional, class Config>
void ISAM2<Conditional, Config>::update(
const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta, const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
double wildfire_threshold, double relinearize_threshold, bool relinearize) { double wildfire_threshold, double relinearize_threshold, bool relinearize) {
@ -463,9 +490,10 @@ namespace gtsam {
tic("step3"); tic("step3");
// 3. Linearize new factor // 3. Linearize new factor
boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_); // boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
toc("step3"); toc("step3");
#if 0 // original algorithm
tic("step4"); tic("step4");
// 4. Linear iSAM step (alg 3) // 4. Linear iSAM step (alg 3)
linear_update(*linearFactors); // in: this linear_update(*linearFactors); // in: this
@ -485,11 +513,41 @@ namespace gtsam {
// todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again // todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again
delta_ = optimize2(*this, wildfire_threshold); delta_ = optimize2(*this, wildfire_threshold);
#else // new algorithm
tic("step4B");
// 4B. Mark linear update
list<Symbol> markedKeys = newFactors.keys();
toc("step4B");
tic("step5B");
// 5B. Mark nonlinear update
if (relinearize) {
list<Symbol> markedRelin = fluid_relinearization(relinearize_threshold); // in: delta_, theta_, nonlinearFactors_, this
// merge with markedKeys
markedKeys.splice(markedKeys.begin(), markedRelin, markedRelin.begin(), markedRelin.end());
markedKeys.sort(); // remove duplicates
markedKeys.unique();
}
toc("step5B");
tic("step6B");
// 6B. Redo top of Bayes tree
boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
recalculate(markedKeys, &(*linearFactors));
toc("step6B");
tic("step7B");
// 7B. Solve
delta_ = optimize2(*this, wildfire_threshold);
toc("step7B");
#endif
toc("all"); toc("all");
tictoc_print(); tictoc_print();
} }
/* ************************************************************************* */ /* ************************************************************************* */

17
inference/ISAM2.h
View File

@ -25,12 +25,12 @@
namespace gtsam { namespace gtsam {
typedef SymbolMap<GaussianFactor::shared_ptr> CachedFactors; typedef SymbolMap<GaussianFactor::shared_ptr> CachedFactors;
template<class Conditional, class Config> template<class Conditional, class Config>
class ISAM2: public BayesTree<Conditional> { class ISAM2: public BayesTree<Conditional> {
protected: protected:
// current linearization point // current linearization point
Config theta_; Config theta_;
@ -44,7 +44,7 @@ namespace gtsam {
// cached intermediate results for restarting computation in the middle // cached intermediate results for restarting computation in the middle
CachedFactors cached_; CachedFactors cached_;
public: public:
/** Create an empty Bayes Tree */ /** Create an empty Bayes Tree */
ISAM2(); ISAM2();
@ -84,16 +84,17 @@ namespace gtsam {
size_t lastNonlinearAffectedVariableCount; size_t lastNonlinearAffectedVariableCount;
size_t lastNonlinearAffectedFactorCount; size_t lastNonlinearAffectedFactorCount;
private: private:
std::list<size_t> getAffectedFactors(const std::list<Symbol>& keys) const; std::list<size_t> getAffectedFactors(const std::list<Symbol>& keys) const;
boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const; boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const;
FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans); FactorGraph<GaussianFactor> getCachedBoundaryFactors(Cliques& orphans);
void recalculate(const std::list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors = NULL);
void linear_update(const FactorGraph<GaussianFactor>& newFactors); void linear_update(const FactorGraph<GaussianFactor>& newFactors);
void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function
void fluid_relinearization(double relinearize_threshold); std::list<Symbol> fluid_relinearization(double relinearize_threshold);
}; // ISAM2 }; // ISAM2
} /// namespace gtsam } /// namespace gtsam

2
slam/GaussianISAM2.cpp
View File

@ -63,7 +63,7 @@ void optimize2(const GaussianISAM2::sharedClique& clique, double threshold, set<
} }
/* ************************************************************************* */ /* ************************************************************************* */
// fast version without threshold // fast full version without threshold
void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) { void optimize2(const GaussianISAM2::sharedClique& clique, VectorConfig& result) {
// parents are assumed to already be solved and available in result // parents are assumed to already be solved and available in result
GaussianISAM2::Clique::const_reverse_iterator it; GaussianISAM2::Clique::const_reverse_iterator it;