12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed insert to use IndexTable, a new class

release/4.3a0
Frank Dellaert 2010-01-22 02:27:26 +00:00
parent 2cc777228b
commit 41a6e64bbb
11 changed files with 222 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
.cproject
View File

@ -469,6 +469,7 @@
</target>
<target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -476,7 +477,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -484,6 +484,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -673,14 +674,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2SLAM.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -715,6 +708,7 @@
</target>
<target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -770,7 +764,6 @@
</target>
<target name="testSimulated2D.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -784,6 +777,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testKey.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

38
cpp/BayesTree-inl.h
View File

@ -228,9 +228,10 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
BayesTree<Conditional>::BayesTree(const BayesNet<Conditional>& bayesNet) {
IndexTable<Symbol> index(bayesNet.ordering());
typename BayesNet<Conditional>::const_reverse_iterator rit;
for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
insert(*rit);
insert(*rit, index);
}
/* ************************************************************************* */
@ -265,21 +266,26 @@ namespace gtsam {
/* ************************************************************************* */
template<class Conditional>
Symbol BayesTree<Conditional>::findParentClique(const list<Symbol>& parents, const list<Symbol>& ordering) const {
Symbol parent;
for (list<Symbol>::const_iterator it = ordering.begin(); it!=ordering.end(); it++) {
list<Symbol>::const_iterator pit = find(parents.begin(), parents.end(), *it);
if (pit!=parents.end()) {
parent = *pit;
break;
Symbol BayesTree<Conditional>::findParentClique(const list<Symbol>& parents,
const IndexTable<Symbol>& index) const {
boost::optional<Symbol> parentCliqueRepresentative;
boost::optional<size_t> lowest;
BOOST_FOREACH(const Symbol& p, parents) {
size_t i = index(p);
if (!lowest || i<*lowest) {
lowest.reset(i);
parentCliqueRepresentative.reset(p);
}
}
return parent;
if (!lowest) throw
invalid_argument("BayesTree::findParentClique: no parents given or key not present in index");
return *parentCliqueRepresentative;
}
/* ************************************************************************* */
template<class Conditional>
void BayesTree<Conditional>::insert(const sharedConditional& conditional, const list<Symbol>* ordering)
void BayesTree<Conditional>::insert(const sharedConditional& conditional,
const IndexTable<Symbol>& index)
{
// get key and parents
const Symbol& key = conditional->key();
@ -291,14 +297,10 @@ namespace gtsam {
return;
}
// otherwise, find the parent clique
Symbol parent;
if (!ordering) {
parent = parents.front(); // assumes parents are in current variable order, which is not the case (after COLAMD was activated)
} else {
parent = findParentClique(parents, *ordering);
}
sharedClique parent_clique = (*this)[parent];
// otherwise, find the parent clique by using the index data structure
// to find the lowest-ordered parent
Symbol parentRepresentative = findParentClique(parents, index);
sharedClique parent_clique = (*this)[parentRepresentative];
// if the parents and parent clique have the same size, add to parent clique
if (parent_clique->size() == parents.size()) {

10
cpp/BayesTree.h
View File

@ -19,6 +19,7 @@
#include "FactorGraph.h"
#include "BayesNet.h"
#include "Key.h"
#include "IndexTable.h"
namespace gtsam {
@ -128,11 +129,14 @@ namespace gtsam {
/** check equality */
bool equals(const BayesTree<Conditional>& other, double tol = 1e-9) const;
/** find parent clique of a conditional, given an ordering */
Symbol findParentClique(const std::list<Symbol>& parents, const std::list<Symbol>& ordering) const;
/**
* Find parent clique of a conditional, given an IndexTable constructed from an ordering.
* It will look at all parents and return the one with the lowest index in the ordering.
*/
Symbol findParentClique(const std::list<Symbol>& parents, const IndexTable<Symbol>& index) const;
/** insert a new conditional */
void insert(const sharedConditional& conditional, const std::list<Symbol>* ordering = NULL);
void insert(const sharedConditional& conditional, const IndexTable<Symbol>& index);
/** number of cliques */
inline size_t size() const {

9
cpp/ISAM-inl.h
View File

@ -48,20 +48,23 @@ namespace gtsam {
ordering = keys;
}
// Create Index from ordering
IndexTable<Symbol> index(ordering);
// eliminate into a Bayes net
BayesNet<Conditional> bayesNet = eliminate<Factor, Conditional>(factors,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, &ordering);
this->insert(*rit, index);
int count = 0;
// add orphans to the bottom of the new tree
BOOST_FOREACH(sharedClique orphan, orphans) {
Symbol key = findParentClique(orphan->separator_, ordering);
sharedClique parent = (*this)[key];
Symbol parentRepresentative = findParentClique(orphan->separator_, index);
sharedClique parent = (*this)[parentRepresentative];
parent->children_ += orphan;
orphan->parent_ = parent; // set new parent!

12
cpp/ISAM2-inl.h
View File

@ -195,16 +195,18 @@ namespace gtsam {
// eliminate into a Bayes net
BayesNet<Conditional> bayesNet = _eliminate(factors, cached_, ordering);
// 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, &ordering);
}
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 key = findParentClique(orphan->separator_, ordering);
sharedClique parent = (*this)[key];
Symbol parentRepresentative = findParentClique(orphan->separator_, index);
sharedClique parent = (*this)[parentRepresentative];
parent->children_ += orphan;
orphan->parent_ = parent; // set new parent!
}

58
cpp/IndexTable.h Normal file
View File

@ -0,0 +1,58 @@
/*
* IndexTable.h
*
* Created on: Jan 21, 2010
* @Author: Frank Dellaert
*/
#pragma once
#include <boost/foreach.hpp> // TODO should not be in header
#include "Testable.h"
namespace gtsam {
/**
* An IndexTable maps from key to size_t index and back
* most commonly used templated on Symbol with orderings
*/
template<class Key>
class IndexTable: public std::vector<Key>, public Testable<IndexTable<Key> > {
private:
/* map back from key to size_t */
typedef typename std::map<Key, size_t> Map;
Map key2index_;
public:
/* bake ordering into IndexTable */
IndexTable(const std::list<Key>& ordering) {
size_t i = 0;
BOOST_FOREACH(const Key& key,ordering){
this->push_back(key);
key2index_.insert(make_pair(key,i++));
}
}
// Testable
virtual void print(const std::string& s="") const {
std::cout << "IndexTable " << s << ":";
BOOST_FOREACH(Key key,*this) std::cout << (std::string)key << " ";
}
virtual bool equals(const IndexTable<Key>& expected, double tol) const {
return key2index_==expected.key2index_; // TODO, sanity check
}
/** Key to index by parentheses ! */
size_t operator()(const Key& key) const {
typename Map::const_iterator it = key2index_.find(key);
if (it==key2index_.end())
throw(std::invalid_argument("IndexTable::[] invalid key"));
return it->second;
}
/* Index to Key is provided by base class operator[] */
};
}

15
cpp/Key.h
View File

@ -54,7 +54,9 @@ namespace gtsam {
int compare(const TypedSymbol& compare) const {return j_-compare.j_;}
// Testable Requirements
void print(const std::string& name) const {} //FIXME
virtual void print(const std::string& s) const {
std::cout << s << ": " << (std::string)(*this) << std::endl;
}
bool equals(const TypedSymbol& expected, double tol) const { return (*this)==expected; }
private:
@ -118,7 +120,6 @@ namespace gtsam {
}
// Testable Requirements
void print(const std::string& name) const {} // FIXME
bool equals(const TypedLabeledSymbol& expected, double tol) const
{ return (*this)==expected; }
@ -139,10 +140,8 @@ namespace gtsam {
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
* keys when linearizing a nonlinear factor graph. This key is not type
* safe, so cannot be used with any Nonlinear* classes.
*
* richard: temporarily named 'Symbol' to make refactoring easier
*/
class Symbol {
class Symbol : Testable<Symbol> {
private:
unsigned char c_;
size_t j_;
@ -190,6 +189,12 @@ namespace gtsam {
}
#endif
// Testable Requirements
void print(const std::string& s) const {
std::cout << s << ": " << (std::string)(*this) << std::endl;
}
bool equals(const Symbol& expected, double tol) const { return (*this)==expected; }
/** Retrieve key character */
unsigned char chr() const { return c_; }

2
cpp/Makefile.am
View File

@ -39,7 +39,7 @@ testMatrix_LDADD = libgtsam.la
# GTSAM basics
# The header files will be installed in ~/include/gtsam
headers = gtsam.h Value.h Testable.h Factor.h Conditional.h
headers += Ordering.h numericalDerivative.h
headers += Ordering.h IndexTable.h numericalDerivative.h
sources += Ordering.cpp smallExample.cpp
# Symbolic Inference

177
cpp/testBayesTree.cpp
View File

@ -11,13 +11,12 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#define GTSAM_MAGIC_KEY
#include "SymbolicBayesNet.h"
#include "SymbolicFactorGraph.h"
#include "Ordering.h"
#include "BayesTree-inl.h"
#include "smallExample.h"
#include "IndexTable.h"
using namespace gtsam;
@ -25,45 +24,48 @@ typedef BayesTree<SymbolicConditional> SymbolicBayesTree;
/* ************************************************************************* */
// SLAM example from RSS sqrtSAM paper
SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
x2(new SymbolicConditional("x2","x3")),
x1(new SymbolicConditional("x1","x2","x3")),
l1(new SymbolicConditional("l1","x1","x2")),
l2(new SymbolicConditional("l2","x1","x3"));
Symbol _x1_('x', 1), _x2_('x', 2), _x3_('x', 3), _l1_('l', 1), _l2_('l', 2);
SymbolicConditional::shared_ptr
x3(new SymbolicConditional(_x3_)),
x2(new SymbolicConditional(_x2_,_x3_)),
x1(new SymbolicConditional(_x1_,_x2_,_x3_)),
l1(new SymbolicConditional(_l1_,_x1_,_x2_)),
l2(new SymbolicConditional(_l2_,_x1_,_x3_));
// Bayes Tree for sqrtSAM example
SymbolicBayesTree createSlamSymbolicBayesTree(){
// Create using insert
Ordering slamOrdering; slamOrdering += _x3_, _x2_, _x1_, _l2_, _l1_;
SymbolicBayesTree bayesTree_slam;
bayesTree_slam.insert(x3);
bayesTree_slam.insert(x2);
bayesTree_slam.insert(x1);
bayesTree_slam.insert(l2);
bayesTree_slam.insert(l1);
bayesTree_slam.insert(x3,slamOrdering);
bayesTree_slam.insert(x2,slamOrdering);
bayesTree_slam.insert(x1,slamOrdering);
bayesTree_slam.insert(l2,slamOrdering);
bayesTree_slam.insert(l1,slamOrdering);
return bayesTree_slam;
}
/* ************************************************************************* */
// Conditionals for ASIA example from the tutorial with A and D evidence
Symbol _B_('B', 0), _L_('L', 0), _E_('E', 0), _S_('S', 0), _T_('T', 0), _X_('X',0);
SymbolicConditional::shared_ptr
B(new SymbolicConditional("B")),
L(new SymbolicConditional("L", "B")),
E(new SymbolicConditional("E", "B", "L")),
S(new SymbolicConditional("S", "L", "B")),
T(new SymbolicConditional("T", "E", "L")),
X(new SymbolicConditional("X", "E"));
B(new SymbolicConditional(_B_)),
L(new SymbolicConditional(_L_, _B_)),
E(new SymbolicConditional(_E_, _B_, _L_)),
S(new SymbolicConditional(_S_, _L_, _B_)),
T(new SymbolicConditional(_T_, _E_, _L_)),
X(new SymbolicConditional(_X_, _E_));
// Bayes Tree for Asia example
SymbolicBayesTree createAsiaSymbolicBayesTree() {
SymbolicBayesTree bayesTree;
bayesTree.insert(B);
bayesTree.insert(L);
bayesTree.insert(E);
bayesTree.insert(S);
bayesTree.insert(T);
bayesTree.insert(X);
Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_;
bayesTree.insert(B,asiaOrdering);
bayesTree.insert(L,asiaOrdering);
bayesTree.insert(E,asiaOrdering);
bayesTree.insert(S,asiaOrdering);
bayesTree.insert(T,asiaOrdering);
bayesTree.insert(X,asiaOrdering);
return bayesTree;
}
@ -109,6 +111,16 @@ TEST( BayesTree, constructor )
// Check whether the same
CHECK(assert_equal(bayesTree,bayesTree2));
// CHECK findParentClique, should *not depend on order of parents*
Ordering ordering; ordering += _X_, _T_, _S_, _E_, _L_, _B_;
IndexTable<Symbol> index(ordering);
list<Symbol> parents1; parents1 += _E_, _L_;
CHECK(assert_equal(_E_,bayesTree.findParentClique(parents1, index)));
list<Symbol> parents2; parents2 += _L_, _E_;
CHECK(assert_equal(_E_,bayesTree.findParentClique(parents2, index)));
}
/* ************************************************************************* */
@ -131,46 +143,48 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental.
/* ************************************************************************* */
TEST( BayesTree, removePath )
{
Symbol _A_('A', 0), _B_('B', 0), _C_('C', 0), _D_('D', 0), _E_('E', 0), _F_('F',0);
SymbolicConditional::shared_ptr
A(new SymbolicConditional("A")),
B(new SymbolicConditional("B", "A")),
C(new SymbolicConditional("C", "A")),
D(new SymbolicConditional("D", "C")),
E(new SymbolicConditional("E", "B")),
F(new SymbolicConditional("F", "E"));
A(new SymbolicConditional(_A_)),
B(new SymbolicConditional(_B_, _A_)),
C(new SymbolicConditional(_C_, _A_)),
D(new SymbolicConditional(_D_, _C_)),
E(new SymbolicConditional(_E_, _B_)),
F(new SymbolicConditional(_F_, _E_));
SymbolicBayesTree bayesTree;
bayesTree.insert(A);
bayesTree.insert(B);
bayesTree.insert(C);
bayesTree.insert(D);
bayesTree.insert(E);
bayesTree.insert(F);
Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
bayesTree.insert(A,ord);
bayesTree.insert(B,ord);
bayesTree.insert(C,ord);
bayesTree.insert(D,ord);
bayesTree.insert(E,ord);
bayesTree.insert(F,ord);
// remove C, expected outcome: factor graph with ABC,
// Bayes Tree now contains two orphan trees: D|C and E|B,F|E
SymbolicFactorGraph expected;
expected.push_factor("A","B");
expected.push_factor("A");
expected.push_factor("A","C");
expected.push_factor(_A_,_B_);
expected.push_factor(_A_);
expected.push_factor(_A_,_C_);
SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree["D"], bayesTree["E"];
expectedOrphans += bayesTree[_D_], bayesTree[_E_];
BayesNet<SymbolicConditional> bn;
SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree["C"], bn, orphans);
bayesTree.removePath(bayesTree[_C_], bn, orphans);
FactorGraph<SymbolicFactor> factors(bn);
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
CHECK(assert_equal(expectedOrphans, orphans));
// remove E: factor graph with EB; E|B removed from second orphan tree
SymbolicFactorGraph expected2;
expected2.push_factor("B","E");
expected2.push_factor(_B_,_E_);
SymbolicBayesTree::Cliques expectedOrphans2;
expectedOrphans2 += bayesTree["F"];
expectedOrphans2 += bayesTree[_F_];
BayesNet<SymbolicConditional> bn2;
SymbolicBayesTree::Cliques orphans2;
bayesTree.removePath(bayesTree["E"], bn2, orphans2);
bayesTree.removePath(bayesTree[_E_], bn2, orphans2);
FactorGraph<SymbolicFactor> factors2(bn2);
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
CHECK(assert_equal(expectedOrphans2, orphans2));
@ -184,17 +198,17 @@ TEST( BayesTree, removePath2 )
// Call remove-path with clique B
BayesNet<SymbolicConditional> bn;
SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree["B"], bn, orphans);
bayesTree.removePath(bayesTree[_B_], bn, orphans);
FactorGraph<SymbolicFactor> factors(bn);
// Check expected outcome
SymbolicFactorGraph expected;
expected.push_factor("B","L","E");
expected.push_factor("B","L");
expected.push_factor("B");
expected.push_factor(_B_,_L_,_E_);
expected.push_factor(_B_,_L_);
expected.push_factor(_B_);
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree["S"], bayesTree["T"], bayesTree["X"];
expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
}
@ -206,18 +220,18 @@ TEST( BayesTree, removePath3 )
// Call remove-path with clique S
BayesNet<SymbolicConditional> bn;
SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree["S"], bn, orphans);
bayesTree.removePath(bayesTree[_S_], bn, orphans);
FactorGraph<SymbolicFactor> factors(bn);
// Check expected outcome
SymbolicFactorGraph expected;
expected.push_factor("B","L","E");
expected.push_factor("B","L");
expected.push_factor("B");
expected.push_factor("L","B","S");
expected.push_factor(_B_,_L_,_E_);
expected.push_factor(_B_,_L_);
expected.push_factor(_B_);
expected.push_factor(_L_,_B_,_S_);
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree["T"], bayesTree["X"];
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
}
@ -227,7 +241,7 @@ TEST( BayesTree, removeTop )
SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
// create a new factor to be inserted
boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor("B","S"));
boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor(_B_,_S_));
// Remove the contaminated part of the Bayes tree
BayesNet<SymbolicConditional> bn;
@ -237,17 +251,17 @@ TEST( BayesTree, removeTop )
// Check expected outcome
SymbolicFactorGraph expected;
expected.push_factor("B","L","E");
expected.push_factor("B","L");
expected.push_factor("B");
expected.push_factor("L","B","S");
expected.push_factor(_B_,_L_,_E_);
expected.push_factor(_B_,_L_);
expected.push_factor(_B_);
expected.push_factor(_L_,_B_,_S_);
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree["T"], bayesTree["X"];
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"));
boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor(_B_));
BayesNet<SymbolicConditional> bn2;
SymbolicBayesTree::Cliques orphans2;
bayesTree.removeTop(newFactor2->keys(), bn2, orphans2);
@ -265,8 +279,8 @@ TEST( BayesTree, removeTop2 )
// create two factors to be inserted
SymbolicFactorGraph newFactors;
newFactors.push_factor("B");
newFactors.push_factor("S");
newFactors.push_factor(_B_);
newFactors.push_factor(_S_);
// Remove the contaminated part of the Bayes tree
BayesNet<SymbolicConditional> bn;
@ -276,35 +290,38 @@ TEST( BayesTree, removeTop2 )
// Check expected outcome
SymbolicFactorGraph expected;
expected.push_factor("B","L","E");
expected.push_factor("B","L");
expected.push_factor("B");
expected.push_factor("L","B","S");
expected.push_factor(_B_,_L_,_E_);
expected.push_factor(_B_,_L_);
expected.push_factor(_B_);
expected.push_factor(_L_,_B_,_S_);
CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
SymbolicBayesTree::Cliques expectedOrphans;
expectedOrphans += bayesTree["T"], bayesTree["X"];
expectedOrphans += bayesTree[_T_], bayesTree[_X_];
CHECK(assert_equal(expectedOrphans, orphans));
}
/* ************************************************************************* */
TEST( BayesTree, removeTop3 )
{
Symbol _l5_('l', 5), _x4_('x', 4);
// 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"));
X(new SymbolicConditional(_l5_)),
A(new SymbolicConditional(_x4_, _l5_)),
B(new SymbolicConditional(_x3_, _x4_)),
C(new SymbolicConditional(_x2_, _x3_));
Ordering newOrdering;
newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
SymbolicBayesTree bayesTree;
bayesTree.insert(X);
bayesTree.insert(A);
bayesTree.insert(B);
bayesTree.insert(C);
bayesTree.insert(X,newOrdering);
bayesTree.insert(A,newOrdering);
bayesTree.insert(B,newOrdering);
bayesTree.insert(C,newOrdering);
// remove all
list<Symbol> keys;
keys += "l5", "x2", "x3", "x4";
keys += _l5_, _x2_, _x3_, _x4_;
BayesNet<SymbolicConditional> bn;
SymbolicBayesTree::Cliques orphans;
bayesTree.removeTop(keys, bn, orphans);

8
cpp/testISAM.cpp
View File

@ -30,7 +30,7 @@ typedef ISAM<SymbolicConditional> SymbolicISAM;
double sigmax1 = 0.786153, sigmax2 = 0.687131, sigmax3 = 0.671512, sigmax4 =
0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
/* ************************************************************************* */
/* ************************************************************************* *
// SLAM example from RSS sqrtSAM paper
SymbolicConditional::shared_ptr x3(new SymbolicConditional("x3")),
@ -51,7 +51,7 @@ SymbolicISAM createSlamSymbolicISAM(){
return bayesTree_slam;
}
/* ************************************************************************* */
/* ************************************************************************* *
// Conditionals for ASIA example from the tutorial with A and D evidence
SymbolicConditional::shared_ptr
@ -74,7 +74,7 @@ SymbolicISAM createAsiaSymbolicISAM() {
return bayesTree;
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST( ISAM, iSAM )
{
SymbolicISAM bayesTree = createAsiaSymbolicISAM();
@ -108,7 +108,7 @@ TEST( ISAM, iSAM )
CHECK(assert_equal(expected,bayesTree));
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST( ISAM, iSAM_slam )
{
// Create using insert

2
cpp/testKey.cpp
View File

@ -31,7 +31,7 @@ TEST ( TypedSymbol, basic_operations ) {
CHECK(key3 < key4);
}
/* ************************************************************************* */
/* ************************************************************************* *
TEST ( TypedLabledSymbol, basic_operations ) {
typedef TypedLabeledSymbol<Pose3, 'x', int> RobotKey;