diff --git a/cpp/Makefile.am b/cpp/Makefile.am index d69115d15..9f6e80a1a 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -49,7 +49,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 = gtsam.h Value.h Testable.h Factor.h Conditional.h Ordering.h Ordering-inl.h sources += Ordering.cpp example = smallExample.cpp diff --git a/cpp/Ordering-inl.h b/cpp/Ordering-inl.h new file mode 100644 index 000000000..e8c89a19d --- /dev/null +++ b/cpp/Ordering-inl.h @@ -0,0 +1,48 @@ +/* + * Ordering-inl.h + * + * Created on: Jan 14, 2010 + * Author: nikai + * Description: the inline file for Ordering + */ + +#pragma once + +#include "graph-inl.h" +#include "Ordering.h" + +using namespace std; + +namespace gtsam { + +/* ************************************************************************* */ +template +class ordering_key_visitor : public boost::default_bfs_visitor { +public: + ordering_key_visitor(std::list& ordering_in) : ordering_(ordering_in) {} + template void discover_vertex(Vertex v, const Graph& g) const { + Key key = boost::get(boost::vertex_name, g, v); + ordering_.push_front(key); + } + std::list& ordering_; +}; + +/* ************************************************************************* */ +template +list predecessorMap2Keys(const PredecessorMap& p_map) { + + typedef typename SGraph::Vertex SVertex; + + SGraph g; + SVertex root; + std::map key2vertex; + boost::tie(g, root, key2vertex) = gtsam::predecessorMap2Graph, SVertex, Key>(p_map); + + // breadth first visit on the graph + std::list keys; + ordering_key_visitor vis(keys); + boost::breadth_first_search(g, root, boost::visitor(vis)); + return keys; +} + +} diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index 1b24b6566..9f3a14fa0 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -10,7 +10,6 @@ #include #include -#include "graph-inl.h" #include "Ordering.h" @@ -20,31 +19,6 @@ using namespace boost::assign; #define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) -class ordering_key_visitor : public boost::default_bfs_visitor { -public: - ordering_key_visitor(Ordering& ordering_in) : ordering_(ordering_in) {} - template void discover_vertex(Vertex v, const Graph& g) const { - string key = boost::get(boost::vertex_name, g, v); - ordering_.push_front(key); - } - Ordering& ordering_; -}; - -/* ************************************************************************* */ -Ordering::Ordering(const PredecessorMap& p_map) { - - typedef SGraph::Vertex SVertex; - - SGraph g; - SVertex root; - map key2vertex; - boost::tie(g, root, key2vertex) = predecessorMap2Graph, SVertex, string>(p_map); - - // breadth first visit on the graph - ordering_key_visitor vis(*this); - boost::breadth_first_search(g, root, boost::visitor(vis)); -} - /* ************************************************************************* */ Ordering Ordering::subtract(const Ordering& keys) const { Ordering newOrdering = *this; @@ -67,7 +41,3 @@ bool Ordering::equals(const Ordering &other, double tol) const { return *this == other; } -/* ************************************************************************* */ - - - diff --git a/cpp/Ordering.h b/cpp/Ordering.h index 029e61379..9682c54fd 100644 --- a/cpp/Ordering.h +++ b/cpp/Ordering.h @@ -40,11 +40,6 @@ namespace gtsam { std::list(strings_in) { } - /** - * Generate the ordering from a spanning tree represented by its parent map - */ - Ordering(const PredecessorMap& p_map); - /** * Remove a set of keys from an ordering * @param keys to remove @@ -63,4 +58,10 @@ namespace gtsam { }; + /** + * Generate a list of keys from a spanning tree represented by its predecessor map + */ + template + std::list predecessorMap2Keys(const PredecessorMap& p_map); + } diff --git a/cpp/testOrdering.cpp b/cpp/testOrdering.cpp index 068c82783..2d70b275c 100644 --- a/cpp/testOrdering.cpp +++ b/cpp/testOrdering.cpp @@ -5,7 +5,9 @@ #include // for operator += #include -#include "Ordering.h" +#include "Ordering-inl.h" +#include "Pose2Config.h" + using namespace std; using namespace gtsam; @@ -15,19 +17,25 @@ using namespace boost::assign; // x1 -> x2 // -> x3 -> x4 // -> x5 -TEST ( Ordering, constructorFromTree ) { - PredecessorMap 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")); +TEST ( Ordering, predecessorMap2Keys ) { + typedef Symbol Key; + PredecessorMap p_map; + p_map.insert(make_pair(Key(1), Key(1))); + p_map.insert(make_pair(Key(2), Key(1))); + p_map.insert(make_pair(Key(3), Key(1))); + p_map.insert(make_pair(Key(4), Key(3))); + p_map.insert(make_pair(Key(5), Key(1))); - Ordering expected; - expected += "x4", "x5", "x3", "x2", "x1"; + list expected; + expected += Key(4), Key(5), Key(3), Key(2), Key(1); - Ordering actual(p_map); - CHECK(assert_equal(expected, actual)); + list actual = predecessorMap2Keys(p_map); + LONGS_EQUAL(expected.size(), actual.size()); + + list::const_iterator it1 = expected.begin(); + list::const_iterator it2 = actual.begin(); + for(; it1!=expected.end(); it1++, it2++) + CHECK(*it1 == *it2) }