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

push_back an entire factor graph

release/4.3a0
Frank Dellaert 2009-11-19 06:34:07 +00:00
parent e2a3d1cb8f
commit b2eee54b5a
3 changed files with 48 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
cpp/FactorGraph-inl.h
View File

@ -99,6 +99,14 @@ void FactorGraph<Factor>::push_back(sharedFactor factor) {
}
}
/* ************************************************************************* */
template<class Factor>
void FactorGraph<Factor>::push_back(const FactorGraph<Factor>& factors) {
const_iterator factor = factors.begin();
for (; factor!= factors.end(); factor++)
push_back(*factor);
}
/* ************************************************************************* */
template<class Factor>
Ordering FactorGraph<Factor>::keys() const {
@ -230,16 +238,11 @@ removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const string& key)
/* ************************************************************************* */
template<class Factor>
FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& fg2) {
typedef FactorGraph<Factor> FG;
// create new linear factor graph equal to the first one
FG fg = fg1;
FactorGraph<Factor> fg = fg1;
// add the second factors_ in the graph
typename FG::const_iterator factor = fg2.begin();
for (; factor!= fg2.end(); factor++)
fg.push_back(*factor);
fg.push_back(fg2);
return fg;
}

3
cpp/FactorGraph.h
View File

@ -86,6 +86,9 @@ namespace gtsam {
/** Add a factor */
void push_back(sharedFactor factor);
/** push back many factors */
void push_back(const FactorGraph<Factor>& factors);
/** return keys in some random order */
Ordering keys() const;

35
cpp/testSymbolicFactorGraph.cpp
View File

@ -180,6 +180,41 @@ TEST( SymbolicFactorGraph, constructFromBayesNet )
CHECK(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST( SymbolicFactorGraph, push_back )
{
// Create two factor graphs and expected combined graph
SymbolicFactorGraph fg1, fg2, expected;
list<string> f1_keys; f1_keys += "x1";
SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys));
fg1.push_back(f1);
expected.push_back(f1);
list<string> f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2");
SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys));
fg1.push_back(f2);
expected.push_back(f2);
list<string> f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1");
SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys));
fg2.push_back(f3);
expected.push_back(f3);
list<string> f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2");
SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys));
fg2.push_back(f4);
expected.push_back(f4);
// combine
SymbolicFactorGraph actual = combine(fg1,fg2);
CHECK(assert_equal(expected, actual));
// combine using push_back
fg1.push_back(fg2);
CHECK(assert_equal(expected, fg1));
}
/* ************************************************************************* */
int main() {
TestResult tr;