diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index 964b3aef7..1eed181d6 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -5,14 +5,55 @@ */ #include +#include #include // for operator += #include +#include + #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& 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 parents(1, root); + while(!parents.empty()){ + list 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; } + /* ************************************************************************* */ diff --git a/cpp/Ordering.h b/cpp/Ordering.h index bb61a5125..5fbec35ab 100644 --- a/cpp/Ordering.h +++ b/cpp/Ordering.h @@ -8,6 +8,7 @@ #include #include +#include #include "Testable.h" namespace gtsam { @@ -38,6 +39,11 @@ namespace gtsam { std::list(strings_in) { } + /** + * Generate the ordering from a spanning tree represented by its parent map + */ + Ordering(const std::map& p_map); + /** * Remove a set of keys from an ordering * @param keys to remove diff --git a/cpp/testOrdering.cpp b/cpp/testOrdering.cpp index f95682773..4c3b664d5 100644 --- a/cpp/testOrdering.cpp +++ b/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 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;