diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 03b17d905..d880baaa3 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -73,34 +73,41 @@ SDGraph toBoostGraph(const G& graph) { SDGraph g; typedef typename boost::graph_traits >::vertex_descriptor BoostVertex; std::map key2vertex; - BoostVertex v1, v2; typename G::const_iterator itFactor; + // Loop over the factors for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) { - if ((*itFactor)->keys().size() > 2) - throw(std::invalid_argument("toBoostGraph: only support factors with at most two keys")); - if ((*itFactor)->keys().size() == 1) + // Ignore factors that are not binary + if ((*itFactor)->keys().size() != 2) continue; + // Cast the factor to the user-specified factor type F boost::shared_ptr factor = boost::dynamic_pointer_cast(*itFactor); + // Ignore factors that are not of type F if (!factor) continue; - KEY key1 = factor->key1(); - KEY key2 = factor->key2(); + // Retrieve the 2 keys (nodes) the factor (edge) is incident on + KEY key1 = factor->keys()[0]; + KEY key2 = factor->keys()[1]; + BoostVertex v1, v2; + + // If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id if (key2vertex.find(key1) == key2vertex.end()) { v1 = add_vertex(key1, g); key2vertex.insert(std::pair(key1, v1)); } else v1 = key2vertex[key1]; + // If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id if (key2vertex.find(key2) == key2vertex.end()) { v2 = add_vertex(key2, g); key2vertex.insert(std::pair(key2, v2)); } else v2 = key2vertex[key2]; + // Add an edge with weight 1.0 boost::property edge_property(1.0); // assume constant edge weight here boost::add_edge(v1, v2, edge_property, g); } @@ -222,12 +229,11 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap return config; } -/* ************************************************************************* */ - /* ************************************************************************* */ template PredecessorMap findMinimumSpanningTree(const G& fg) { + // Convert to a graph that boost understands SDGraph g = gtsam::toBoostGraph(fg); // find minimum spanning tree diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index 675bbaaad..920de5609 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -109,19 +109,6 @@ TEST( Graph, composePoses ) } ///* ************************************************************************* */ -// A linear factor implementing the functions key1 and key2 -// needed for findMinimumSpanningTree -class Factor2 : public JacobianFactor { - public: - - /** Construct binary factor */ - Factor2(Key i1, const Matrix& A1, Key i2, const Matrix& A2, const Vector& b, - const SharedDiagonal& model = SharedDiagonal()) : - JacobianFactor(i1, A1, i2, A2, b, model) { - } - Key key1() const {return keys_[0];} - Key key2() const {return keys_[1];} -}; TEST( GaussianFactorGraph, findMinimumSpanningTree ) { @@ -130,14 +117,14 @@ TEST( GaussianFactorGraph, findMinimumSpanningTree ) Vector2 b(0, 0); const SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 0.5, 0.5)); using namespace symbol_shorthand; - g += Factor2(X(1), I, X(2), I, b, model); - g += Factor2(X(1), I, X(3), I, b, model); - g += Factor2(X(1), I, X(4), I, b, model); - g += Factor2(X(2), I, X(3), I, b, model); - g += Factor2(X(2), I, X(4), I, b, model); - g += Factor2(X(3), I, X(4), I, b, model); + g += JacobianFactor(X(1), I, X(2), I, b, model); + g += JacobianFactor(X(1), I, X(3), I, b, model); + g += JacobianFactor(X(1), I, X(4), I, b, model); + g += JacobianFactor(X(2), I, X(3), I, b, model); + g += JacobianFactor(X(2), I, X(4), I, b, model); + g += JacobianFactor(X(3), I, X(4), I, b, model); - PredecessorMap tree = findMinimumSpanningTree(g); + PredecessorMap tree = findMinimumSpanningTree(g); EXPECT_LONGS_EQUAL(tree[X(1)], X(1)); EXPECT_LONGS_EQUAL(tree[X(2)], X(1)); EXPECT_LONGS_EQUAL(tree[X(3)], X(1));