diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 823aa0345..3865f277a 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -27,7 +27,7 @@ namespace gtsam { template boost::shared_ptr::BayesNetType> EliminateableFactorGraph::eliminateSequential( - const Eliminate& function, OptionalOrdering ordering, OptionalVariableIndex variableIndex) const + OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const { if(ordering && variableIndex) { // Do elimination @@ -43,13 +43,13 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateSequential(function, ordering, VariableIndexUnordered(asDerived())); + return eliminateSequential(ordering, function, VariableIndexUnordered(asDerived())); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - return eliminateSequential(function, OrderingUnordered::COLAMD(*variableIndex)); + return eliminateSequential(OrderingUnordered::COLAMD(*variableIndex), function); } } @@ -57,7 +57,7 @@ namespace gtsam { template boost::shared_ptr::BayesTreeType> EliminateableFactorGraph::eliminateMultifrontal( - const Eliminate& function, OptionalOrdering ordering, OptionalVariableIndex variableIndex) const + OptionalOrdering ordering, const Eliminate& function, OptionalVariableIndex variableIndex) const { if(ordering && variableIndex) { // Do elimination with given ordering @@ -73,13 +73,13 @@ namespace gtsam { // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // for no variable index first so that it's always computed if we need to call COLAMD because // no Ordering is provided. - return eliminateMultifrontal(function, ordering, VariableIndexUnordered(asDerived())); + return eliminateMultifrontal(ordering, function, VariableIndexUnordered(asDerived())); } else /*if(!ordering)*/ { // If no Ordering provided, compute one and call this function again. We are guaranteed to // have a VariableIndex already here because we computed one if needed in the previous 'else' // block. - return eliminateMultifrontal(function, OrderingUnordered::COLAMD(*variableIndex)); + return eliminateMultifrontal(OrderingUnordered::COLAMD(*variableIndex), function); } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index 405b6184c..8c5daf529 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -113,8 +113,8 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateSequential( - const Eliminate& function = EliminationTraits::DefaultEliminate, OptionalOrdering ordering = boost::none, + const Eliminate& function = EliminationTraits::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; /** Do multifrontal elimination of all variables to produce a Bayes tree. If an ordering is not @@ -138,8 +138,8 @@ namespace gtsam { * \endcode * */ boost::shared_ptr eliminateMultifrontal( - const Eliminate& function = EliminationTraits::DefaultEliminate, OptionalOrdering ordering = boost::none, + const Eliminate& function = EliminationTraits::DefaultEliminate, OptionalVariableIndex variableIndex = boost::none) const; /** Do sequential elimination of some variables in the given \c ordering to produce a Bayes net diff --git a/gtsam/linear/GaussianFactorGraphUnordered.cpp b/gtsam/linear/GaussianFactorGraphUnordered.cpp index 9f0cd937a..39048509a 100644 --- a/gtsam/linear/GaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/GaussianFactorGraphUnordered.cpp @@ -204,7 +204,7 @@ namespace gtsam { /* ************************************************************************* */ VectorValuesUnordered GaussianFactorGraphUnordered::optimize(const Eliminate& function) const { - return BaseEliminateable::eliminateMultifrontal(function)->optimize(); + return BaseEliminateable::eliminateMultifrontal(boost::none, function)->optimize(); } /* ************************************************************************* */ diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index f41fa706d..d613136a7 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include #include @@ -32,12 +34,11 @@ TEST(SymbolicFactorGraph, eliminateFullSequential) // Test with simpleTestGraph1 OrderingUnordered order; order += 0,1,2,3,4; - SymbolicBayesNetUnordered actual1 = *simpleTestGraph1.eliminateSequential(EliminateSymbolicUnordered, order); + SymbolicBayesNetUnordered actual1 = *simpleTestGraph1.eliminateSequential(order); EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1)); // Test with Asia graph - SymbolicBayesNetUnordered actual2 = *asiaGraph.eliminateSequential( - EliminateSymbolicUnordered, asiaOrdering); + SymbolicBayesNetUnordered actual2 = *asiaGraph.eliminateSequential(asiaOrdering); EXPECT(assert_equal(asiaBayesNet, actual2)); } @@ -58,8 +59,8 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) SymbolicBayesNetUnordered::shared_ptr actualBayesNet; SymbolicFactorGraphUnordered::shared_ptr actualSfg; - boost::tie(actualBayesNet, actualSfg) = simpleTestGraph2.eliminatePartialSequential( - EliminateSymbolicUnordered, OrderingUnordered(list_of(0)(1))); + boost::tie(actualBayesNet, actualSfg) = + simpleTestGraph2.eliminatePartialSequential(OrderingUnordered(list_of(0)(1))); EXPECT(assert_equal(expectedSfg, *actualSfg)); EXPECT(assert_equal(expectedBayesNet, *actualBayesNet)); @@ -70,11 +71,11 @@ TEST(SymbolicFactorGraph, eliminateFullMultifrontal) { OrderingUnordered ordering; ordering += 0,1,2,3; SymbolicBayesTreeUnordered actual1 = - *simpleChain.eliminateMultifrontal(EliminateSymbolicUnordered, ordering); + *simpleChain.eliminateMultifrontal(ordering); EXPECT(assert_equal(simpleChainBayesTree, actual1)); SymbolicBayesTreeUnordered actual2 = - *asiaGraph.eliminateMultifrontal(EliminateSymbolicUnordered, asiaOrdering); + *asiaGraph.eliminateMultifrontal(asiaOrdering); EXPECT(assert_equal(asiaBayesTree, actual2)); } @@ -95,8 +96,8 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) SymbolicBayesTreeUnordered::shared_ptr actualBayesTree; SymbolicFactorGraphUnordered::shared_ptr actualFactorGraph; - boost::tie(actualBayesTree, actualFactorGraph) = simpleTestGraph2.eliminatePartialMultifrontal( - EliminateSymbolicUnordered, OrderingUnordered(list_of(4)(5))); + boost::tie(actualBayesTree, actualFactorGraph) = + simpleTestGraph2.eliminatePartialMultifrontal(OrderingUnordered(list_of(4)(5))); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph)); EXPECT(assert_equal(expectedBayesTree, *actualBayesTree)); @@ -120,52 +121,91 @@ TEST(SymbolicFactorGraph, eliminate_disconnected_graph) { OrderingUnordered order; order += 0,1,2,3,4; - SymbolicBayesNetUnordered actual = *fg.eliminateSequential(EliminateSymbolicUnordered, order); + SymbolicBayesNetUnordered actual = *fg.eliminateSequential(order); EXPECT(assert_equal(expected,actual)); } /* ************************************************************************* */ -//TEST(SymbolicFactorGraph, eliminateFrontals) { -// -// SymbolicFactorGraph sfgOrig; -// sfgOrig.push_factor(0,1); -// sfgOrig.push_factor(0,2); -// sfgOrig.push_factor(1,3); -// sfgOrig.push_factor(1,4); -// sfgOrig.push_factor(2,3); -// sfgOrig.push_factor(4,5); -// -// SymbolicConditionalUnordered::shared_ptr actualCond; -// SymbolicFactorGraph::shared_ptr actualSfg; -// boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2); -// -// vector condIndices; -// condIndices += 0,1,2,3,4; -// IndexConditional expectedCond(condIndices, 2); -// -// SymbolicFactorGraph expectedSfg; -// expectedSfg.push_factor(2,3); -// expectedSfg.push_factor(4,5); -// expectedSfg.push_factor(2,3,4); -// -// EXPECT(assert_equal(expectedSfg, actualSfg)); -// EXPECT(assert_equal(expectedCond, *actualCond)); -//} - -///* ************************************************************************* */ -//TEST( SymbolicFactorGraph, EliminateOne ) +//TEST(SymbolicFactorGraph, marginals) //{ -// // create a test graph +// // Create factor graph // SymbolicFactorGraph fg; -// fg.push_factor(vx2, vx1); +// fg.push_factor(0, 1); +// fg.push_factor(0, 2); +// fg.push_factor(1, 4); +// fg.push_factor(2, 4); +// fg.push_factor(3, 4); // -// SymbolicSequentialSolver::EliminateUntil(fg, vx2+1); -// SymbolicFactorGraph expected; -// expected.push_back(boost::shared_ptr()); -// expected.push_factor(vx1); +// // eliminate +// SymbolicSequentialSolver solver(fg); +// SymbolicBayesNet::shared_ptr actual = solver.eliminate(); +// SymbolicBayesNet expected; +// expected.push_front(boost::make_shared(4)); +// expected.push_front(boost::make_shared(3, 4)); +// expected.push_front(boost::make_shared(2, 4)); +// expected.push_front(boost::make_shared(1, 2, 4)); +// expected.push_front(boost::make_shared(0, 1, 2)); +// EXPECT(assert_equal(expected,*actual)); // -// CHECK(assert_equal(expected, fg)); +// { +// // jointBayesNet +// vector js; +// js.push_back(0); +// js.push_back(4); +// js.push_back(3); +// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); +// SymbolicBayesNet expectedBN; +// expectedBN.push_front(boost::make_shared(3)); +// expectedBN.push_front(boost::make_shared(4, 3)); +// expectedBN.push_front(boost::make_shared(0, 4)); +// EXPECT( assert_equal(expectedBN,*actualBN)); +// +// // jointFactorGraph +// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); +// SymbolicFactorGraph expectedFG; +// expectedFG.push_factor(0, 4); +// expectedFG.push_factor(4, 3); +// expectedFG.push_factor(3); +// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); +// } +// +// { +// // jointBayesNet +// vector js; +// js.push_back(0); +// js.push_back(2); +// js.push_back(3); +// SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js); +// SymbolicBayesNet expectedBN; +// expectedBN.push_front(boost::make_shared(2)); +// expectedBN.push_front(boost::make_shared(3, 2)); +// expectedBN.push_front(boost::make_shared(0, 3, 2)); +// EXPECT( assert_equal(expectedBN,*actualBN)); +// +// // jointFactorGraph +// SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js); +// SymbolicFactorGraph expectedFG; +// expectedFG.push_factor(0, 3, 2); +// expectedFG.push_factor(3, 2); +// expectedFG.push_factor(2); +// EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG))); +// } +// +// { +// // conditionalBayesNet +// vector js; +// js.push_back(0); +// js.push_back(2); +// js.push_back(3); +// size_t nrFrontals = 2; +// SymbolicBayesNet::shared_ptr actualBN = // +// solver.conditionalBayesNet(js, nrFrontals); +// SymbolicBayesNet expectedBN; +// expectedBN.push_front(boost::make_shared(2, 3)); +// expectedBN.push_front(boost::make_shared(0, 2, 3)); +// EXPECT( assert_equal(expectedBN,*actualBN)); +// } //} /* ************************************************************************* */