update+relin combined for speed; new backsub/threshold confirmed to yield correct result
Michael Kaess
parent
aca6602a32
commit
b825655ba6
|
|
@ -83,7 +83,7 @@ void tictoc_print() {
|
|||
timing.print();
|
||||
}
|
||||
#else
|
||||
void tictoc_print() {}
|
||||
void tictoc_print() {}
|
||||
double tic(string id) {return 0.;}
|
||||
double toc(string id) {return 0.;}
|
||||
#endif
|
||||
|
|
@ -91,10 +91,10 @@ double toc(string id) {return 0.;}
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
using namespace std;
|
||||
using namespace std;
|
||||
|
||||
// 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) {
|
||||
// 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) {
|
||||
|
||||
// combine the factors of all nodes connected to the variable to be eliminated
|
||||
// if no factors are connected to key, returns an empty factor
|
||||
|
|
@ -117,42 +117,42 @@ namespace gtsam {
|
|||
|
||||
// return the conditional Gaussian
|
||||
return conditional;
|
||||
}
|
||||
}
|
||||
|
||||
// from GaussianFactorGraph.cpp, see _eliminateOne above
|
||||
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
|
||||
// from GaussianFactorGraph.cpp, see _eliminateOne above
|
||||
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
|
||||
GaussianBayesNet chordalBayesNet; // empty
|
||||
BOOST_FOREACH(const Symbol& key, ordering) {
|
||||
GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
|
||||
chordalBayesNet.push_back(cg);
|
||||
}
|
||||
return chordalBayesNet;
|
||||
}
|
||||
}
|
||||
|
||||
// special const version used in constructor below
|
||||
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
|
||||
// special const version used in constructor below
|
||||
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
|
||||
// make a copy that can be modified locally
|
||||
FactorGraph<GaussianFactor> graph_ignored = graph;
|
||||
return _eliminate(graph_ignored, cached, ordering);
|
||||
}
|
||||
}
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
template<class Conditional, class Config>
|
||||
ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>() {}
|
||||
/** Create an empty Bayes Tree */
|
||||
template<class Conditional, class Config>
|
||||
ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>() {}
|
||||
|
||||
/** Create a Bayes Tree from a nonlinear factor graph */
|
||||
template<class Conditional, class Config>
|
||||
ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config)
|
||||
: BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)),
|
||||
/** Create a Bayes Tree from a nonlinear factor graph */
|
||||
template<class Conditional, class Config>
|
||||
ISAM2<Conditional, Config>::ISAM2(const NonlinearFactorGraph<Config>& nlfg, const Ordering& ordering, const Config& config)
|
||||
: BayesTree<Conditional>(nlfg.linearize(config)->eliminate(ordering)),
|
||||
theta_(config), delta_(VectorConfig()), nonlinearFactors_(nlfg) {
|
||||
// todo: repeats calculation above, just to set "cached"
|
||||
// De-referencing shared pointer can be quite expensive because creates temporary
|
||||
_eliminate_const(*nlfg.linearize(config), cached_, ordering);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const {
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const {
|
||||
FactorGraph<NonlinearFactor<Config> > allAffected;
|
||||
list<size_t> indices;
|
||||
BOOST_FOREACH(const Symbol& key, keys) {
|
||||
|
|
@ -162,14 +162,14 @@ namespace gtsam {
|
|||
indices.sort();
|
||||
indices.unique();
|
||||
return indices;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// retrieve all factors that ONLY contain the affected variables
|
||||
// (note that the remaining stuff is summarized in the cached factors)
|
||||
template<class Conditional, class Config>
|
||||
boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors
|
||||
(const set<Symbol>& affectedKeys) const {
|
||||
/* ************************************************************************* */
|
||||
// retrieve all factors that ONLY contain the affected variables
|
||||
// (note that the remaining stuff is summarized in the cached factors)
|
||||
template<class Conditional, class Config>
|
||||
boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors
|
||||
(const set<Symbol>& affectedKeys) const {
|
||||
|
||||
list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list...
|
||||
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
|
||||
|
|
@ -191,12 +191,12 @@ namespace gtsam {
|
|||
|
||||
// TODO: temporary might be expensive, return shared pointer ?
|
||||
return nonlinearAffectedFactors.linearize(theta_);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// find intermediate (linearized) factors from cache that are passed into the affected area
|
||||
template<class Conditional, class Config>
|
||||
FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) {
|
||||
/* ************************************************************************* */
|
||||
// find intermediate (linearized) factors from cache that are passed into the affected area
|
||||
template<class Conditional, class Config>
|
||||
FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) {
|
||||
FactorGraph<GaussianFactor> cachedBoundary;
|
||||
|
||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
||||
|
|
@ -207,15 +207,15 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
return cachedBoundary;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
void ISAM2<Conditional, Config>::linear_update(const FactorGraph<GaussianFactor>& newFactors) {
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
void ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* 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
|
||||
static int counter = 0;
|
||||
int maxClique = 0;
|
||||
|
|
@ -236,10 +236,11 @@ namespace gtsam {
|
|||
// 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.
|
||||
// (b) Store orphaned sub-trees \BayesTree_{O} of removed cliques.
|
||||
const list<Symbol> markedKeys = newFactors.keys();
|
||||
tic("re-removetop");
|
||||
Cliques orphans;
|
||||
BayesNet<GaussianConditional> affectedBayesNet;
|
||||
this->removeTop(markedKeys, affectedBayesNet, orphans);
|
||||
toc("re-removetop");
|
||||
|
||||
// FactorGraph<GaussianFactor> factors(affectedBayesNet);
|
||||
// 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
|
||||
|
||||
tic("linear_lookup1");
|
||||
tic("re-lookup");
|
||||
// ordering provides all keys in conditionals, there cannot be others because path to root included
|
||||
set<Symbol> affectedKeys;
|
||||
list<Symbol> tmp = affectedBayesNet.ordering();
|
||||
|
|
@ -271,17 +272,23 @@ namespace gtsam {
|
|||
<< " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
|
||||
#endif
|
||||
|
||||
toc("linear_lookup1");
|
||||
toc("re-lookup");
|
||||
|
||||
tic("re-cached");
|
||||
// add the cached intermediate results from the boundary of the orphans ...
|
||||
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
|
||||
factors.push_back(cachedBoundary);
|
||||
toc("re-cached");
|
||||
|
||||
// END OF COPIED CODE
|
||||
|
||||
|
||||
// 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])
|
||||
|
||||
|
|
@ -290,13 +297,14 @@ namespace gtsam {
|
|||
set<Symbol> markedKeysSet;
|
||||
markedKeysSet.insert(markedKeys.begin(), markedKeys.end());
|
||||
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
|
||||
tic("linear_eliminate");
|
||||
tic("eliminate");
|
||||
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
|
||||
toc("linear_eliminate");
|
||||
toc("eliminate");
|
||||
|
||||
tic("re-assemble");
|
||||
// Create Index from ordering
|
||||
IndexTable<Symbol> index(ordering);
|
||||
|
||||
|
|
@ -307,9 +315,11 @@ namespace gtsam {
|
|||
|
||||
// Save number of affectedCliques
|
||||
lastAffectedCliqueCount = this->size();
|
||||
tic("re-assemble");
|
||||
|
||||
// 4. Insert the orphans back into the new Bayes tree.
|
||||
|
||||
tic("re-orphans");
|
||||
// add orphans to the bottom of the new tree
|
||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
||||
Symbol parentRepresentative = findParentClique(orphan->separator_, index);
|
||||
|
|
@ -317,14 +327,22 @@ namespace gtsam {
|
|||
parent->children_ += orphan;
|
||||
orphan->parent_ = parent; // set new parent!
|
||||
}
|
||||
toc("re-orphans");
|
||||
|
||||
// Output: BayesTree(this)
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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) {
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
void ISAM2<Conditional, Config>::linear_update(const FactorGraph<GaussianFactor>& newFactors) {
|
||||
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?
|
||||
bool found = false;
|
||||
BOOST_FOREACH(const Symbol& key, clique->separator_) {
|
||||
|
|
@ -338,11 +356,11 @@ namespace gtsam {
|
|||
BOOST_FOREACH(const sharedClique& child, clique->children_) {
|
||||
find_all(child, keys, marked);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
void ISAM2<Conditional, Config>::fluid_relinearization(double relinearize_threshold) {
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
list<Symbol> ISAM2<Conditional, Config>::fluid_relinearization(double relinearize_threshold) {
|
||||
|
||||
// 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.
|
||||
|
||||
// todo: for simplicity, currently simply remove the top and recreate it using the original ordering
|
||||
//recalculate(affectedSymbols);
|
||||
return affectedSymbols;
|
||||
|
||||
#if 0
|
||||
|
||||
tic("nonlin-mess");
|
||||
Cliques orphans;
|
||||
BayesNet<GaussianConditional> affectedBayesNet;
|
||||
this->removeTop(affectedSymbols, affectedBayesNet, orphans);
|
||||
// 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
|
||||
Ordering original_ordering = tmp_factors.getOrdering(); // todo - remove multiple lines up to here
|
||||
|
||||
|
|
@ -404,7 +426,7 @@ namespace gtsam {
|
|||
lastNonlinearAffectedVariableCount = affectedKeys.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 ...
|
||||
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
|
||||
|
|
@ -430,14 +452,19 @@ namespace gtsam {
|
|||
parent->children_ += orphan;
|
||||
orphan->parent_ = parent; // set new parent!
|
||||
}
|
||||
#endif
|
||||
|
||||
// Output: updated Bayes tree (this), updated linearization point theta_
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
void ISAM2<Conditional, Config>::update(
|
||||
list<Symbol> empty;
|
||||
return empty;
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional, class Config>
|
||||
void ISAM2<Conditional, Config>::update(
|
||||
const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
|
||||
double wildfire_threshold, double relinearize_threshold, bool relinearize) {
|
||||
|
||||
|
|
@ -463,9 +490,10 @@ namespace gtsam {
|
|||
|
||||
tic("step3");
|
||||
// 3. Linearize new factor
|
||||
boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
|
||||
// boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
|
||||
toc("step3");
|
||||
|
||||
#if 0 // original algorithm
|
||||
tic("step4");
|
||||
// 4. Linear iSAM step (alg 3)
|
||||
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
|
||||
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");
|
||||
|
||||
tictoc_print();
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -25,12 +25,12 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
typedef SymbolMap<GaussianFactor::shared_ptr> CachedFactors;
|
||||
typedef SymbolMap<GaussianFactor::shared_ptr> CachedFactors;
|
||||
|
||||
template<class Conditional, class Config>
|
||||
class ISAM2: public BayesTree<Conditional> {
|
||||
template<class Conditional, class Config>
|
||||
class ISAM2: public BayesTree<Conditional> {
|
||||
|
||||
protected:
|
||||
protected:
|
||||
|
||||
// current linearization point
|
||||
Config theta_;
|
||||
|
|
@ -44,7 +44,7 @@ namespace gtsam {
|
|||
// cached intermediate results for restarting computation in the middle
|
||||
CachedFactors cached_;
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
ISAM2();
|
||||
|
|
@ -84,16 +84,17 @@ namespace gtsam {
|
|||
size_t lastNonlinearAffectedVariableCount;
|
||||
size_t lastNonlinearAffectedFactorCount;
|
||||
|
||||
private:
|
||||
private:
|
||||
|
||||
std::list<size_t> getAffectedFactors(const std::list<Symbol>& keys) const;
|
||||
boost::shared_ptr<GaussianFactorGraph> relinearizeAffectedFactors(const std::set<Symbol>& affectedKeys) const;
|
||||
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 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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
// parents are assumed to already be solved and available in result
|
||||
GaussianISAM2::Clique::const_reverse_iterator it;
|
||||
|
|
|
|||
Loading…
Reference in New Issue