From 3c668617903b45a438c942da015f4cb4385b1f84 Mon Sep 17 00:00:00 2001 From: Kai Ni Date: Wed, 13 Jan 2010 23:59:46 +0000 Subject: [PATCH] fix the graph related functions in FactorGraph as well as its unit tests --- cpp/FactorGraph-inl.h | 20 ++++++++++++-------- cpp/FactorGraph.h | 6 +++--- cpp/GaussianFactor.h | 16 ++++++++++++++++ cpp/Ordering.cpp | 8 -------- cpp/graph-inl.h | 2 +- cpp/testGaussianFactorGraph.cpp | 8 ++++---- 6 files changed, 36 insertions(+), 24 deletions(-) diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 203dec0e2..490a43a1a 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -269,20 +269,24 @@ void FactorGraph::associateFactor(int index, sharedFactor factor) { } } -/* ************************************************************************* * -template -map FactorGraph::findMinimumSpanningTree() const { +/* ************************************************************************* */ +template template +map FactorGraph::findMinimumSpanningTree() const { - SDGraph g = gtsam::toBoostGraph, sharedFactor>(*this); + typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; + typedef typename boost::graph_traits >::vertex_iterator BoostVertexIterator; + + SDGraph g = gtsam::toBoostGraph, sharedFactor, Key>(*this); // find minimum spanning tree vector p_map(boost::num_vertices(g)); prim_minimum_spanning_tree(g, &p_map[0]); // convert edge to string pairs - map tree; + map tree; BoostVertexIterator itVertex = boost::vertices(g).first; - for (vector::iterator vi = p_map.begin(); vi!=p_map.end(); itVertex++, vi++) { + typename vector::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); // printf("%s parent: %s\n", key.c_str(), parent.c_str()); @@ -292,8 +296,8 @@ map FactorGraph::findMinimumSpanningTree() const { return tree; } -template -void FactorGraph::split(map tree, FactorGraph& Ab1, FactorGraph& Ab2) const { +template template +void FactorGraph::split(map tree, FactorGraph& Ab1, FactorGraph& Ab2) const { BOOST_FOREACH(sharedFactor factor, factors_){ if (factor->keys().size() > 2) diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 6e126609e..34e6ff4f2 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -120,14 +120,14 @@ namespace gtsam { /** * find the minimum spanning tree. */ -// std::map findMinimumSpanningTree() const; + template std::map findMinimumSpanningTree() const; /** * Split the graph into two parts: one corresponds to the given spanning tre, * and the other corresponds to the rest of the factors */ -// void split(std::map tree, -// FactorGraph& Ab1, FactorGraph& Ab2) const; + template void split(std::map tree, + FactorGraph& Ab1, FactorGraph& Ab2) const; private: /** Associate factor index with the variables connected to the factor */ diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h index 5201b4d3b..d8da80d7e 100644 --- a/cpp/GaussianFactor.h +++ b/cpp/GaussianFactor.h @@ -171,6 +171,22 @@ public: */ std::list keys() const; + /** + * return the first key + * @return The set of all variable keys + */ + std::string key1() const { return As_.begin()->first; } + + /** + * return the first key + * @return The set of all variable keys + */ + std::string key2() const { + if (As_.size() < 2) throw std::invalid_argument("GaussianFactor: less than 2 keys!"); + return (++(As_.begin()))->first; + } + + /** * Find all variables and their dimensions * @return The set of all variable/dimension pairs diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index e3fae0d96..415e4b149 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -20,7 +20,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) {} @@ -31,13 +30,6 @@ public: Ordering& ordering_; }; -class ordering_edge_action { -public: - void act (string& child, string& parent, SVertex& v1, SVertex& v2, SGraph& g){ - //boost::add_edge(v1, v2, g); - } -}; -*/ /* ************************************************************************* * Ordering::Ordering(const map& p_map) { diff --git a/cpp/graph-inl.h b/cpp/graph-inl.h index 28f948c69..9cc751ef2 100644 --- a/cpp/graph-inl.h +++ b/cpp/graph-inl.h @@ -30,7 +30,7 @@ namespace gtsam { template SDGraph toBoostGraph(const G& graph) { // convert the factor graph to boost graph - SDGraph g(0); + SDGraph g; typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; map key2vertex; BoostVertex v1, v2; diff --git a/cpp/testGaussianFactorGraph.cpp b/cpp/testGaussianFactorGraph.cpp index 3afed4df8..1acfb8216 100644 --- a/cpp/testGaussianFactorGraph.cpp +++ b/cpp/testGaussianFactorGraph.cpp @@ -730,7 +730,7 @@ TEST( GaussianFactorGraph, constrained_multi2 ) CHECK(assert_equal(expected, actual)); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( GaussianFactorGraph, findMinimumSpanningTree ) { GaussianFactorGraph g; @@ -743,14 +743,14 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree ) g.add("x2", I, "x4", I, b, 0); g.add("x3", I, "x4", I, b, 0); - map tree = g.findMinimumSpanningTree(); + map tree = g.findMinimumSpanningTree(); CHECK(tree["x1"].compare("x1")==0); CHECK(tree["x2"].compare("x1")==0); CHECK(tree["x3"].compare("x1")==0); CHECK(tree["x4"].compare("x1")==0); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( GaussianFactorGraph, split ) { GaussianFactorGraph g; @@ -769,7 +769,7 @@ TEST( GaussianFactorGraph, split ) tree["x4"] = "x1"; GaussianFactorGraph Ab1, Ab2; - g.split(tree, Ab1, Ab2); + g.split(tree, Ab1, Ab2); LONGS_EQUAL(3, Ab1.size()); LONGS_EQUAL(2, Ab2.size()); }