generate ordering from the spanning tree

cpp/Ordering.cpp

@@ -5,14 +5,55 @@
  */
 
 #include <iostream>
+#include <stdexcept>
 #include <boost/assign/std/list.hpp> // for operator +=
 #include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+
 #include "Ordering.h"
 
 using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
+#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL)
+
+/* ************************************************************************* */
+Ordering::Ordering(const map<string, string>& p_map) {
+
+	// find the root
+	string root;
+	bool foundRoot = false;
+	string child, parent;
+	FOREACH_PAIR(child, parent, p_map) {
+		if (child.compare(parent) == 0) {
+			root = child;
+			foundRoot = true;
+			break;
+		}
+	}
+	push_front(root);
+
+	if (!foundRoot)
+		throw invalid_argument("Ordering: invalid predecessor map!");
+
+	// push front the nodes from the root level to the leaf level
+	list<string> parents(1, root);
+	while(!parents.empty()){
+		list<string> children;
+		BOOST_FOREACH(string key, parents){
+			FOREACH_PAIR(child, parent, p_map){
+				if (parent.compare(key)==0 && child.compare(key)!=0) {
+					children.push_back(child);
+					push_front(child);
+				}
+			}
+		}
+
+		parents = children;
+	}
+}
+
 /* ************************************************************************* */
 Ordering Ordering::subtract(const Ordering& keys) const {
 	Ordering newOrdering = *this;
@@ -34,6 +75,7 @@ void Ordering::print(const string& s) const {
 bool Ordering::equals(const Ordering &other, double tol) const {
 	return *this == other;
 }
+
 /* ************************************************************************* */
 
 

cpp/Ordering.h

@@ -8,6 +8,7 @@
 
 #include <list>
 #include <string>
+#include <map>
 #include "Testable.h"
 
 namespace gtsam {
@@ -38,6 +39,11 @@ namespace gtsam {
 			std::list<std::string>(strings_in) {
 		}
 
+		/**
+		 * Generate the ordering from a spanning tree represented by its parent map
+		 */
+		Ordering(const std::map<std::string, std::string>& p_map);
+
 		/**
 		 * Remove a set of keys from an ordering
 		 * @param keys to remove

cpp/testOrdering.cpp

@@ -11,6 +11,26 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
+/* ************************************************************************* */
+// x1 -> x2
+//		-> x3 -> x4
+//    -> x5
+TEST ( Ordering, constructor ) {
+	map<string, string> p_map;
+	p_map.insert(make_pair("x1", "x1"));
+	p_map.insert(make_pair("x2", "x1"));
+	p_map.insert(make_pair("x3", "x1"));
+	p_map.insert(make_pair("x4", "x3"));
+	p_map.insert(make_pair("x5", "x1"));
+
+	Ordering expected;
+	expected += "x4", "x5", "x3", "x2", "x1";
+
+	Ordering actual(p_map);
+	CHECK(assert_equal(expected, actual));
+}
+
+
 /* ************************************************************************* */
 TEST ( Ordering, subtract ) {
 	Ordering init, delta;