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();
|
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,225 +91,360 @@ 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
|
||||||
tic("eliminate_removeandcombinefactors");
|
tic("eliminate_removeandcombinefactors");
|
||||||
boost::shared_ptr<GaussianFactor> joint_factor = removeAndCombineFactors(graph,key);
|
boost::shared_ptr<GaussianFactor> joint_factor = removeAndCombineFactors(graph,key);
|
||||||
toc("eliminate_removeandcombinefactors");
|
toc("eliminate_removeandcombinefactors");
|
||||||
|
|
||||||
// eliminate that joint factor
|
// eliminate that joint factor
|
||||||
boost::shared_ptr<GaussianFactor> factor;
|
boost::shared_ptr<GaussianFactor> factor;
|
||||||
boost::shared_ptr<GaussianConditional> conditional;
|
boost::shared_ptr<GaussianConditional> conditional;
|
||||||
tic("eliminate_eliminate");
|
tic("eliminate_eliminate");
|
||||||
boost::tie(conditional, factor) = joint_factor->eliminate(key);
|
boost::tie(conditional, factor) = joint_factor->eliminate(key);
|
||||||
toc("eliminate_eliminate");
|
toc("eliminate_eliminate");
|
||||||
|
|
||||||
// ADDED: remember the intermediate result to be able to later restart computation in the middle
|
// ADDED: remember the intermediate result to be able to later restart computation in the middle
|
||||||
cached[key] = factor;
|
cached[key] = factor;
|
||||||
|
|
||||||
// add new factor on separator back into the graph
|
// add new factor on separator back into the graph
|
||||||
if (!factor->empty()) graph.push_back(factor);
|
if (!factor->empty()) graph.push_back(factor);
|
||||||
|
|
||||||
// return the conditional Gaussian
|
// return the conditional Gaussian
|
||||||
return conditional;
|
return conditional;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
|
||||||
// from GaussianFactorGraph.cpp, see _eliminateOne above
|
// special const version used in constructor below
|
||||||
GaussianBayesNet _eliminate(FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
|
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
|
||||||
GaussianBayesNet chordalBayesNet; // empty
|
// make a copy that can be modified locally
|
||||||
BOOST_FOREACH(const Symbol& key, ordering) {
|
FactorGraph<GaussianFactor> graph_ignored = graph;
|
||||||
GaussianConditional::shared_ptr cg = _eliminateOne(graph, cached, key);
|
return _eliminate(graph_ignored, cached, ordering);
|
||||||
chordalBayesNet.push_back(cg);
|
}
|
||||||
}
|
|
||||||
return chordalBayesNet;
|
/** 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)),
|
||||||
|
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 {
|
||||||
|
FactorGraph<NonlinearFactor<Config> > allAffected;
|
||||||
|
list<size_t> indices;
|
||||||
|
BOOST_FOREACH(const Symbol& key, keys) {
|
||||||
|
const list<size_t> l = nonlinearFactors_.factors(key);
|
||||||
|
indices.insert(indices.begin(), l.begin(), l.end());
|
||||||
}
|
}
|
||||||
|
indices.sort();
|
||||||
|
indices.unique();
|
||||||
|
return indices;
|
||||||
|
}
|
||||||
|
|
||||||
// special const version used in constructor below
|
/* ************************************************************************* */
|
||||||
GaussianBayesNet _eliminate_const(const FactorGraph<GaussianFactor>& graph, CachedFactors& cached, const Ordering& ordering) {
|
// retrieve all factors that ONLY contain the affected variables
|
||||||
// make a copy that can be modified locally
|
// (note that the remaining stuff is summarized in the cached factors)
|
||||||
FactorGraph<GaussianFactor> graph_ignored = graph;
|
template<class Conditional, class Config>
|
||||||
return _eliminate(graph_ignored, cached, ordering);
|
boost::shared_ptr<GaussianFactorGraph> ISAM2<Conditional, Config>::relinearizeAffectedFactors
|
||||||
}
|
(const set<Symbol>& affectedKeys) const {
|
||||||
|
|
||||||
/** Create an empty Bayes Tree */
|
list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list...
|
||||||
template<class Conditional, class Config>
|
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
|
||||||
ISAM2<Conditional, Config>::ISAM2() : BayesTree<Conditional>() {}
|
list<size_t> candidates = getAffectedFactors(affectedKeysList);
|
||||||
|
|
||||||
/** Create a Bayes Tree from a nonlinear factor graph */
|
NonlinearFactorGraph<Config> nonlinearAffectedFactors;
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
BOOST_FOREACH(size_t idx, candidates) {
|
||||||
template<class Conditional, class Config>
|
bool inside = true;
|
||||||
list<size_t> ISAM2<Conditional, Config>::getAffectedFactors(const list<Symbol>& keys) const {
|
BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
|
||||||
FactorGraph<NonlinearFactor<Config> > allAffected;
|
if (affectedKeys.find(key) == affectedKeys.end()) {
|
||||||
list<size_t> indices;
|
inside = false;
|
||||||
BOOST_FOREACH(const Symbol& key, keys) {
|
break;
|
||||||
const list<size_t> l = nonlinearFactors_.factors(key);
|
|
||||||
indices.insert(indices.begin(), l.begin(), l.end());
|
|
||||||
}
|
|
||||||
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 {
|
|
||||||
|
|
||||||
list<Symbol> affectedKeysList; // todo: shouldn't have to convert back to list...
|
|
||||||
affectedKeysList.insert(affectedKeysList.begin(), affectedKeys.begin(), affectedKeys.end());
|
|
||||||
list<size_t> candidates = getAffectedFactors(affectedKeysList);
|
|
||||||
|
|
||||||
NonlinearFactorGraph<Config> nonlinearAffectedFactors;
|
|
||||||
|
|
||||||
BOOST_FOREACH(size_t idx, candidates) {
|
|
||||||
bool inside = true;
|
|
||||||
BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
|
|
||||||
if (affectedKeys.find(key) == affectedKeys.end()) {
|
|
||||||
inside = false;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
if (inside)
|
|
||||||
nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
|
|
||||||
}
|
}
|
||||||
|
if (inside)
|
||||||
// TODO: temporary might be expensive, return shared pointer ?
|
nonlinearAffectedFactors.push_back(nonlinearFactors_[idx]);
|
||||||
return nonlinearAffectedFactors.linearize(theta_);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// TODO: temporary might be expensive, return shared pointer ?
|
||||||
// find intermediate (linearized) factors from cache that are passed into the affected area
|
return nonlinearAffectedFactors.linearize(theta_);
|
||||||
template<class Conditional, class Config>
|
}
|
||||||
FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) {
|
|
||||||
FactorGraph<GaussianFactor> cachedBoundary;
|
|
||||||
|
|
||||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
/* ************************************************************************* */
|
||||||
// find the last variable that was eliminated
|
// find intermediate (linearized) factors from cache that are passed into the affected area
|
||||||
const Symbol& key = orphan->ordering().back();
|
template<class Conditional, class Config>
|
||||||
// retrieve the cached factor and add to boundary
|
FactorGraph<GaussianFactor> ISAM2<Conditional, Config>::getCachedBoundaryFactors(Cliques& orphans) {
|
||||||
cachedBoundary.push_back(cached_[key]);
|
FactorGraph<GaussianFactor> cachedBoundary;
|
||||||
}
|
|
||||||
|
|
||||||
return cachedBoundary;
|
BOOST_FOREACH(sharedClique orphan, orphans) {
|
||||||
|
// find the last variable that was eliminated
|
||||||
|
const Symbol& key = orphan->ordering().back();
|
||||||
|
// retrieve the cached factor and add to boundary
|
||||||
|
cachedBoundary.push_back(cached_[key]);
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
return cachedBoundary;
|
||||||
template<class Conditional, class Config>
|
}
|
||||||
void ISAM2<Conditional, Config>::linear_update(const FactorGraph<GaussianFactor>& newFactors) {
|
|
||||||
|
|
||||||
// Input: BayesTree(this), newFactors
|
/* ************************************************************************* */
|
||||||
|
template<class Conditional, class Config>
|
||||||
|
void ISAM2<Conditional, Config>::recalculate(const list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors) {
|
||||||
|
|
||||||
//#define PRINT_STATS // figures for paper, disable for timing
|
// Input: BayesTree(this), newFactors
|
||||||
|
|
||||||
|
//#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;
|
||||||
double avgClique = 0;
|
double avgClique = 0;
|
||||||
int numCliques = 0;
|
int numCliques = 0;
|
||||||
int nnzR = 0;
|
int nnzR = 0;
|
||||||
if (counter>0) { // cannot call on empty tree
|
if (counter>0) { // cannot call on empty tree
|
||||||
GaussianISAM2_P::CliqueData cdata = this->getCliqueData();
|
GaussianISAM2_P::CliqueData cdata = this->getCliqueData();
|
||||||
GaussianISAM2_P::CliqueStats cstats = cdata.getStats();
|
GaussianISAM2_P::CliqueStats cstats = cdata.getStats();
|
||||||
maxClique = cstats.maxConditionalSize;
|
maxClique = cstats.maxConditionalSize;
|
||||||
avgClique = cstats.avgConditionalSize;
|
avgClique = cstats.avgConditionalSize;
|
||||||
numCliques = cdata.conditionalSizes.size();
|
numCliques = cdata.conditionalSizes.size();
|
||||||
nnzR = calculate_nnz(this->root());
|
nnzR = calculate_nnz(this->root());
|
||||||
}
|
}
|
||||||
counter++;
|
counter++;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// 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;
|
||||||
|
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
|
||||||
|
// [all the necessary data is actually contained in the affectedBayesNet, including what was passed in from the boundaries,
|
||||||
|
// so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't
|
||||||
|
// contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose
|
||||||
|
// not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected
|
||||||
|
// in the cached_ values which again will be wrong]
|
||||||
|
// so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary
|
||||||
|
|
||||||
|
// BEGIN OF COPIED CODE
|
||||||
|
|
||||||
|
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();
|
||||||
|
affectedKeys.insert(tmp.begin(), tmp.end());
|
||||||
|
|
||||||
|
FactorGraph<GaussianFactor> factors(*relinearizeAffectedFactors(affectedKeys)); // todo: no need to relinearize here, should have cached linearized factors
|
||||||
|
|
||||||
|
lastAffectedMarkedCount = markedKeys.size();
|
||||||
|
lastAffectedVariableCount = affectedKeys.size();
|
||||||
|
lastAffectedFactorCount = factors.size();
|
||||||
|
|
||||||
|
#ifdef PRINT_STATS
|
||||||
|
// output for generating figures
|
||||||
|
cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size()
|
||||||
|
<< " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique
|
||||||
|
<< " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
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
|
||||||
|
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])
|
||||||
|
|
||||||
|
// create an ordering for the new and contaminated factors
|
||||||
|
// markedKeys are passed in: those variables will be forced to the end in the ordering
|
||||||
|
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
|
||||||
|
|
||||||
|
// eliminate into a Bayes net
|
||||||
|
tic("eliminate");
|
||||||
|
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
|
||||||
|
toc("eliminate");
|
||||||
|
|
||||||
|
tic("re-assemble");
|
||||||
|
// Create Index from ordering
|
||||||
|
IndexTable<Symbol> index(ordering);
|
||||||
|
|
||||||
|
// insert conditionals back in, straight into the topless bayesTree
|
||||||
|
typename BayesNet<Conditional>::const_reverse_iterator rit;
|
||||||
|
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
||||||
|
this->insert(*rit, index);
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
sharedClique parent = (*this)[parentRepresentative];
|
||||||
|
parent->children_ += orphan;
|
||||||
|
orphan->parent_ = parent; // set new parent!
|
||||||
|
}
|
||||||
|
toc("re-orphans");
|
||||||
|
|
||||||
|
// Output: BayesTree(this)
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
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_) {
|
||||||
|
if (find(marked.begin(), marked.end(), key) != marked.end())
|
||||||
|
found = true;
|
||||||
|
}
|
||||||
|
if (found) {
|
||||||
|
// then add this clique
|
||||||
|
keys.push_back(clique->keys().front());
|
||||||
|
}
|
||||||
|
BOOST_FOREACH(const sharedClique& child, clique->children_) {
|
||||||
|
find_all(child, keys, marked);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
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_
|
||||||
|
|
||||||
|
// 1. Mark variables in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
|
||||||
|
list<Symbol> marked;
|
||||||
|
VectorConfig deltaMarked;
|
||||||
|
for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) {
|
||||||
|
Symbol key = it->first;
|
||||||
|
Vector v = it->second;
|
||||||
|
if (max(abs(v)) >= relinearize_threshold) {
|
||||||
|
marked.push_back(key);
|
||||||
|
deltaMarked.insert(key, v);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (marked.size()>0) {
|
||||||
|
|
||||||
|
// 2. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
|
||||||
|
theta_ = theta_.expmap(deltaMarked);
|
||||||
|
|
||||||
|
// 3. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
|
||||||
|
|
||||||
|
// mark all cliques that involve marked variables
|
||||||
|
list<Symbol> affectedSymbols(marked); // add all marked
|
||||||
|
tic("nonlin-find_all");
|
||||||
|
find_all(this->root(), affectedSymbols, marked); // add other cliques that have the marked ones in the separator
|
||||||
|
affectedSymbols.sort(); // remove duplicates
|
||||||
|
affectedSymbols.unique();
|
||||||
|
toc("nonlin-find_all");
|
||||||
|
|
||||||
|
// 4. From the leaves to the top, if a clique is marked:
|
||||||
|
// re-linearize the original factors in \Factors associated with the clique,
|
||||||
|
// 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;
|
Cliques orphans;
|
||||||
BayesNet<GaussianConditional> affectedBayesNet;
|
BayesNet<GaussianConditional> affectedBayesNet;
|
||||||
this->removeTop(markedKeys, affectedBayesNet, orphans);
|
this->removeTop(affectedSymbols, affectedBayesNet, orphans);
|
||||||
|
// remember 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
|
||||||
|
|
||||||
// FactorGraph<GaussianFactor> factors(affectedBayesNet);
|
boost::shared_ptr<GaussianFactorGraph> factors;
|
||||||
// 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,
|
|
||||||
// so this would be correct; however, in the process we also generate new cached_ entries that will be wrong (ie. they don't
|
|
||||||
// contain what would be passed up at a certain point if batch elimination was done, but that's what we need); we could choose
|
|
||||||
// not to update cached_ from here, but then the new information (and potentially different variable ordering) is not reflected
|
|
||||||
// in the cached_ values which again will be wrong]
|
|
||||||
// so instead we have to retrieve the original linearized factors AND add the cached factors from the boundary
|
|
||||||
|
|
||||||
// BEGIN OF COPIED CODE
|
|
||||||
|
|
||||||
tic("linear_lookup1");
|
|
||||||
// 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();
|
||||||
affectedKeys.insert(tmp.begin(), tmp.end());
|
affectedKeys.insert(tmp.begin(), tmp.end());
|
||||||
|
toc("nonlin-mess");
|
||||||
|
|
||||||
FactorGraph<GaussianFactor> factors(*relinearizeAffectedFactors(affectedKeys)); // todo: no need to relinearize here, should have cached linearized factors
|
tic("nonlin_relin");
|
||||||
|
factors = relinearizeAffectedFactors(affectedKeys);
|
||||||
|
toc("nonlin_relin");
|
||||||
|
|
||||||
lastAffectedMarkedCount = markedKeys.size();
|
lastNonlinearMarkedCount = marked.size();
|
||||||
lastAffectedVariableCount = affectedKeys.size();
|
lastNonlinearAffectedVariableCount = affectedKeys.size();
|
||||||
lastAffectedFactorCount = factors.size();
|
lastNonlinearAffectedFactorCount = factors->size();
|
||||||
|
|
||||||
#ifdef PRINT_STATS
|
// cout << "nonlinear: #marked: " << marked.size() << " #affected: " << affectedKeys.size() << " #factors: " << factors->size() << endl;
|
||||||
// output for generating figures
|
|
||||||
cout << "linear: #markedKeys: " << markedKeys.size() << " #affectedVariables: " << affectedKeys.size()
|
|
||||||
<< " #affectedFactors: " << factors.size() << " maxCliqueSize: " << maxClique
|
|
||||||
<< " avgCliqueSize: " << avgClique << " #Cliques: " << numCliques << " nnzR: " << nnzR << endl;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
toc("linear_lookup1");
|
|
||||||
|
|
||||||
// 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);
|
||||||
|
|
||||||
// END OF COPIED CODE
|
|
||||||
|
|
||||||
|
|
||||||
// 2. Add the new factors \Factors' into the resulting factor graph
|
|
||||||
factors.push_back(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])
|
|
||||||
|
|
||||||
// create an ordering for the new and contaminated factors
|
|
||||||
// markedKeys are passed in: those variables will be forced to the end in the ordering
|
|
||||||
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
|
|
||||||
|
|
||||||
// eliminate into a Bayes net
|
// eliminate into a Bayes net
|
||||||
tic("linear_eliminate");
|
tic("nonlin_eliminate");
|
||||||
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
|
BayesNet<Conditional> bayesNet = _eliminate(*factors, cached_, original_ordering);
|
||||||
toc("linear_eliminate");
|
toc("nonlin_eliminate");
|
||||||
|
|
||||||
// Create Index from ordering
|
// Create Index from ordering
|
||||||
IndexTable<Symbol> index(ordering);
|
IndexTable<Symbol> index(original_ordering);
|
||||||
|
|
||||||
// insert conditionals back in, straight into the topless bayesTree
|
// insert conditionals back in, straight into the topless bayesTree
|
||||||
typename BayesNet<Conditional>::const_reverse_iterator rit;
|
typename BayesNet<Conditional>::const_reverse_iterator rit;
|
||||||
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
||||||
this->insert(*rit, index);
|
this->insert(*rit, index);
|
||||||
|
|
||||||
// Save number of affectedCliques
|
|
||||||
lastAffectedCliqueCount = this->size();
|
|
||||||
|
|
||||||
// 4. Insert the orphans back into the new Bayes tree.
|
|
||||||
|
|
||||||
// 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,179 +452,102 @@ namespace gtsam {
|
||||||
parent->children_ += orphan;
|
parent->children_ += orphan;
|
||||||
orphan->parent_ = parent; // set new parent!
|
orphan->parent_ = parent; // set new parent!
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// Output: BayesTree(this)
|
// Output: updated Bayes tree (this), updated linearization point theta_
|
||||||
}
|
}
|
||||||
|
|
||||||
/* ************************************************************************* */
|
list<Symbol> empty;
|
||||||
// find all variables that are directly connected by a measurement to one of the marked variables
|
return empty;
|
||||||
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_) {
|
template<class Conditional, class Config>
|
||||||
if (find(marked.begin(), marked.end(), key) != marked.end())
|
void ISAM2<Conditional, Config>::update(
|
||||||
found = true;
|
const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
|
||||||
}
|
double wildfire_threshold, double relinearize_threshold, bool relinearize) {
|
||||||
if (found) {
|
|
||||||
// then add this clique
|
lastAffectedVariableCount = 0;
|
||||||
keys.push_back(clique->keys().front());
|
lastAffectedFactorCount = 0;
|
||||||
}
|
lastAffectedCliqueCount = 0;
|
||||||
BOOST_FOREACH(const sharedClique& child, clique->children_) {
|
lastAffectedMarkedCount = 0;
|
||||||
find_all(child, keys, marked);
|
lastNonlinearMarkedCount = 0;
|
||||||
}
|
lastNonlinearAffectedVariableCount = 0;
|
||||||
|
lastNonlinearAffectedFactorCount = 0;
|
||||||
|
|
||||||
|
tic("all");
|
||||||
|
|
||||||
|
tic("step1");
|
||||||
|
// 1. Add any new factors \Factors:=\Factors\cup\Factors'.
|
||||||
|
nonlinearFactors_.push_back(newFactors);
|
||||||
|
toc("step1");
|
||||||
|
|
||||||
|
tic("step2");
|
||||||
|
// 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
|
||||||
|
theta_.insert(newTheta);
|
||||||
|
toc("step2");
|
||||||
|
|
||||||
|
tic("step3");
|
||||||
|
// 3. Linearize new factor
|
||||||
|
// 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
|
||||||
|
toc("step4");
|
||||||
|
|
||||||
|
tic("step5");
|
||||||
|
// 5. Calculate Delta (alg 0)
|
||||||
|
delta_ = optimize2(*this, wildfire_threshold);
|
||||||
|
toc("step5");
|
||||||
|
|
||||||
|
tic("step6");
|
||||||
|
// 6. Iterate Algorithm 4 until no more re-linearizations occur
|
||||||
|
if (relinearize) {
|
||||||
|
fluid_relinearization(relinearize_threshold); // in: delta_, theta_, nonlinearFactors_, this
|
||||||
}
|
}
|
||||||
|
toc("step6");
|
||||||
|
|
||||||
/* ************************************************************************* */
|
// todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again
|
||||||
template<class Conditional, class Config>
|
delta_ = optimize2(*this, wildfire_threshold);
|
||||||
void ISAM2<Conditional, Config>::fluid_relinearization(double relinearize_threshold) {
|
#else // new algorithm
|
||||||
|
|
||||||
// Input: nonlinear factors factors_, linearization point theta_, Bayes tree (this), delta_
|
tic("step4B");
|
||||||
|
// 4B. Mark linear update
|
||||||
|
list<Symbol> markedKeys = newFactors.keys();
|
||||||
|
toc("step4B");
|
||||||
|
|
||||||
// 1. Mark variables in \Delta above threshold \beta: J=\{\Delta_{j}\in\Delta|\Delta_{j}\geq\beta\}.
|
tic("step5B");
|
||||||
list<Symbol> marked;
|
// 5B. Mark nonlinear update
|
||||||
VectorConfig deltaMarked;
|
if (relinearize) {
|
||||||
for (VectorConfig::const_iterator it = delta_.begin(); it!=delta_.end(); it++) {
|
list<Symbol> markedRelin = fluid_relinearization(relinearize_threshold); // in: delta_, theta_, nonlinearFactors_, this
|
||||||
Symbol key = it->first;
|
|
||||||
Vector v = it->second;
|
|
||||||
if (max(abs(v)) >= relinearize_threshold) {
|
|
||||||
marked.push_back(key);
|
|
||||||
deltaMarked.insert(key, v);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (marked.size()>0) {
|
// merge with markedKeys
|
||||||
|
markedKeys.splice(markedKeys.begin(), markedRelin, markedRelin.begin(), markedRelin.end());
|
||||||
// 2. Update linearization point for marked variables: \Theta_{J}:=\Theta_{J}+\Delta_{J}.
|
markedKeys.sort(); // remove duplicates
|
||||||
theta_ = theta_.expmap(deltaMarked);
|
markedKeys.unique();
|
||||||
|
|
||||||
// 3. Mark all cliques that involve marked variables \Theta_{J} and all their ancestors.
|
|
||||||
|
|
||||||
// mark all cliques that involve marked variables
|
|
||||||
list<Symbol> affectedSymbols(marked); // add all marked
|
|
||||||
tic("nonlin-find_all");
|
|
||||||
find_all(this->root(), affectedSymbols, marked); // add other cliques that have the marked ones in the separator
|
|
||||||
affectedSymbols.sort(); // remove duplicates
|
|
||||||
affectedSymbols.unique();
|
|
||||||
toc("nonlin-find_all");
|
|
||||||
|
|
||||||
// 4. From the leaves to the top, if a clique is marked:
|
|
||||||
// re-linearize the original factors in \Factors associated with the clique,
|
|
||||||
// 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
|
|
||||||
|
|
||||||
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...
|
|
||||||
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
|
|
||||||
|
|
||||||
boost::shared_ptr<GaussianFactorGraph> factors;
|
|
||||||
|
|
||||||
// ordering provides all keys in conditionals, there cannot be others because path to root included
|
|
||||||
set<Symbol> affectedKeys;
|
|
||||||
list<Symbol> tmp = affectedBayesNet.ordering();
|
|
||||||
affectedKeys.insert(tmp.begin(), tmp.end());
|
|
||||||
toc("nonlin-mess");
|
|
||||||
|
|
||||||
tic("nonlin_relin");
|
|
||||||
factors = relinearizeAffectedFactors(affectedKeys);
|
|
||||||
toc("nonlin_relin");
|
|
||||||
|
|
||||||
lastNonlinearMarkedCount = marked.size();
|
|
||||||
lastNonlinearAffectedVariableCount = affectedKeys.size();
|
|
||||||
lastNonlinearAffectedFactorCount = factors->size();
|
|
||||||
|
|
||||||
// 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);
|
|
||||||
factors->push_back(cachedBoundary);
|
|
||||||
|
|
||||||
// eliminate into a Bayes net
|
|
||||||
tic("nonlin_eliminate");
|
|
||||||
BayesNet<Conditional> bayesNet = _eliminate(*factors, cached_, original_ordering);
|
|
||||||
toc("nonlin_eliminate");
|
|
||||||
|
|
||||||
// Create Index from ordering
|
|
||||||
IndexTable<Symbol> index(original_ordering);
|
|
||||||
|
|
||||||
// insert conditionals back in, straight into the topless bayesTree
|
|
||||||
typename BayesNet<Conditional>::const_reverse_iterator rit;
|
|
||||||
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
|
|
||||||
this->insert(*rit, index);
|
|
||||||
|
|
||||||
// add orphans to the bottom of the new tree
|
|
||||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
|
||||||
Symbol parentRepresentative = findParentClique(orphan->separator_, index);
|
|
||||||
sharedClique parent = (*this)[parentRepresentative];
|
|
||||||
parent->children_ += orphan;
|
|
||||||
orphan->parent_ = parent; // set new parent!
|
|
||||||
}
|
|
||||||
|
|
||||||
// Output: updated Bayes tree (this), updated linearization point theta_
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
toc("step5B");
|
||||||
|
|
||||||
/* ************************************************************************* */
|
tic("step6B");
|
||||||
template<class Conditional, class Config>
|
// 6B. Redo top of Bayes tree
|
||||||
void ISAM2<Conditional, Config>::update(
|
boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
|
||||||
const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
|
recalculate(markedKeys, &(*linearFactors));
|
||||||
double wildfire_threshold, double relinearize_threshold, bool relinearize) {
|
toc("step6B");
|
||||||
|
|
||||||
lastAffectedVariableCount = 0;
|
tic("step7B");
|
||||||
lastAffectedFactorCount = 0;
|
// 7B. Solve
|
||||||
lastAffectedCliqueCount = 0;
|
delta_ = optimize2(*this, wildfire_threshold);
|
||||||
lastAffectedMarkedCount = 0;
|
toc("step7B");
|
||||||
lastNonlinearMarkedCount = 0;
|
|
||||||
lastNonlinearAffectedVariableCount = 0;
|
|
||||||
lastNonlinearAffectedFactorCount = 0;
|
|
||||||
|
|
||||||
tic("all");
|
#endif
|
||||||
|
toc("all");
|
||||||
|
|
||||||
tic("step1");
|
tictoc_print();
|
||||||
// 1. Add any new factors \Factors:=\Factors\cup\Factors'.
|
}
|
||||||
nonlinearFactors_.push_back(newFactors);
|
|
||||||
toc("step1");
|
|
||||||
|
|
||||||
tic("step2");
|
|
||||||
// 2. Initialize any new variables \Theta_{new} and add \Theta:=\Theta\cup\Theta_{new}.
|
|
||||||
theta_.insert(newTheta);
|
|
||||||
toc("step2");
|
|
||||||
|
|
||||||
tic("step3");
|
|
||||||
// 3. Linearize new factor
|
|
||||||
boost::shared_ptr<GaussianFactorGraph> linearFactors = newFactors.linearize(theta_);
|
|
||||||
toc("step3");
|
|
||||||
|
|
||||||
tic("step4");
|
|
||||||
// 4. Linear iSAM step (alg 3)
|
|
||||||
linear_update(*linearFactors); // in: this
|
|
||||||
toc("step4");
|
|
||||||
|
|
||||||
tic("step5");
|
|
||||||
// 5. Calculate Delta (alg 0)
|
|
||||||
delta_ = optimize2(*this, wildfire_threshold);
|
|
||||||
toc("step5");
|
|
||||||
|
|
||||||
tic("step6");
|
|
||||||
// 6. Iterate Algorithm 4 until no more re-linearizations occur
|
|
||||||
if (relinearize) {
|
|
||||||
fluid_relinearization(relinearize_threshold); // in: delta_, theta_, nonlinearFactors_, this
|
|
||||||
}
|
|
||||||
toc("step6");
|
|
||||||
|
|
||||||
// todo: not part of algorithm in paper: linearization point and delta_ do not fit... have to update delta again
|
|
||||||
delta_ = optimize2(*this, wildfire_threshold);
|
|
||||||
|
|
||||||
toc("all");
|
|
||||||
|
|
||||||
tictoc_print();
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -25,75 +25,76 @@
|
||||||
|
|
||||||
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_;
|
||||||
|
|
||||||
// the linear solution, an update to the estimate in theta
|
// the linear solution, an update to the estimate in theta
|
||||||
VectorConfig delta_;
|
VectorConfig delta_;
|
||||||
|
|
||||||
// for keeping all original nonlinear factors
|
// for keeping all original nonlinear factors
|
||||||
NonlinearFactorGraph<Config> nonlinearFactors_;
|
NonlinearFactorGraph<Config> nonlinearFactors_;
|
||||||
|
|
||||||
// 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();
|
||||||
|
|
||||||
/** Create a Bayes Tree from a Bayes Net */
|
/** Create a Bayes Tree from a Bayes Net */
|
||||||
ISAM2(const NonlinearFactorGraph<Config>& fg, const Ordering& ordering, const Config& config);
|
ISAM2(const NonlinearFactorGraph<Config>& fg, const Ordering& ordering, const Config& config);
|
||||||
|
|
||||||
/** Destructor */
|
/** Destructor */
|
||||||
virtual ~ISAM2() {}
|
virtual ~ISAM2() {}
|
||||||
|
|
||||||
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
|
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
|
||||||
|
|
||||||
typedef typename BayesTree<Conditional>::Cliques Cliques;
|
typedef typename BayesTree<Conditional>::Cliques Cliques;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* ISAM2.
|
* ISAM2.
|
||||||
*/
|
*/
|
||||||
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
|
void update(const NonlinearFactorGraph<Config>& newFactors, const Config& newTheta,
|
||||||
double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true);
|
double wildfire_threshold = 0., double relinearize_threshold = 0., bool relinearize = true);
|
||||||
|
|
||||||
// needed to create initial estimates
|
// needed to create initial estimates
|
||||||
const Config getLinearizationPoint() const {return theta_;}
|
const Config getLinearizationPoint() const {return theta_;}
|
||||||
|
|
||||||
// estimate based on incomplete delta (threshold!)
|
// estimate based on incomplete delta (threshold!)
|
||||||
const Config calculateEstimate() const {return theta_.expmap(delta_);}
|
const Config calculateEstimate() const {return theta_.expmap(delta_);}
|
||||||
|
|
||||||
// estimate based on full delta (note that this is based on the current linearization point)
|
// estimate based on full delta (note that this is based on the current linearization point)
|
||||||
const Config calculateBestEstimate() const {return theta_.expmap(optimize2(*this, 0.));}
|
const Config calculateBestEstimate() const {return theta_.expmap(optimize2(*this, 0.));}
|
||||||
|
|
||||||
const NonlinearFactorGraph<Config>& getFactorsUnsafe() const { return nonlinearFactors_; }
|
const NonlinearFactorGraph<Config>& getFactorsUnsafe() const { return nonlinearFactors_; }
|
||||||
|
|
||||||
size_t lastAffectedVariableCount;
|
size_t lastAffectedVariableCount;
|
||||||
size_t lastAffectedFactorCount;
|
size_t lastAffectedFactorCount;
|
||||||
size_t lastAffectedCliqueCount;
|
size_t lastAffectedCliqueCount;
|
||||||
size_t lastAffectedMarkedCount;
|
size_t lastAffectedMarkedCount;
|
||||||
size_t lastNonlinearMarkedCount;
|
size_t lastNonlinearMarkedCount;
|
||||||
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 linear_update(const FactorGraph<GaussianFactor>& newFactors);
|
void recalculate(const std::list<Symbol>& markedKeys, const FactorGraph<GaussianFactor>* newFactors = NULL);
|
||||||
void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function
|
void linear_update(const FactorGraph<GaussianFactor>& newFactors);
|
||||||
void fluid_relinearization(double relinearize_threshold);
|
void find_all(sharedClique clique, std::list<Symbol>& keys, const std::list<Symbol>& marked); // helper function
|
||||||
|
std::list<Symbol> fluid_relinearization(double relinearize_threshold);
|
||||||
|
|
||||||
}; // ISAM2
|
}; // ISAM2
|
||||||
|
|
||||||
} /// namespace gtsam
|
} /// 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) {
|
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;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue