bug fixes after COLAMD activiation: BayesTree::insert was wrong, BayesTree::removeTop/Path fixed and simplified
Michael Kaess
parent
66607897a5
commit
d2291a38d1
|
|
@ -264,11 +264,11 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
void BayesTree<Conditional>::insert(const sharedConditional& conditional)
|
||||
void BayesTree<Conditional>::insert(const sharedConditional& conditional, const list<Symbol>* ordering)
|
||||
{
|
||||
// get key and parents
|
||||
const Symbol& key = conditional->key();
|
||||
list<Symbol> parents = conditional->parents(); // rtodo: const reference?
|
||||
list<Symbol> parents = conditional->parents(); // todo: const reference?
|
||||
|
||||
// if no parents, start a new root clique
|
||||
if (parents.empty()) {
|
||||
|
|
@ -277,7 +277,18 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
// otherwise, find the parent clique
|
||||
Symbol parent = parents.front();
|
||||
Symbol parent;
|
||||
if (!ordering) {
|
||||
parent = parents.front(); // assumes parents are in current variable order, which is not the case (after COLAMD was activated)
|
||||
} else {
|
||||
for (list<Symbol>::const_iterator it = ordering->begin(); it!=ordering->end(); it++) {
|
||||
list<Symbol>::iterator pit = find(parents.begin(), parents.end(), *it);
|
||||
if (pit!=parents.end()) {
|
||||
parent = *pit;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
sharedClique parent_clique = (*this)[parent];
|
||||
|
||||
// if the parents and parent clique have the same size, add to parent clique
|
||||
|
|
@ -370,20 +381,25 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
|
||||
BayesTree<Conditional>::removePath(sharedClique clique) {
|
||||
|
||||
FactorGraph<Factor> factors;
|
||||
Cliques orphans;
|
||||
void BayesTree<Conditional>::removePath(sharedClique clique,
|
||||
FactorGraph<Factor> &factors, 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);
|
||||
|
||||
// remove me
|
||||
this->removeClique(clique);
|
||||
|
||||
// remove path above me
|
||||
boost::tie(factors,orphans) = this->removePath<Factor>(clique->parent_);
|
||||
this->removePath<Factor>(clique->parent_, factors, orphans);
|
||||
|
||||
// add children to list of orphans (splice also removed them from clique->children_)
|
||||
orphans.splice (orphans.begin(), clique->children_);
|
||||
|
|
@ -394,31 +410,30 @@ namespace gtsam {
|
|||
// 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
|
||||
|
||||
return make_pair(factors,orphans);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void BayesTree<Conditional>::removeTop(const list<Symbol>& keys,
|
||||
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
|
||||
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
|
||||
|
||||
// process each key of the new factor
|
||||
BOOST_FOREACH(const Symbol& key, keys)
|
||||
try {
|
||||
// get the clique and remove it from orphans (if it exists)
|
||||
// get the clique
|
||||
sharedClique clique = (*this)[key];
|
||||
orphans.remove(clique);
|
||||
|
||||
// remove path from clique to root
|
||||
FactorGraph<Factor> factors1; Cliques orphans1;
|
||||
boost::tie(factors1,orphans1) = this->removePath<Factor>(clique);
|
||||
|
||||
// add to global factors and orphans
|
||||
factors.push_back(factors1);
|
||||
orphans.splice (orphans.begin(), orphans1);
|
||||
this->removePath<Factor>(clique, factors, orphans);
|
||||
|
||||
} catch (std::invalid_argument e) {
|
||||
}
|
||||
|
|
|
|||
|
|
@ -129,7 +129,7 @@ namespace gtsam {
|
|||
bool equals(const BayesTree<Conditional>& other, double tol = 1e-9) const;
|
||||
|
||||
/** insert a new conditional */
|
||||
void insert(const sharedConditional& conditional);
|
||||
void insert(const sharedConditional& conditional, const std::list<Symbol>* ordering = NULL);
|
||||
|
||||
/** number of cliques */
|
||||
inline size_t size() const {
|
||||
|
|
@ -171,7 +171,7 @@ namespace gtsam {
|
|||
* plus a list of orphaned subtree roots. Used in removeTop below.
|
||||
*/
|
||||
template<class Factor>
|
||||
std::pair<FactorGraph<Factor>, Cliques> removePath(sharedClique clique);
|
||||
void removePath(sharedClique clique, FactorGraph<Factor> &factors, Cliques& orphans);
|
||||
|
||||
/**
|
||||
* Given a list of keys, turn "contaminated" part of the tree back into a factor graph.
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ namespace gtsam {
|
|||
boost::shared_ptr<GaussianConditional> conditional;
|
||||
boost::tie(conditional, factor) = joint_factor->eliminate(key);
|
||||
|
||||
// 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;
|
||||
|
||||
// add new factor on separator back into the graph
|
||||
|
|
@ -119,12 +119,14 @@ namespace gtsam {
|
|||
void ISAM2<Conditional, Config>::update_internal(const NonlinearFactorGraph<Config>& newFactors,
|
||||
const Config& config, Cliques& orphans) {
|
||||
|
||||
|
||||
#if 1
|
||||
// determine which variables to relinearize
|
||||
|
||||
FactorGraph<GaussianFactor> affectedFactors;
|
||||
list<Symbol> newFactorsKeys = newFactors.keys();
|
||||
|
||||
#if 1
|
||||
#if 0
|
||||
|
||||
// relinearize all keys that are in newFactors, and already exist (not new variables!)
|
||||
list<Symbol> keysToRelinearize;
|
||||
|
|
@ -155,6 +157,15 @@ namespace gtsam {
|
|||
list<Symbol> keysToBeRemoved = nonlinearFactors_.keys();
|
||||
#endif
|
||||
|
||||
#if 0
|
||||
printf("original tree\n");
|
||||
this->print();
|
||||
printf("ToBeRemoved\n");
|
||||
BOOST_FOREACH(string key, keysToBeRemoved) {
|
||||
printf("%s ", key.c_str());
|
||||
}
|
||||
#endif
|
||||
|
||||
// remove affected factors
|
||||
this->removeTop(keysToBeRemoved, affectedFactors, orphans);
|
||||
|
||||
|
|
@ -170,9 +181,23 @@ namespace gtsam {
|
|||
list<Symbol> affectedKeys = affectedFactors.keys();
|
||||
FactorGraph<GaussianFactor> factors = relinearizeAffectedFactors(affectedKeys); // todo: searches through all factors, potentially expensive
|
||||
|
||||
#if 0
|
||||
printf("affected factors:\n");
|
||||
factors.print();
|
||||
#endif
|
||||
|
||||
// ... 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);
|
||||
|
|
@ -198,8 +223,16 @@ namespace gtsam {
|
|||
|
||||
// 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);
|
||||
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit ) {
|
||||
this->insert(*rit, &ordering);
|
||||
}
|
||||
|
||||
#if 0
|
||||
printf("new factors\n");
|
||||
newFactors.print();
|
||||
printf("orphans:\n");
|
||||
orphans.print();
|
||||
#endif
|
||||
|
||||
// add orphans to the bottom of the new tree
|
||||
BOOST_FOREACH(sharedClique orphan, orphans) {
|
||||
|
|
@ -211,6 +244,8 @@ namespace gtsam {
|
|||
orphan->parent_ = parent; // set new parent!
|
||||
}
|
||||
|
||||
// this->print();
|
||||
|
||||
// update solution - todo: potentially expensive
|
||||
delta_ = optimize2(*this);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -145,7 +145,7 @@ TEST( BayesTree, removePath )
|
|||
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["C"]);
|
||||
bayesTree.removePath<SymbolicFactor>(bayesTree["C"], factors, orphans);
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
|
||||
|
|
@ -155,9 +155,11 @@ TEST( BayesTree, removePath )
|
|||
SymbolicBayesTree::Cliques expectedOrphans2;
|
||||
expectedOrphans2 += bayesTree["F"];
|
||||
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["E"]);
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors));
|
||||
CHECK(assert_equal(expectedOrphans2, orphans));
|
||||
FactorGraph<SymbolicFactor> factors2;
|
||||
SymbolicBayesTree::Cliques orphans2;
|
||||
bayesTree.removePath<SymbolicFactor>(bayesTree["E"], factors2, orphans2);
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
|
||||
CHECK(assert_equal(expectedOrphans2, orphans2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -168,7 +170,7 @@ TEST( BayesTree, removePath2 )
|
|||
// Call remove-path with clique B
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["B"]);
|
||||
bayesTree.removePath<SymbolicFactor>(bayesTree["B"], factors, orphans);
|
||||
|
||||
// Check expected outcome
|
||||
SymbolicFactorGraph expected;
|
||||
|
|
@ -189,7 +191,7 @@ TEST( BayesTree, removePath3 )
|
|||
// Call remove-path with clique S
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["S"]);
|
||||
bayesTree.removePath<SymbolicFactor>(bayesTree["S"], factors, orphans);
|
||||
|
||||
// Check expected outcome
|
||||
SymbolicFactorGraph expected;
|
||||
|
|
@ -238,7 +240,6 @@ TEST( BayesTree, removeTop )
|
|||
CHECK(assert_equal(expectedOrphans2, orphans2));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, removeTop2 )
|
||||
{
|
||||
|
|
@ -267,6 +268,32 @@ TEST( BayesTree, removeTop2 )
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, removeTop3 )
|
||||
{
|
||||
// simple test case that failed after COLAMD was fixed/activated
|
||||
SymbolicConditional::shared_ptr
|
||||
X(new SymbolicConditional("l5")),
|
||||
A(new SymbolicConditional("x4", "l5")),
|
||||
B(new SymbolicConditional("x3", "x4")),
|
||||
C(new SymbolicConditional("x2", "x3"));
|
||||
|
||||
SymbolicBayesTree bayesTree;
|
||||
bayesTree.insert(X);
|
||||
bayesTree.insert(A);
|
||||
bayesTree.insert(B);
|
||||
bayesTree.insert(C);
|
||||
|
||||
// remove all
|
||||
list<Symbol> keys;
|
||||
keys += "l5", "x2", "x3", "x4";
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
bayesTree.removeTop<SymbolicFactor>(keys, factors, orphans);
|
||||
|
||||
CHECK(orphans.size() == 0);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
|
|
|
|||
Loading…
Reference in New Issue