Fixed insert to use IndexTable, a new class

.cproject

@@ -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>

cpp/BayesTree-inl.h

@@ -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 BayesTree<Conditional>::findParentClique(const list<Symbol>& parents,
-		Symbol parent;
+			const IndexTable<Symbol>& index) const {
-		for (list<Symbol>::const_iterator it = ordering.begin(); it!=ordering.end(); it++) {
+		boost::optional<Symbol> parentCliqueRepresentative;
-			list<Symbol>::const_iterator pit = find(parents.begin(), parents.end(), *it);
+		boost::optional<size_t> lowest;
-			if (pit!=parents.end()) {
+		BOOST_FOREACH(const Symbol& p, parents) {
-				parent = *pit;
+			size_t i = index(p);
-				break;
+			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
+		// otherwise, find the parent clique by using the index data structure
-		Symbol parent;
+		// to find the lowest-ordered parent
-		if (!ordering) {
+		Symbol parentRepresentative = findParentClique(parents, index);
-			parent = parents.front(); // assumes parents are in current variable order, which is not the case (after COLAMD was activated)
+		sharedClique parent_clique = (*this)[parentRepresentative];
-		} else {
-			parent = findParentClique(parents, *ordering);
-		}
-		sharedClique parent_clique = (*this)[parent];
 
 		// if the parents and parent clique have the same size, add to parent clique
 		if (parent_clique->size() == parents.size()) {

cpp/BayesTree.h

@@ -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 {

cpp/ISAM-inl.h

@@ -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);
+			Symbol parentRepresentative = findParentClique(orphan->separator_, index);
-			sharedClique parent = (*this)[key];
+			sharedClique parent = (*this)[parentRepresentative];
 
 			parent->children_ += orphan;
 			orphan->parent_ = parent; // set new parent!

cpp/ISAM2-inl.h

@@ -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 ) {
+		for ( rit=bayesNet.rbegin(); rit != bayesNet.rend(); ++rit )
-			this->insert(*rit, &ordering);
+			this->insert(*rit, index);
-		}
 
 		// add orphans to the bottom of the new tree
 		BOOST_FOREACH(sharedClique orphan, orphans) {
-			Symbol key = findParentClique(orphan->separator_, ordering);
+			Symbol parentRepresentative = findParentClique(orphan->separator_, index);
-			sharedClique parent = (*this)[key];
+			sharedClique parent = (*this)[parentRepresentative];
 			parent->children_ += orphan;
 			orphan->parent_ = parent; // set new parent!
 		}

cpp/IndexTable.h

@@ -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[] */
+	};
+
+}

cpp/Key.h

@@ -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_; }
 

cpp/Makefile.am

@@ -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

cpp/testBayesTree.cpp

@@ -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")),
+Symbol _x1_('x', 1), _x2_('x', 2), _x3_('x', 3), _l1_('l', 1), _l2_('l', 2);
-		x2(new SymbolicConditional("x2","x3")),
+SymbolicConditional::shared_ptr
-		x1(new SymbolicConditional("x1","x2","x3")),
+		x3(new SymbolicConditional(_x3_)),
-		l1(new SymbolicConditional("l1","x1","x2")),
+		x2(new SymbolicConditional(_x2_,_x3_)),
-		l2(new SymbolicConditional("l2","x1","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(x3,slamOrdering);
-	bayesTree_slam.insert(x2);
+	bayesTree_slam.insert(x2,slamOrdering);
-	bayesTree_slam.insert(x1);
+	bayesTree_slam.insert(x1,slamOrdering);
-	bayesTree_slam.insert(l2);
+	bayesTree_slam.insert(l2,slamOrdering);
-	bayesTree_slam.insert(l1);
+	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")),
+	B(new SymbolicConditional(_B_)),
-	L(new SymbolicConditional("L", "B")),
+	L(new SymbolicConditional(_L_, _B_)),
-	E(new SymbolicConditional("E", "B", "L")),
+	E(new SymbolicConditional(_E_, _B_, _L_)),
-	S(new SymbolicConditional("S", "L", "B")),
+	S(new SymbolicConditional(_S_, _L_, _B_)),
-	T(new SymbolicConditional("T", "E", "L")),
+	T(new SymbolicConditional(_T_, _E_, _L_)),
-	X(new SymbolicConditional("X", "E"));
+	X(new SymbolicConditional(_X_, _E_));
 
 // Bayes Tree for Asia example
 SymbolicBayesTree createAsiaSymbolicBayesTree() {
 	SymbolicBayesTree bayesTree;
-	bayesTree.insert(B);
+	Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_;
-	bayesTree.insert(L);
+	bayesTree.insert(B,asiaOrdering);
-	bayesTree.insert(E);
+	bayesTree.insert(L,asiaOrdering);
-	bayesTree.insert(S);
+	bayesTree.insert(E,asiaOrdering);
-	bayesTree.insert(T);
+	bayesTree.insert(S,asiaOrdering);
-	bayesTree.insert(X);
+	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")),
+			A(new SymbolicConditional(_A_)),
-			B(new SymbolicConditional("B", "A")),
+			B(new SymbolicConditional(_B_, _A_)),
-			C(new SymbolicConditional("C", "A")),
+			C(new SymbolicConditional(_C_, _A_)),
-			D(new SymbolicConditional("D", "C")),
+			D(new SymbolicConditional(_D_, _C_)),
-			E(new SymbolicConditional("E", "B")),
+			E(new SymbolicConditional(_E_, _B_)),
-			F(new SymbolicConditional("F", "E"));
+			F(new SymbolicConditional(_F_, _E_));
 	SymbolicBayesTree bayesTree;
-	bayesTree.insert(A);
+	Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
-	bayesTree.insert(B);
+	bayesTree.insert(A,ord);
-	bayesTree.insert(C);
+	bayesTree.insert(B,ord);
-	bayesTree.insert(D);
+	bayesTree.insert(C,ord);
-	bayesTree.insert(E);
+	bayesTree.insert(D,ord);
-	bayesTree.insert(F);
+	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_,_B_);
-	expected.push_factor("A");
+	expected.push_factor(_A_);
-	expected.push_factor("A","C");
+	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_,_E_);
-	expected.push_factor("B","L");
+	expected.push_factor(_B_,_L_);
-	expected.push_factor("B");
+	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_,_E_);
-	expected.push_factor("B","L");
+	expected.push_factor(_B_,_L_);
-	expected.push_factor("B");
+	expected.push_factor(_B_);
-	expected.push_factor("L","B","S");
+	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_,_E_);
-	expected.push_factor("B","L");
+	expected.push_factor(_B_,_L_);
-	expected.push_factor("B");
+	expected.push_factor(_B_);
-	expected.push_factor("L","B","S");
+	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(_B_);
-	newFactors.push_factor("S");
+	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_,_E_);
-	expected.push_factor("B","L");
+	expected.push_factor(_B_,_L_);
-	expected.push_factor("B");
+	expected.push_factor(_B_);
-	expected.push_factor("L","B","S");
+	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")),
+	X(new SymbolicConditional(_l5_)),
-	A(new SymbolicConditional("x4", "l5")),
+	A(new SymbolicConditional(_x4_, _l5_)),
-	B(new SymbolicConditional("x3", "x4")),
+	B(new SymbolicConditional(_x3_, _x4_)),
-	C(new SymbolicConditional("x2", "x3"));
+	C(new SymbolicConditional(_x2_, _x3_));
 
+	Ordering newOrdering;
+	newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
 	SymbolicBayesTree bayesTree;
-	bayesTree.insert(X);
+	bayesTree.insert(X,newOrdering);
-	bayesTree.insert(A);
+	bayesTree.insert(A,newOrdering);
-	bayesTree.insert(B);
+	bayesTree.insert(B,newOrdering);
-	bayesTree.insert(C);
+	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);

cpp/testISAM.cpp

@@ -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

cpp/testKey.cpp

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