12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

made removeAndCombineFactors a function, not a method

release/4.3a0
Frank Dellaert 2009-11-11 05:12:45 +00:00
parent 710d396d0b
commit 8d2d48d252
4 changed files with 17 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cpp/FactorGraph-inl.h
View File

@ -205,15 +205,11 @@ FactorGraph<Factor>::findAndRemoveFactors(const string& key) {
/* ************************************************************************* */
/* find factors and remove them from the factor graph: O(n) */
/* ************************************************************************* */
template<class Factor>
typename FactorGraph<Factor>::sharedFactor
FactorGraph<Factor>::removeAndCombineFactors(const string& key)
template<class Factor> boost::shared_ptr<Factor>
removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const string& key)
{
bool verbose = false;
if (verbose) cout << "FactorGraph::removeAndCombineFactors" << endl;
vector<sharedFactor> found = findAndRemoveFactors(key);
sharedFactor new_factor(new Factor(found));
if (verbose) cout << "FactorGraph::removeAndCombineFactors done" << endl;
vector<boost::shared_ptr<Factor> > found = factorGraph.findAndRemoveFactors(key);
boost::shared_ptr<Factor> new_factor(new Factor(found));
return new_factor;
}
@ -225,7 +221,7 @@ boost::shared_ptr<Conditional> _eliminateOne(FactorGraph<Factor>& 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<Factor> joint_factor = graph.removeAndCombineFactors(key);
boost::shared_ptr<Factor> joint_factor = removeAndCombineFactors(graph,key);
// eliminate that joint factor
boost::shared_ptr<Factor> factor;

16
cpp/FactorGraph.h
View File

@ -104,13 +104,6 @@ namespace gtsam {
*/
std::vector<sharedFactor> 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<class Factor> boost::shared_ptr<Factor>
removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const std::string& key);
/** doubly templated functions */
/**

4
cpp/testLinearFactorGraph.cpp
View File

@ -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,

2
cpp/testSymbolicFactorGraph.cpp
View File

@ -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<string> keys; keys.push_back("l1"); keys.push_back("x1"); keys.push_back("x2");
SymbolicFactor expected(keys);