diff --git a/.cproject b/.cproject index 6df3b4557..afe5aa25d 100644 --- a/.cproject +++ b/.cproject @@ -5,45 +5,45 @@ - - - - - - - - - + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -297,474 +297,483 @@ - - -make --j2 -install -true -true -true - - -make --j2 -check -true -true -true - - -make --k -check -true -false -true - - -make --j2 -testSimpleCamera.run -true -true -true - - -make --f local.mk -testCal3_S2.run -true -true -true - - -make --j2 -testVSLAMFactor.run -true -true -true - - -make --j2 -testCalibratedCamera.run -true -true -true - - -make --j2 -testGaussianConditional.run -true -true -true - - -make --j2 -testPose2.run -true -true -true - - -make --j2 -testRot3.run -true -true -true - - -make --j2 -testNonlinearOptimizer.run -true -true -true - - -make --j2 -testGaussianFactor.run -true -true -true - - -make --j2 -testGaussianFactorGraph.run -true -true -true - - -make --j2 -testNonlinearFactorGraph.run -true -true -true - - -make --j2 -testPose3.run -true -true -true - - -make --j2 -testVectorConfig.run -true -true -true - - -make --j2 -testPoint2.run -true -true -true - - -make --j2 -testNonlinearFactor.run -true -true -true - - -make --j2 -timeGaussianFactor.run -true -true -true - - -make --j2 -timeGaussianFactorGraph.run -true -true -true - - -make --j2 -testGaussianBayesNet.run -true -true -true - - -make -testBayesTree.run -true -false -true - - -make - -testSymbolicBayesNet.run -true -false -true - - -make -testSymbolicFactorGraph.run -true -false -true - - -make --j2 -testVector.run -true -true -true - - -make --j2 -testMatrix.run -true -true -true - - -make --j2 -testVSLAMGraph.run -true -true -true - - -make --j2 -testNonlinearEquality.run -true -true -true - - -make --j2 -testSQP.run -true -true -true - - -make --j2 -testNonlinearConstraint.run -true -true -true - - -make --j2 -testSQPOptimizer.run -true -true -true - - -make --j2 -testVSLAMConfig.run -true -true -true - - -make --j2 -testControlConfig.run -true -true -true - - -make --j2 -testControlPoint.run -true -true -true - - -make --j2 -testControlGraph.run -true -true -true - - -make --j2 -testOrdering.run -true -true -true - - -make --j2 -testPose2Constraint.run -true -true -true - - -make --j2 -testRot2.run -true -true -true - - -make --j2 -testGaussianBayesTree.run -true -true -true - - -make --j2 -testPose3Factor.run -true -true -true - - -make --j2 -testUrbanGraph.run -true -true -true - - -make --j2 -testUrbanConfig.run -true -true -true - - -make --j2 -testUrbanMeasurement.run -true -true -true - - -make --j2 -testUrbanOdometry.run -true -true -true - - -make --j2 -testIterative.run -true -true -true - - -make --j2 -testGaussianISAM2.run -true -true -true - - -make --j2 -testSubgraphPreconditioner.run -true -true -true - - -make --j2 -testBayesNetPreconditioner.run -true -true -true - - -make --j2 -testPose2Factor.run -true -true -true - - -make --j2 -timeRot3.run -true -true -true - - -make --j2 -install -true -true -true - - -make --j2 -testPose2Graph.run -true -true -true - - -make --j2 -testPose2Config.run -true -true -true - - -make --j2 -testLieConfig.run -true -true -true - - -make --j2 -testBearingFactor.run -true -true -true - - -make --j2 -install -true -true -true - - -make --j2 -clean -true -true -true - - -make --j2 -check -true -true -true - - - - + + + make + -j2 + install + true + true + true + + + make + -j2 + check + true + true + true + + + make + -k + check + true + false + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -f local.mk + testCal3_S2.run + true + true + true + + + make + -j2 + testVSLAMFactor.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + testGaussianConditional.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testGaussianFactorGraph.run + true + true + true + + + make + -j2 + testNonlinearFactorGraph.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + testVectorConfig.run + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + -j2 + testNonlinearFactor.run + true + true + true + + + make + -j2 + timeGaussianFactor.run + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + + testBayesTree.run + true + false + true + + + make + testSymbolicBayesNet.run + true + false + true + + + make + + testSymbolicFactorGraph.run + true + false + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testVSLAMGraph.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testSQP.run + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testSQPOptimizer.run + true + true + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + + + make + -j2 + testControlConfig.run + true + true + true + + + make + -j2 + testControlPoint.run + true + true + true + + + make + -j2 + testControlGraph.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + -j2 + testPose2Constraint.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testGaussianBayesTree.run + true + true + true + + + make + -j2 + testPose3Factor.run + true + true + true + + + make + -j2 + testUrbanGraph.run + true + true + true + + + make + -j2 + testUrbanConfig.run + true + true + true + + + make + -j2 + testUrbanMeasurement.run + true + true + true + + + make + -j2 + testUrbanOdometry.run + true + true + true + + + make + -j2 + testIterative.run + true + true + true + + + make + -j2 + testGaussianISAM2.run + true + true + true + + + make + -j2 + testSubgraphPreconditioner.run + true + true + true + + + make + -j2 + testBayesNetPreconditioner.run + true + true + true + + + make + -j2 + testPose2Factor.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + testPose2Graph.run + true + true + true + + + make + -j2 + testPose2Config.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testBearingFactor.run + true + true + true + + + make + + testGraph.run + true + false + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + + - - + + diff --git a/cpp/BetweenFactor.h b/cpp/BetweenFactor.h index 5509980c0..82ee8f3f4 100644 --- a/cpp/BetweenFactor.h +++ b/cpp/BetweenFactor.h @@ -79,6 +79,9 @@ namespace gtsam { inline const std::string& key1() const { return key1_;} inline const std::string& key2() const { return key2_;} + /** return the measured */ + inline const T measured() const {return measured_;} + /** keys as a list */ inline std::list keys() const { return keys_;} diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 42d8954b3..a5738868a 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -18,8 +18,6 @@ #include #include #include -#include -#include #include #include #include "Ordering.h" @@ -275,52 +273,16 @@ void FactorGraph::associateFactor(int index, sharedFactor factor) { template map FactorGraph::findMinimumSpanningTree() const { - typedef boost::adjacency_list< - boost::vecS, boost::vecS, boost::undirectedS, - boost::property, - boost::property > Graph; - typedef boost::graph_traits::vertex_descriptor Vertex; - typedef boost::graph_traits::vertex_iterator VertexIterator; - typedef boost::graph_traits::edge_descriptor Edge; - - // convert the factor graph to boost graph - Graph g(0); - map key2vertex; - Vertex v1, v2; - BOOST_FOREACH(sharedFactor factor, factors_){ - if (factor->keys().size() > 2) - throw(invalid_argument("findMinimumSpanningTree: only support factors with at most two keys")); - - if (factor->keys().size() == 1) - continue; - - string key1 = factor->keys().front(); - string key2 = factor->keys().back(); - - if (key2vertex.find(key1) == key2vertex.end()) { - v1 = add_vertex(key1, g); - key2vertex.insert(make_pair(key1, v1)); - } else - v1 = key2vertex[key1]; - - if (key2vertex.find(key2) == key2vertex.end()) { - v2 = add_vertex(key2, g); - key2vertex.insert(make_pair(key2, v2)); - } else - v2 = key2vertex[key2]; - - boost::property edge_property(1); // assume constant edge weight here - boost::add_edge(v1, v2, edge_property, g); - } + SDGraph g = gtsam::toBoostGraph, sharedFactor>(*this); // find minimum spanning tree - vector p_map(boost::num_vertices(g)); + vector p_map(boost::num_vertices(g)); prim_minimum_spanning_tree(g, &p_map[0]); // convert edge to string pairs map tree; - VertexIterator itVertex = boost::vertices(g).first; - for (vector::iterator vi = p_map.begin(); vi!=p_map.end(); itVertex++, vi++) { + BoostVertexIterator itVertex = boost::vertices(g).first; + for (vector::iterator 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()); diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 0681716a2..b1874a017 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -17,6 +17,7 @@ #include "Testable.h" #include "BayesNet.h" +#include "graph-inl.h" namespace gtsam { diff --git a/cpp/Makefile.am b/cpp/Makefile.am index a93a79c7e..b2d00eec2 100644 --- a/cpp/Makefile.am +++ b/cpp/Makefile.am @@ -66,14 +66,17 @@ testSymbolicBayesNet_LDADD = libgtsam.la # Inference headers += inference.h inference-inl.h +headers += graph-inl.h headers += FactorGraph.h FactorGraph-inl.h headers += BayesNet.h BayesNet-inl.h headers += BayesTree.h BayesTree-inl.h headers += ISAM.h ISAM-inl.h GaussianISAM.h headers += ISAM2.h ISAM2-inl.h GaussianISAM2.h sources += GaussianISAM.cpp GaussianISAM2.cpp -check_PROGRAMS += testFactorgraph testInference testOrdering +check_PROGRAMS += testGraph testFactorgraph testInference testOrdering check_PROGRAMS += testBayesTree testISAM testGaussianISAM testGaussianISAM2 +testGraph_SOURCES = testGraph.cpp +testGraph_LDADD = libgtsam.la testFactorgraph_SOURCES = testFactorgraph.cpp testInference_SOURCES = $(example) testInference.cpp testFactorgraph_LDADD = libgtsam.la diff --git a/cpp/Ordering.cpp b/cpp/Ordering.cpp index 4e5a4e2bb..8f14582e0 100644 --- a/cpp/Ordering.cpp +++ b/cpp/Ordering.cpp @@ -9,65 +9,44 @@ #include // for operator += #include #include -#include -#include +#include "graph-inl.h" #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) -class key_visitor : public boost::default_bfs_visitor { +class ordering_key_visitor : public boost::default_bfs_visitor { public: - key_visitor(Ordering& ordering_in) : ordering_(ordering_in) {} - template void discover_vertex(Vertex u, const Graph& g) const { - string key = boost::get(boost::vertex_name, g, u); + 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_; }; +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) { - typedef boost::adjacency_list > Graph; - typedef boost::graph_traits::vertex_descriptor Vertex; - - // build the graph corresponding to the predecessor map - Graph g(0); - map key2vertex; - Vertex v1, v2, root; - string child, parent; - bool foundRoot = false; - FOREACH_PAIR(child, parent, p_map) { - if (key2vertex.find(child) == key2vertex.end()) { - v1 = add_vertex(child, g); - key2vertex.insert(make_pair(child, v1)); - } else - v1 = key2vertex[child]; - - if (key2vertex.find(parent) == key2vertex.end()) { - v2 = add_vertex(parent, g); - key2vertex.insert(make_pair(parent, v2)); - } else - v2 = key2vertex[parent]; - - if (child.compare(parent) == 0) { - root = v1; - foundRoot = true; - } else - boost::add_edge(v1, v2, g); - } - - if (!foundRoot) - throw invalid_argument("Ordering: invalid predecessor map!"); + SGraph g; + SVertex root; + map key2vertex; + boost::tie(g, root, key2vertex) = predecessorMap2Graph(p_map); // breadth first visit on the graph - key_visitor vis(*this); + ordering_key_visitor vis(*this); boost::breadth_first_search(g, root, boost::visitor(vis)); } diff --git a/cpp/Pose2Graph.cpp b/cpp/Pose2Graph.cpp index c25b23d6b..a11d22e7a 100644 --- a/cpp/Pose2Graph.cpp +++ b/cpp/Pose2Graph.cpp @@ -3,11 +3,14 @@ * @brief A factor graph for the 2D PoseSLAM problem * @authors Frank Dellaert, Viorela Ila */ -//#include "NonlinearOptimizer-inl.h" #include "FactorGraph-inl.h" #include "NonlinearFactorGraph-inl.h" +#include "graph-inl.h" #include "Pose2Graph.h" +using namespace std; +using namespace gtsam; + namespace gtsam { // explicit instantiation so all the code is there and we can link with it @@ -15,8 +18,12 @@ template class FactorGraph > ; template class NonlinearFactorGraph ; //template class NonlinearOptimizer ; +/* ************************************************************************* */ + bool Pose2Graph::equals(const Pose2Graph& p, double tol) const { return false; } +/* ************************************************************************* */ + } // namespace gtsam diff --git a/cpp/graph-inl.h b/cpp/graph-inl.h new file mode 100644 index 000000000..ef0180f53 --- /dev/null +++ b/cpp/graph-inl.h @@ -0,0 +1,193 @@ +/* + * graph-inl.h + * + * Created on: Jan 11, 2010 + * Author: nikai + * Description: Graph algorithm using boost library + */ + +#pragma once + +#include +#include +#include +#include + +using namespace std; + +#define FOREACH_PAIR( KEY, VAL, COL) BOOST_FOREACH (boost::tie(KEY,VAL),COL) + +namespace gtsam { + +/* ************************************************************************* */ +/** + * type definitions + */ +typedef boost::adjacency_list< + boost::vecS, boost::vecS, boost::undirectedS, + boost::property, + boost::property > SDGraph; +typedef boost::graph_traits::vertex_descriptor BoostVertex; +typedef boost::graph_traits::vertex_iterator BoostVertexIterator; + +typedef boost::adjacency_list< + boost::vecS, boost::vecS, boost::directedS, + boost::property > SGraph; +typedef boost::graph_traits::vertex_descriptor SVertex; + +/* ************************************************************************* */ +/** + * Convert the factor graph to a boost undirected graph + */ +template +SDGraph toBoostGraph(const G& graph) { + // convert the factor graph to boost graph + SDGraph g(0); + map key2vertex; + BoostVertex v1, v2; + BOOST_FOREACH(F factor, graph) { + if (factor->keys().size() > 2) + throw(invalid_argument("toBoostGraph: only support factors with at most two keys")); + + if (factor->keys().size() == 1) + continue; + + string key1 = factor->keys().front(); + string key2 = factor->keys().back(); + + if (key2vertex.find(key1) == key2vertex.end()) { + v1 = add_vertex(key1, g); + key2vertex.insert(make_pair(key1, v1)); + } else + v1 = key2vertex[key1]; + + if (key2vertex.find(key2) == key2vertex.end()) { + v2 = add_vertex(key2, g); + key2vertex.insert(make_pair(key2, v2)); + } else + v2 = key2vertex[key2]; + + boost::property edge_property(1.0); // assume constant edge weight here + boost::add_edge(v1, v2, edge_property, g); + } + + return g; +} + +/* ************************************************************************* */ +/** + * build the graph corresponding to the predecessor map. Excute action for each edge. + */ +template +boost::tuple > predecessorMap2Graph(const map& p_map) { + + G g(0); + map key2vertex; + V v1, v2, root; + string child, parent; + bool foundRoot = false; + FOREACH_PAIR(child, parent, p_map) { + if (key2vertex.find(child) == key2vertex.end()) { + v1 = add_vertex(child, g); + key2vertex.insert(make_pair(child, v1)); + } else + v1 = key2vertex[child]; + + if (key2vertex.find(parent) == key2vertex.end()) { + v2 = add_vertex(parent, g); + key2vertex.insert(make_pair(parent, v2)); + } else + v2 = key2vertex[parent]; + + if (child.compare(parent) == 0) { + root = v1; + foundRoot = true; + } else + boost::add_edge(v2, v1, g); // edge is from parent to child + } + + if (!foundRoot) + throw invalid_argument("predecessorMap2Graph: invalid predecessor map!"); + + return boost::tuple >(g, root, key2vertex); +} + +/* ************************************************************************* */ +/** + * Visit each edge and compose the poses + */ +template +class compose_key_visitor : public boost::default_bfs_visitor { +public: + compose_key_visitor(PoseConfig& config_in): config(config_in) {} + template void tree_edge(Edge edge, const Graph& g) const { + string key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); + string key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); + Pose relativePose = boost::get(boost::edge_weight, g, edge); + config.insert(key_to, compose(config.get(key_from), relativePose)); + } + +private: + PoseConfig& config; + +}; + +/* ************************************************************************* */ +/** + * Compose the poses by following the chain sepcified by the spanning tree + */ +template +Config composePoses(const G& graph, const map& tree, + const Pose& rootPose) { + + //TODO: change edge_weight_t to edge_pose_t + typedef typename boost::adjacency_list< + boost::vecS, boost::vecS, boost::directedS, + boost::property, + boost::property > PoseGraph; + typedef typename boost::graph_traits::vertex_descriptor PoseVertex; + typedef typename boost::graph_traits::edge_descriptor PoseEdge; + + PoseGraph g; + PoseVertex root; + map key2vertex; + boost::tie(g, root, key2vertex) = predecessorMap2Graph(tree); + + // attach the relative poses to the edges + PoseEdge edge1, edge2; + bool found1, found2; + BOOST_FOREACH(typename G::sharedFactor nl_factor, graph) { + + if (nl_factor->keys().size() > 2) + throw invalid_argument("composePoses: only support factors with at most two keys"); + if (nl_factor->keys().size() == 1) continue; + + // e.g. in pose2graph, nonlinear factor needs to be converted to pose2factor + boost::shared_ptr factor = boost::dynamic_pointer_cast(nl_factor); + + PoseVertex v_from = key2vertex.find(factor->keys().front())->second; + PoseVertex v_to = key2vertex.find(factor->keys().back())->second; + + Pose measured = factor->measured(); + tie(edge1, found1) = boost::edge(v_from, v_to, g); + tie(edge2, found2) = boost::edge(v_to, v_from, g); + if ((found1 && found2) || (!found1 && !found2)) + throw invalid_argument ("composePoses: invalid spanning tree"); + if (found1) + boost::put(boost::edge_weight, g, edge1, measured); + else if (found2) + boost::put(boost::edge_weight, g, edge2, inverse(measured)); + } + + // compose poses + Config config; + config.insert(boost::get(boost::vertex_name, g, root), rootPose); + compose_key_visitor vis(config); + boost::breadth_first_search(g, root, boost::visitor(vis)); + + return config; +} + +/* ************************************************************************* */ + +} diff --git a/cpp/testGraph.cpp b/cpp/testGraph.cpp new file mode 100644 index 000000000..5f9efa689 --- /dev/null +++ b/cpp/testGraph.cpp @@ -0,0 +1,52 @@ +/* + * testGraph.cpp + * + * Created on: Jan 12, 2010 + * Author: nikai + * Description: unit test for graph-inl.h + */ + +#include +#include +#include + +#include + +#include "Pose2Graph.h" +#include "graph-inl.h" + +using namespace std; +using namespace boost; +using namespace gtsam; + +/* ************************************************************************* */ +TEST( Graph, composePoses ) +{ + Pose2Graph graph; + Matrix cov = eye(3); + graph.push_back(boost::shared_ptr(new Pose2Factor("x1", "x2", Pose2(2.0, 0.0, 0.0), cov))); + graph.push_back(boost::shared_ptr(new Pose2Factor("x2", "x3", Pose2(3.0, 0.0, 0.0), cov))); + + map tree; + tree.insert(make_pair("x1", "x2")); + tree.insert(make_pair("x2", "x2")); + tree.insert(make_pair("x3", "x2")); + + Pose2 rootPose(3.0, 0.0, 0.0); + + Pose2Config actual = composePoses(graph, tree, rootPose); + Pose2Config expected; + expected.insert("x1", Pose2(1.0, 0.0, 0.0)); + expected.insert("x2", Pose2(3.0, 0.0, 0.0)); + expected.insert("x3", Pose2(6.0, 0.0, 0.0)); + + LONGS_EQUAL(3, actual.size()); + CHECK(assert_equal(expected, actual)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */