isam functionality separated into ISAM class, still within BayesTree
Michael Kaess
parent
2cb52694b5
commit
4403b51fcd
|
|
@ -237,6 +237,10 @@ namespace gtsam {
|
|||
void BayesTree<Conditional>::print(const string& s) const {
|
||||
cout << s << ": size == " << size() << endl;
|
||||
if (nodes_.empty()) return;
|
||||
if (root_.use_count() == 0) {
|
||||
printf("WARNING: Forest...\n");
|
||||
return;
|
||||
}
|
||||
root_->printTree("");
|
||||
}
|
||||
|
||||
|
|
@ -367,7 +371,7 @@ namespace gtsam {
|
|||
template<class Conditional>
|
||||
template<class Factor>
|
||||
pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
|
||||
BayesTree<Conditional>::removePath(sharedClique clique) {
|
||||
ISAM<Conditional>::removePath(sharedClique clique) {
|
||||
|
||||
FactorGraph<Factor> factors;
|
||||
Cliques orphans;
|
||||
|
|
@ -376,10 +380,10 @@ namespace gtsam {
|
|||
if (clique!=NULL) {
|
||||
|
||||
// remove me
|
||||
removeClique(clique);
|
||||
this->removeClique(clique);
|
||||
|
||||
// remove path above me
|
||||
boost::tie(factors,orphans) = removePath<Factor>(clique->parent_);
|
||||
boost::tie(factors,orphans) = this->removePath<Factor>(clique->parent_);
|
||||
|
||||
// add children to list of orphans (splice also removed them from clique->children_)
|
||||
orphans.splice (orphans.begin(), clique->children_);
|
||||
|
|
@ -387,8 +391,9 @@ namespace gtsam {
|
|||
// Convert clique to a factor graph, using constructor in FactorGraph
|
||||
FactorGraph<Factor> clique_factors(*clique);
|
||||
|
||||
// add to the list of "imvalidated" factors
|
||||
// add to the list of "invalidated" factors
|
||||
factors.push_back(clique_factors);
|
||||
|
||||
}
|
||||
|
||||
return make_pair(factors,orphans);
|
||||
|
|
@ -398,7 +403,7 @@ namespace gtsam {
|
|||
// TODO: add to factors and orphans
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
|
||||
void ISAM<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
|
||||
FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
|
||||
|
||||
// process each key of the new factor
|
||||
|
|
@ -410,11 +415,12 @@ namespace gtsam {
|
|||
|
||||
// remove path from clique to root
|
||||
FactorGraph<Factor> factors1; Cliques orphans1;
|
||||
boost::tie(factors1,orphans1) = removePath<Factor>(clique);
|
||||
boost::tie(factors1,orphans1) = this->removePath<Factor>(clique);
|
||||
|
||||
// add to global factors and orphans
|
||||
factors.push_back(factors1);
|
||||
orphans.splice (orphans.begin(), orphans1);
|
||||
|
||||
} catch (std::invalid_argument e) {
|
||||
}
|
||||
}
|
||||
|
|
@ -423,11 +429,11 @@ namespace gtsam {
|
|||
template<class Conditional>
|
||||
template<class Factor>
|
||||
pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
|
||||
BayesTree<Conditional>::removeTop(const FactorGraph<Factor>& newFactors) {
|
||||
|
||||
ISAM<Conditional>::removeTop(const FactorGraph<Factor>& newFactors) {
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<Factor> factors;
|
||||
Cliques orphans;
|
||||
|
||||
BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors)
|
||||
this->removeTop<Factor>(factor, factors, orphans);
|
||||
|
||||
|
|
@ -437,17 +443,24 @@ namespace gtsam {
|
|||
/* ************************************************************************* */
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void BayesTree<Conditional>::update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans) {
|
||||
void ISAM<Conditional>::update_internal(const FactorGraph<Factor>& newFactors, Cliques& orphans) {
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<Factor> factors;
|
||||
boost::tie(factors, orphans) = removeTop<Factor>(newFactors);
|
||||
boost::tie(factors, orphans) = this->removeTop<Factor>(newFactors);
|
||||
|
||||
// add the factors themselves
|
||||
factors.push_back(newFactors);
|
||||
|
||||
// create an ordering for the new and contaminated factors
|
||||
Ordering ordering = factors.getOrdering();
|
||||
Ordering ordering;
|
||||
if (true) {
|
||||
ordering = factors.getOrdering();
|
||||
} else {
|
||||
list<string> keys = factors.keys();
|
||||
keys.sort(); // todo: correct sorting order?
|
||||
ordering = keys;
|
||||
}
|
||||
|
||||
// eliminate into a Bayes net
|
||||
BayesNet<Conditional> bayesNet = eliminate<Factor, Conditional>(factors,ordering);
|
||||
|
|
@ -455,7 +468,7 @@ 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 )
|
||||
insert(*rit);
|
||||
this->insert(*rit);
|
||||
|
||||
int count = 0;
|
||||
// add orphans to the bottom of the new tree
|
||||
|
|
@ -472,9 +485,9 @@ namespace gtsam {
|
|||
|
||||
template<class Conditional>
|
||||
template<class Factor>
|
||||
void BayesTree<Conditional>::update(const FactorGraph<Factor>& newFactors) {
|
||||
void ISAM<Conditional>::update(const FactorGraph<Factor>& newFactors) {
|
||||
Cliques orphans;
|
||||
update_internal(newFactors, orphans);
|
||||
this->update_internal<Factor>(newFactors, orphans);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -104,6 +104,8 @@ namespace gtsam {
|
|||
sharedClique addClique(const sharedConditional& conditional,
|
||||
sharedClique parent_clique = sharedClique());
|
||||
|
||||
protected:
|
||||
|
||||
/** remove a clique: warning, can result in a forest */
|
||||
void removeClique(sharedClique clique);
|
||||
|
||||
|
|
@ -163,6 +165,27 @@ namespace gtsam {
|
|||
template<class Factor>
|
||||
BayesNet<Conditional> jointBayesNet(const std::string& key1, const std::string& key2) const;
|
||||
|
||||
}; // BayesTree
|
||||
|
||||
template<class Conditional>
|
||||
class ISAM: public BayesTree<Conditional> {
|
||||
|
||||
public:
|
||||
|
||||
/** Create an empty Bayes Tree */
|
||||
ISAM() : BayesTree<Conditional>() {}
|
||||
|
||||
/** Create a Bayes Tree from a Bayes Net */
|
||||
ISAM(const BayesNet<Conditional>& bayesNet) : BayesTree<Conditional>(bayesNet) {}
|
||||
|
||||
/** Destructor */
|
||||
virtual ~ISAM() {
|
||||
}
|
||||
|
||||
typedef typename BayesTree<Conditional>::sharedClique sharedClique;
|
||||
|
||||
typedef typename BayesTree<Conditional>::Cliques Cliques;
|
||||
|
||||
/**
|
||||
* Remove path from clique to root and return that path as factors
|
||||
* plus a list of orphaned subtree roots. Used in removeTop below.
|
||||
|
|
@ -195,6 +218,6 @@ namespace gtsam {
|
|||
template<class Factor>
|
||||
void update(const FactorGraph<Factor>& newFactors);
|
||||
|
||||
}; // BayesTree
|
||||
}; // ISAM
|
||||
|
||||
} /// namespace gtsam
|
||||
|
|
|
|||
|
|
@ -20,6 +20,10 @@ using namespace boost::assign;
|
|||
using namespace gtsam;
|
||||
|
||||
typedef BayesTree<SymbolicConditional> SymbolicBayesTree;
|
||||
typedef ISAM<SymbolicConditional> SymbolicISAM;
|
||||
|
||||
//template class BayesTree<SymbolicConditional>; // todo: needed?
|
||||
//template class ISAM<SymbolicConditional>;
|
||||
|
||||
/* ************************************************************************* */
|
||||
// SLAM example from RSS sqrtSAM paper
|
||||
|
|
@ -30,9 +34,9 @@ SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
|
|||
l2(new SymbolicConditional("l2","x1","x3"));
|
||||
|
||||
// Bayes Tree for sqrtSAM example
|
||||
SymbolicBayesTree createSlamSymbolicBayesTree(){
|
||||
SymbolicISAM createSlamSymbolicBayesTree(){
|
||||
// Create using insert
|
||||
SymbolicBayesTree bayesTree_slam;
|
||||
SymbolicISAM bayesTree_slam;
|
||||
bayesTree_slam.insert(x3);
|
||||
bayesTree_slam.insert(x2);
|
||||
bayesTree_slam.insert(x1);
|
||||
|
|
@ -54,8 +58,8 @@ SymbolicConditional::shared_ptr
|
|||
X(new SymbolicConditional("X", "E"));
|
||||
|
||||
// Bayes Tree for Asia example
|
||||
SymbolicBayesTree createAsiaSymbolicBayesTree() {
|
||||
SymbolicBayesTree bayesTree;
|
||||
SymbolicISAM createAsiaSymbolicBayesTree() {
|
||||
SymbolicISAM bayesTree;
|
||||
bayesTree.insert(B);
|
||||
bayesTree.insert(L);
|
||||
bayesTree.insert(E);
|
||||
|
|
@ -124,7 +128,7 @@ TEST( BayesTree, removePath )
|
|||
D(new SymbolicConditional("D", "C")),
|
||||
E(new SymbolicConditional("E", "B")),
|
||||
F(new SymbolicConditional("F", "E"));
|
||||
SymbolicBayesTree bayesTree;
|
||||
SymbolicISAM bayesTree;
|
||||
bayesTree.insert(A);
|
||||
bayesTree.insert(B);
|
||||
bayesTree.insert(C);
|
||||
|
|
@ -138,11 +142,11 @@ TEST( BayesTree, removePath )
|
|||
expected.push_factor("A","B");
|
||||
expected.push_factor("A");
|
||||
expected.push_factor("A","C");
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["D"], bayesTree["E"];
|
||||
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["C"]);
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
|
|
@ -150,7 +154,7 @@ TEST( BayesTree, removePath )
|
|||
// remove E: factor graph with EB; E|B removed from second orphan tree
|
||||
SymbolicFactorGraph expected2;
|
||||
expected2.push_factor("B","E");
|
||||
SymbolicBayesTree::Cliques expectedOrphans2;
|
||||
SymbolicISAM::Cliques expectedOrphans2;
|
||||
expectedOrphans2 += bayesTree["F"];
|
||||
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["E"]);
|
||||
|
|
@ -161,11 +165,11 @@ TEST( BayesTree, removePath )
|
|||
/* ************************************************************************* */
|
||||
TEST( BayesTree, removePath2 )
|
||||
{
|
||||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// Call remove-path with clique B
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["B"]);
|
||||
|
||||
// Check expected outcome
|
||||
|
|
@ -174,7 +178,7 @@ TEST( BayesTree, removePath2 )
|
|||
expected.push_factor("B","L");
|
||||
expected.push_factor("B");
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["S"], bayesTree["T"], bayesTree["X"];
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
}
|
||||
|
|
@ -182,11 +186,11 @@ TEST( BayesTree, removePath2 )
|
|||
/* ************************************************************************* */
|
||||
TEST( BayesTree, removePath3 )
|
||||
{
|
||||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// Call remove-path with clique S
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["S"]);
|
||||
|
||||
// Check expected outcome
|
||||
|
|
@ -196,22 +200,22 @@ TEST( BayesTree, removePath3 )
|
|||
expected.push_factor("B");
|
||||
expected.push_factor("L","B","S");
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["T"], bayesTree["X"];
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, removeTop )
|
||||
TEST( ISAM, removeTop )
|
||||
{
|
||||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// create a new factor to be inserted
|
||||
boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor("B","S"));
|
||||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
bayesTree.removeTop<SymbolicFactor>(newFactor, factors, orphans);
|
||||
|
||||
// Check expected outcome
|
||||
|
|
@ -221,26 +225,26 @@ TEST( BayesTree, removeTop )
|
|||
expected.push_factor("B");
|
||||
expected.push_factor("L","B","S");
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["T"], bayesTree["X"];
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
|
||||
// Try removeTop again with a factor that should not change a thing
|
||||
boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor("B"));
|
||||
FactorGraph<SymbolicFactor> factors2;
|
||||
SymbolicBayesTree::Cliques orphans2;
|
||||
SymbolicISAM::Cliques orphans2;
|
||||
bayesTree.removeTop<SymbolicFactor>(newFactor2, factors2, orphans2);
|
||||
SymbolicFactorGraph expected2;
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
|
||||
SymbolicBayesTree::Cliques expectedOrphans2;
|
||||
SymbolicISAM::Cliques expectedOrphans2;
|
||||
CHECK(assert_equal(expectedOrphans2, orphans2));
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, removeTop2 )
|
||||
TEST( ISAM, removeTop2 )
|
||||
{
|
||||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// create two factors to be inserted
|
||||
SymbolicFactorGraph newFactors;
|
||||
|
|
@ -249,7 +253,7 @@ TEST( BayesTree, removeTop2 )
|
|||
|
||||
// Remove the contaminated part of the Bayes tree
|
||||
FactorGraph<SymbolicFactor> factors;
|
||||
SymbolicBayesTree::Cliques orphans;
|
||||
SymbolicISAM::Cliques orphans;
|
||||
boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactors);
|
||||
|
||||
// Check expected outcome
|
||||
|
|
@ -259,15 +263,15 @@ TEST( BayesTree, removeTop2 )
|
|||
expected.push_factor("B");
|
||||
expected.push_factor("L","B","S");
|
||||
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
|
||||
SymbolicBayesTree::Cliques expectedOrphans;
|
||||
SymbolicISAM::Cliques expectedOrphans;
|
||||
expectedOrphans += bayesTree["T"], bayesTree["X"];
|
||||
CHECK(assert_equal(expectedOrphans, orphans));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, iSAM )
|
||||
TEST( ISAM, iSAM )
|
||||
{
|
||||
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
|
||||
SymbolicISAM bayesTree = createAsiaSymbolicBayesTree();
|
||||
|
||||
// Now we modify the Bayes tree by inserting a new factor over B and S
|
||||
|
||||
|
|
@ -279,7 +283,7 @@ TEST( BayesTree, iSAM )
|
|||
B_(new SymbolicConditional("B", "E", "L", "S"));
|
||||
|
||||
// Create expected Bayes tree
|
||||
SymbolicBayesTree expected;
|
||||
SymbolicISAM expected;
|
||||
expected.insert(S_);
|
||||
expected.insert(L_);
|
||||
expected.insert(E_);
|
||||
|
|
@ -298,19 +302,19 @@ TEST( BayesTree, iSAM )
|
|||
// Check whether the same
|
||||
CHECK(assert_equal(expected,bayesTree));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( BayesTree, iSAM_slam )
|
||||
TEST( ISAM, iSAM_slam )
|
||||
{
|
||||
// Create using insert
|
||||
SymbolicBayesTree bayesTree_slam = createSlamSymbolicBayesTree();
|
||||
SymbolicISAM bayesTree_slam = createSlamSymbolicBayesTree();
|
||||
|
||||
//New conditionals for the expected Bayes tree
|
||||
|
||||
SymbolicConditional::shared_ptr
|
||||
l1_(new SymbolicConditional("l1","x1","x2","x3"));
|
||||
|
||||
// Create expected Bayes tree
|
||||
SymbolicBayesTree expected_slam;
|
||||
SymbolicISAM expected_slam;
|
||||
expected_slam.insert(x3);
|
||||
expected_slam.insert(x2);
|
||||
expected_slam.insert(x1);
|
||||
|
|
@ -327,7 +331,7 @@ TEST( BayesTree, iSAM_slam )
|
|||
bayesTree_slam.update(factorGraph_slam);
|
||||
|
||||
// Check whether the same
|
||||
CHECK(assert_equal(expected_slam,bayesTree_slam));
|
||||
CHECK(assert_equal(expected_slam,bayesTree_slam));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue