removeTop/Path now returns a BayesNet to avoid converting the conditionals to a factor graph
Michael Kaess
parent
eb03f78e7d
commit
188561d925
|
|
@ -397,18 +397,12 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void BayesTree<Conditional>::removePath(sharedClique clique,
|
||||
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
|
||||
BayesNet<Conditional>& bn, typename BayesTree<Conditional>::Cliques& orphans) {
|
||||
|
||||
// base case is NULL, if so we do nothing and return empties above
|
||||
if (clique!=NULL) {
|
||||
|
||||
#if 0
|
||||
printf("++++++ removing\n");
|
||||
clique->print();
|
||||
#endif
|
||||
|
||||
// remove the clique from orphans in case it has been added earlier
|
||||
orphans.remove(clique);
|
||||
|
||||
|
|
@ -416,32 +410,20 @@ namespace gtsam {
|
|||
this->removeClique(clique);
|
||||
|
||||
// remove path above me
|
||||
this->removePath<Factor>(clique->parent_, factors, orphans);
|
||||
this->removePath(clique->parent_, bn, orphans);
|
||||
|
||||
// add children to list of orphans (splice also removed them from clique->children_)
|
||||
orphans.splice (orphans.begin(), clique->children_);
|
||||
|
||||
// Convert clique to a factor graph, using constructor in FactorGraph
|
||||
FactorGraph<Factor> clique_factors(*clique);
|
||||
|
||||
// add to the list of "invalidated" factors
|
||||
factors.push_back(clique_factors);
|
||||
|
||||
#if 0
|
||||
printf("++++++ factors\n");
|
||||
factors.print();
|
||||
printf("++++++ orphans\n");
|
||||
orphans.print();
|
||||
#endif
|
||||
bn.push_back(*clique);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void BayesTree<Conditional>::removeTop(const list<Symbol>& keys,
|
||||
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
|
||||
BayesNet<Conditional>& bn, typename BayesTree<Conditional>::Cliques& orphans) {
|
||||
|
||||
// process each key of the new factor
|
||||
BOOST_FOREACH(const Symbol& key, keys)
|
||||
|
|
@ -450,7 +432,7 @@ namespace gtsam {
|
|||
sharedClique clique = (*this)[key];
|
||||
|
||||
// remove path from clique to root
|
||||
this->removePath<Factor>(clique, factors, orphans);
|
||||
this->removePath(clique, bn, orphans);
|
||||
|
||||
} catch (std::invalid_argument e) {
|
||||
}
|
||||
|
|
|
|||
|
|
@ -181,16 +181,13 @@ namespace gtsam {
|
|||
* Remove path from clique to root and return that path as factors
|
||||
* plus a list of orphaned subtree roots. Used in removeTop below.
|
||||
*/
|
||||
template<class Factor>
|
||||
void removePath(sharedClique clique, FactorGraph<Factor> &factors, Cliques& orphans);
|
||||
void removePath(sharedClique clique, BayesNet<Conditional>& bn, Cliques& orphans);
|
||||
|
||||
/**
|
||||
* Given a list of keys, turn "contaminated" part of the tree back into a factor graph.
|
||||
* Factors and orphans are added to the in/out arguments.
|
||||
*/
|
||||
template<class Factor>
|
||||
void removeTop(const std::list<Symbol>& keys,
|
||||
FactorGraph<Factor> &factors, Cliques& orphans);
|
||||
void removeTop(const std::list<Symbol>& keys, BayesNet<Conditional>& bn, Cliques& orphans);
|
||||
|
||||
}; // BayesTree
|
||||
|
||||
|
|
|
|||
|
|
@ -31,8 +31,9 @@ namespace gtsam {
|
|||
void ISAM<Conditional>::update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans) {
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<Factor> factors;
|
||||
removeTop(newFactors.keys(), factors, orphans);
|
||||
BayesNet<Conditional> bn;
|
||||
removeTop(newFactors.keys(), bn, orphans);
|
||||
FactorGraph<Factor> factors(bn);
|
||||
|
||||
// add the factors themselves
|
||||
factors.push_back(newFactors);
|
||||
|
|
|
|||
|
|
@ -137,9 +137,6 @@ namespace gtsam {
|
|||
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
|
||||
const Config& config, Cliques& orphans) {
|
||||
|
||||
#if 1 // 0=skip most, do batch
|
||||
|
||||
FactorGraph<GaussianFactor> affectedFactors;
|
||||
list<Symbol> newFactorsKeys = newFactors.keys();
|
||||
|
||||
#if 1 // 0=relinearize all in each step
|
||||
|
|
@ -163,7 +160,8 @@ namespace gtsam {
|
|||
#endif
|
||||
|
||||
// remove affected factors
|
||||
this->removeTop(keysToBeRemoved, affectedFactors, orphans);
|
||||
BayesNet<GaussianConditional> affectedBayesNet;
|
||||
this->removeTop(keysToBeRemoved, affectedBayesNet, orphans);
|
||||
|
||||
// selectively update the linearization point
|
||||
VectorConfig selected_delta;
|
||||
|
|
@ -174,21 +172,12 @@ namespace gtsam {
|
|||
linPoint_ = expmap(linPoint_, selected_delta);
|
||||
|
||||
// relinearize the affected factors ...
|
||||
list<Symbol> affectedKeys = affectedFactors.keys();
|
||||
list<Symbol> affectedKeys = affectedBayesNet.ordering(); // all keys in conditionals, there cannot be others because path to root included
|
||||
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys);
|
||||
|
||||
// ... add the cached intermediate results from the boundary of the orphans ...
|
||||
FactorGraph<GaussianFactor> cachedBoundary = getCachedBoundaryFactors(orphans);
|
||||
factors.push_back(cachedBoundary);
|
||||
#else
|
||||
// todo - debug only: batch operation
|
||||
FactorGraph<GaussianFactor> affectedFactors;
|
||||
list<Symbol> keysToBeRemoved = nonlinearFactors_.keys();
|
||||
this->removeTop(keysToBeRemoved, affectedFactors, orphans);
|
||||
this->print("---------------");
|
||||
linPoint_ = expmap(linPoint_, delta_); // todo-debug only
|
||||
FactorGraph<GaussianFactor> factors = nonlinearFactors_.linearize(linPoint_);
|
||||
#endif
|
||||
|
||||
// add new variables
|
||||
linPoint_.insert(config);
|
||||
|
|
|
|||
|
|
@ -155,9 +155,10 @@ TEST( BayesTree, removePath )
|
|||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["D"], bayesTree["E"];
|
||||
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
BayesNet<SymbolicConditional> bn;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
bayesTree.removePath<SymbolicFactor>(bayesTree["C"], factors, orphans);
|
||||
bayesTree.removePath(bayesTree["C"], bn, orphans);
|
||||
FactorGraph<SymbolicFactor> factors(bn);
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
|
||||
|
|
@ -167,9 +168,10 @@ TEST( BayesTree, removePath )
|
|||
SymbolicBayesTree::Cliques expectedOrphans2;
|
||||
expectedOrphans2 += bayesTree["F"];
|
||||
|
||||
FactorGraph<SymbolicFactor> factors2;
|
||||
BayesNet<SymbolicConditional> bn2;
|
||||
SymbolicBayesTree::Cliques orphans2;
|
||||
bayesTree.removePath<SymbolicFactor>(bayesTree["E"], factors2, orphans2);
|
||||
bayesTree.removePath(bayesTree["E"], bn2, orphans2);
|
||||
FactorGraph<SymbolicFactor> factors2(bn2);
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
|
||||
CHECK(assert_equal(expectedOrphans2, orphans2));
|
||||
}
|
||||
|
|
@ -180,9 +182,10 @@ TEST( BayesTree, removePath2 )
|
|||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// Call remove-path with clique B
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
BayesNet<SymbolicConditional> bn;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
bayesTree.removePath<SymbolicFactor>(bayesTree["B"], factors, orphans);
|
||||
bayesTree.removePath(bayesTree["B"], bn, orphans);
|
||||
FactorGraph<SymbolicFactor> factors(bn);
|
||||
|
||||
// Check expected outcome
|
||||
SymbolicFactorGraph expected;
|
||||
|
|
@ -201,9 +204,10 @@ TEST( BayesTree, removePath3 )
|
|||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// Call remove-path with clique S
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
BayesNet<SymbolicConditional> bn;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
bayesTree.removePath<SymbolicFactor>(bayesTree["S"], factors, orphans);
|
||||
bayesTree.removePath(bayesTree["S"], bn, orphans);
|
||||
FactorGraph<SymbolicFactor> factors(bn);
|
||||
|
||||
// Check expected outcome
|
||||
SymbolicFactorGraph expected;
|
||||
|
|
@ -226,9 +230,10 @@ TEST( BayesTree, removeTop )
|
|||
boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor("B","S"));
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
BayesNet<SymbolicConditional> bn;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
bayesTree.removeTop<SymbolicFactor>(newFactor->keys(), factors, orphans);
|
||||
bayesTree.removeTop(newFactor->keys(), bn, orphans);
|
||||
FactorGraph<SymbolicFactor> factors(bn);
|
||||
|
||||
// Check expected outcome
|
||||
SymbolicFactorGraph expected;
|
||||
|
|
@ -243,9 +248,10 @@ TEST( BayesTree, removeTop )
|
|||
|
||||
// Try removeTop again with a factor that should not change a thing
|
||||
boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor("B"));
|
||||
FactorGraph<SymbolicFactor> factors2;
|
||||
BayesNet<SymbolicConditional> bn2;
|
||||
SymbolicBayesTree::Cliques orphans2;
|
||||
bayesTree.removeTop<SymbolicFactor>(newFactor2->keys(), factors2, orphans2);
|
||||
bayesTree.removeTop(newFactor2->keys(), bn2, orphans2);
|
||||
FactorGraph<SymbolicFactor> factors2(bn2);
|
||||
SymbolicFactorGraph expected2;
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
|
||||
SymbolicBayesTree::Cliques expectedOrphans2;
|
||||
|
|
@ -263,9 +269,10 @@ TEST( BayesTree, removeTop2 )
|
|||
newFactors.push_factor("S");
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
BayesNet<SymbolicConditional> bn;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
bayesTree.removeTop<SymbolicFactor>(newFactors.keys(), factors, orphans);
|
||||
bayesTree.removeTop(newFactors.keys(), bn, orphans);
|
||||
FactorGraph<SymbolicFactor> factors(bn);
|
||||
|
||||
// Check expected outcome
|
||||
SymbolicFactorGraph expected;
|
||||
|
|
@ -298,9 +305,10 @@ TEST( BayesTree, removeTop3 )
|
|||
// remove all
|
||||
list<Symbol> keys;
|
||||
keys += "l5", "x2", "x3", "x4";
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
BayesNet<SymbolicConditional> bn;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
bayesTree.removeTop<SymbolicFactor>(keys, factors, orphans);
|
||||
bayesTree.removeTop(keys, bn, orphans);
|
||||
FactorGraph<SymbolicFactor> factors(bn);
|
||||
|
||||
CHECK(orphans.size() == 0);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue