diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h index 47ae4dbb0..86371920c 100644 --- a/cpp/FactorGraph-inl.h +++ b/cpp/FactorGraph-inl.h @@ -205,15 +205,11 @@ FactorGraph::findAndRemoveFactors(const string& key) { /* ************************************************************************* */ /* find factors and remove them from the factor graph: O(n) */ /* ************************************************************************* */ -template -typename FactorGraph::sharedFactor -FactorGraph::removeAndCombineFactors(const string& key) +template boost::shared_ptr +removeAndCombineFactors(FactorGraph& factorGraph, const string& key) { - bool verbose = false; - if (verbose) cout << "FactorGraph::removeAndCombineFactors" << endl; - vector found = findAndRemoveFactors(key); - sharedFactor new_factor(new Factor(found)); - if (verbose) cout << "FactorGraph::removeAndCombineFactors done" << endl; + vector > found = factorGraph.findAndRemoveFactors(key); + boost::shared_ptr new_factor(new Factor(found)); return new_factor; } @@ -225,7 +221,7 @@ boost::shared_ptr _eliminateOne(FactorGraph& graph, const s // combine the factors of all nodes connected to the variable to be eliminated // if no factors are connected to key, returns an empty factor - boost::shared_ptr joint_factor = graph.removeAndCombineFactors(key); + boost::shared_ptr joint_factor = removeAndCombineFactors(graph,key); // eliminate that joint factor boost::shared_ptr factor; diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h index 3f25b7cc4..f1e87f342 100644 --- a/cpp/FactorGraph.h +++ b/cpp/FactorGraph.h @@ -104,13 +104,6 @@ namespace gtsam { */ std::vector findAndRemoveFactors(const std::string& key); - /** - * extract and combine all the factors that involve a given node - * @param key the key for the given node - * @return the combined linear factor - */ - sharedFactor removeAndCombineFactors(const std::string& key); - private: /** Serialization function */ @@ -122,6 +115,15 @@ namespace gtsam { } }; // FactorGraph + /** + * Extract and combine all the factors that involve a given node + * Put this here as not all Factors have a combine constructor + * @param key the key for the given node + * @return the combined linear factor + */ + template boost::shared_ptr + removeAndCombineFactors(FactorGraph& factorGraph, const std::string& key); + /** doubly templated functions */ /** diff --git a/cpp/testLinearFactorGraph.cpp b/cpp/testLinearFactorGraph.cpp index ecfe46837..f5ae30748 100644 --- a/cpp/testLinearFactorGraph.cpp +++ b/cpp/testLinearFactorGraph.cpp @@ -79,7 +79,7 @@ TEST( LinearFactorGraph, combine_factors_x1 ) Vector sigmas = Vector_(6, sigma1, sigma1, sigma2, sigma2, sigma3, sigma3); // combine all factors - LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x1"); + LinearFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1"); // the expected linear factor Matrix Al1 = Matrix_(6,2, @@ -141,7 +141,7 @@ TEST( LinearFactorGraph, combine_factors_x2 ) Vector sigmas = Vector_(4, sigma1, sigma1, sigma2, sigma2); // combine all factors - LinearFactor::shared_ptr actual = fg.removeAndCombineFactors("x2"); + LinearFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2"); // the expected linear factor Matrix Al1 = Matrix_(4,2, diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp index d6a742785..e17c96621 100644 --- a/cpp/testSymbolicFactorGraph.cpp +++ b/cpp/testSymbolicFactorGraph.cpp @@ -94,7 +94,7 @@ TEST( LinearFactorGraph, removeAndCombineFactors ) SymbolicFactorGraph fg(factorGraph); // combine all factors connected to x1 - SymbolicFactor::shared_ptr actual = fg.removeAndCombineFactors("x1"); + SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1"); list keys; keys.push_back("l1"); keys.push_back("x1"); keys.push_back("x2"); SymbolicFactor expected(keys);