diff --git a/gtsam/inference/tests/testSymbolicFactor.cpp b/gtsam/inference/tests/testSymbolicFactor.cpp index 0c25a6179..5f57c9914 100644 --- a/gtsam/inference/tests/testSymbolicFactor.cpp +++ b/gtsam/inference/tests/testSymbolicFactor.cpp @@ -19,8 +19,10 @@ #include #include #include +#include #include +#include using namespace std; using namespace gtsam; @@ -45,6 +47,37 @@ TEST(SymbolicFactor, eliminate) { CHECK(assert_equal(**fragmentCond++, *expected2)); } +/* ************************************************************************* */ +TEST(SymbolicFactor, CombineAndEliminate) { + SymbolicFactorGraph factors; + factors.push_factor(2,4,6); + factors.push_factor(1,2,5); + factors.push_factor(0,3); + + IndexFactor expected_factor(4,5,6); + BayesNet expected_bn; + vector parents; + + parents.clear(); parents += 1,2,3,4,5,6; + expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(0, parents))); + + parents.clear(); parents += 2,3,4,5,6; + expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(1, parents))); + + parents.clear(); parents += 3,4,5,6; + expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(2, parents))); + + parents.clear(); parents += 4,5,6; + expected_bn.push_back(IndexConditional::shared_ptr(new IndexConditional(3, parents))); + + BayesNet::shared_ptr actual_bn; + IndexFactor::shared_ptr actual_factor; + boost::tie(actual_bn, actual_factor) = IndexFactor::CombineAndEliminate(factors, 4); + + CHECK(assert_equal(expected_bn, *actual_bn)); + CHECK(assert_equal(expected_factor, *actual_factor)); +} + /* ************************************************************************* */ int main() { TestResult tr;