ordering is fixed

cpp/FactorGraph-inl.h

@@ -274,19 +274,16 @@ void FactorGraph<Factor>::associateFactor(int index, sharedFactor factor) {
 template<class Factor> template <class Key>
 map<Key,Key> FactorGraph<Factor>::findMinimumSpanningTree() const {
 
-	typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor BoostVertex;
-	typedef typename boost::graph_traits<SDGraph<Key> >::vertex_iterator BoostVertexIterator;
-
 	SDGraph<Key> g = gtsam::toBoostGraph<FactorGraph<Factor>, sharedFactor, Key>(*this);
 
 	// find minimum spanning tree
-	vector<BoostVertex> p_map(boost::num_vertices(g));
+	vector<typename SDGraph<Key>::Vertex> p_map(boost::num_vertices(g));
 	prim_minimum_spanning_tree(g, &p_map[0]);
 
 	// convert edge to string pairs
 	map<Key, Key> tree;
-	BoostVertexIterator itVertex = boost::vertices(g).first;
+	typename SDGraph<Key>::vertex_iterator itVertex = boost::vertices(g).first;
-	typename vector<BoostVertex>::iterator vi;
+	typename vector<typename SDGraph<Key>::Vertex>::iterator vi;
 	for (vi = p_map.begin(); vi!=p_map.end(); itVertex++, vi++) {
 		string key = boost::get(boost::vertex_name, g, *itVertex);
 		string parent = boost::get(boost::vertex_name, g, *vi);

cpp/Ordering.cpp

@@ -38,7 +38,7 @@ Ordering::Ordering(const PredecessorMap<string>& p_map) {
 	SGraph<string> g;
 	SVertex root;
 	map<string, SVertex> key2vertex;
-	//boost::tie(g, root, key2vertex) = predecessorMap2Graph<string>(p_map);
+	boost::tie(g, root, key2vertex) = predecessorMap2Graph<SGraph<string>, SVertex, string>(p_map);
 
 	// breadth first visit on the graph
 	ordering_key_visitor vis(*this);

cpp/graph-inl.h

@@ -61,12 +61,11 @@ SDGraph<Key> toBoostGraph(const G& graph) {
 }
 
 /* ************************************************************************* */
-template<class Key>
+template<class G, class V, class Key>
-boost::tuple<SDGraph<Key>, typename SDGraph<Key>::Vertex, std::map<Key, typename SDGraph<Key>::Vertex> >
+boost::tuple<G, V, map<Key,V> >
 predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
 
-	typedef typename SDGraph<Key>::Vertex V;
+	G g;
-	SDGraph<Key> g;
 	map<Key, V> key2vertex;
 	V v1, v2, root;
 	Key child, parent;
@@ -94,7 +93,7 @@ predecessorMap2Graph(const PredecessorMap<Key>& p_map) {
 	if (!foundRoot)
 		throw invalid_argument("predecessorMap2Graph: invalid predecessor map!");
 
-	return boost::tuple<SDGraph<Key>, V, std::map<Key, V> >(g, root, key2vertex);
+	return boost::tuple<G, V, std::map<Key, V> >(g, root, key2vertex);
 }
 
 /* ************************************************************************* */
@@ -133,8 +132,8 @@ boost::shared_ptr<Config> composePoses(const G& graph, const PredecessorMap<type
 	PoseGraph g;
 	PoseVertex root;
 	map<typename Config::Key, PoseVertex> key2vertex;
-//	boost::tie(g, root, key2vertex) =
+	boost::tie(g, root, key2vertex) =
-//			predecessorMap2Graph<typename Config::Key>(tree);
+			predecessorMap2Graph<PoseGraph, PoseVertex, typename Config::Key>(tree);
 
 	// attach the relative poses to the edges
 	PoseEdge edge1, edge2;

cpp/graph.h

@@ -24,13 +24,15 @@ namespace gtsam {
 	class SDGraph: public boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS,
 	boost::property<boost::vertex_name_t, Key>, boost::property<
 	boost::edge_weight_t, double> > {
+	public:
+		typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor Vertex;
 	};
 
 	template<class Key>
 	class SGraph : public boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
-			boost::property<boost::vertex_name_t, std::string> > {
+			boost::property<boost::vertex_name_t, Key> > {
 	public:
-		typedef typename boost::graph_traits<SDGraph<Key> >::vertex_descriptor Vertex;
+		typedef typename boost::graph_traits<SGraph<Key> >::vertex_descriptor Vertex;
 	};
 
 	//typedef boost::graph_traits<SGraph>::vertex_descriptor SVertex;
@@ -54,9 +56,8 @@ namespace gtsam {
 	 * G = Graph type
 	 * V = Vertex type
 	 */
-	template<class Key>
+	template<class G, class V, class Key>
-	boost::tuple<SDGraph<Key>, typename SDGraph<Key>::Vertex, std::map<Key, typename SDGraph<Key>::Vertex> >
+	boost::tuple<G, V, std::map<Key,V> >	predecessorMap2Graph(const PredecessorMap<Key>& p_map);
-		predecessorMap2Graph(const PredecessorMap<Key>& p_map);
 
 	/**
 	 * Compose the poses by following the chain specified by the spanning tree

cpp/testGraph.cpp

@@ -20,6 +20,24 @@ using namespace std;
 using namespace boost;
 using namespace gtsam;
 
+/* ************************************************************************* */
+TEST( Graph, predecessorMap2Graph )
+{
+	typedef SGraph<string>::Vertex SVertex;
+	SGraph<string> graph;
+	SVertex root;
+	map<string, SVertex> key2vertex;
+
+	PredecessorMap<string> p_map;
+	p_map.insert(make_pair("x1", "x2"));
+	p_map.insert(make_pair("x2", "x2"));
+	p_map.insert(make_pair("x3", "x2"));
+	tie(graph, root, key2vertex) = predecessorMap2Graph<SGraph<string>, SVertex, string>(p_map);
+
+	LONGS_EQUAL(3, boost::num_vertices(graph));
+	CHECK(root == key2vertex["x2"]);
+}
+
 /* ************************************************************************* */
 TEST( Graph, composePoses )
 {

cpp/testOrdering.cpp

@@ -11,12 +11,12 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
-/* ************************************************************************* *
+/* ************************************************************************* */
 // x1 -> x2
 //		-> x3 -> x4
 //    -> x5
 TEST ( Ordering, constructorFromTree ) {
-	map<string, string> p_map;
+	PredecessorMap<string> p_map;
 	p_map.insert(make_pair("x1", "x1"));
 	p_map.insert(make_pair("x2", "x1"));
 	p_map.insert(make_pair("x3", "x1"));