diff --git a/.cproject b/.cproject
index 0b3a13e37..766138423 100644
--- a/.cproject
+++ b/.cproject
@@ -849,10 +849,10 @@
make
- -j5
+ -j1
testSymbolicSequentialSolver.run
true
- true
+ false
true
diff --git a/gtsam/inference/GenericSequentialSolver-inl.h b/gtsam/inference/GenericSequentialSolver-inl.h
index 368aae831..19e2d76d4 100644
--- a/gtsam/inference/GenericSequentialSolver-inl.h
+++ b/gtsam/inference/GenericSequentialSolver-inl.h
@@ -82,45 +82,54 @@ namespace gtsam {
return eliminationTree_->eliminate(function);
}
- /* ************************************************************************* */
- template
- typename FactorGraph::shared_ptr //
- GenericSequentialSolver::jointFactorGraph(
- const std::vector& js, Eliminate function) const {
+ /* ************************************************************************* */
+ template
+ typename BayesNet::shared_ptr //
+ GenericSequentialSolver::jointBayesNet(
+ const std::vector& js, Eliminate function) const {
- // Compute a COLAMD permutation with the marginal variables constrained to the end.
- Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js));
- Permutation::shared_ptr permutationInverse(permutation->inverse());
+ // Compute a COLAMD permutation with the marginal variables constrained to the end.
+ Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js));
+ Permutation::shared_ptr permutationInverse(permutation->inverse());
- // Permute the factors - NOTE that this permutes the original factors, not
- // copies. Other parts of the code may hold shared_ptr's to these factors so
- // we must undo the permutation before returning.
- BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_)
- if (factor) factor->permuteWithInverse(*permutationInverse);
+ // Permute the factors - NOTE that this permutes the original factors, not
+ // copies. Other parts of the code may hold shared_ptr's to these factors so
+ // we must undo the permutation before returning.
+ BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_)
+ if (factor) factor->permuteWithInverse(*permutationInverse);
- // Eliminate all variables
- typename BayesNet::shared_ptr
- bayesNet(EliminationTree::Create(*factors_)->eliminate(function));
+ // Eliminate all variables
+ typename BayesNet::shared_ptr
+ bayesNet(EliminationTree::Create(*factors_)->eliminate(function));
- // Undo the permuation on the original factors and on the structure.
- BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_)
- if (factor) factor->permuteWithInverse(*permutation);
+ // Undo the permutation on the original factors and on the structure.
+ BOOST_FOREACH(const typename boost::shared_ptr& factor, *factors_)
+ if (factor) factor->permuteWithInverse(*permutation);
- // Take the joint marginal from the Bayes net.
- sharedFactorGraph joint(new FactorGraph );
- joint->reserve(js.size());
- typename BayesNet::const_reverse_iterator
- conditional = bayesNet->rbegin();
+ // Get rid of conditionals on variables that we want to marginalize out
+ size_t nrMarginalizedOut = bayesNet->size()-js.size();
+ for(int i=0;ipop_front();
- for (size_t i = 0; i < js.size(); ++i)
- joint->push_back((*(conditional++))->toFactor());
+ // Undo the permutation on the conditionals
+ BOOST_FOREACH(const boost::shared_ptr& c, *bayesNet)
+ c->permuteWithInverse(*permutation);
- // Undo the permutation on the eliminated joint marginal factors
- BOOST_FOREACH(const typename boost::shared_ptr& factor, *joint)
- factor->permuteWithInverse(*permutation);
+ return bayesNet;
+ }
- return joint;
- }
+ /* ************************************************************************* */
+ template
+ typename FactorGraph::shared_ptr //
+ GenericSequentialSolver::jointFactorGraph(
+ const std::vector& js, Eliminate function) const {
+
+ // Eliminate all variables
+ typename BayesNet::shared_ptr
+ bayesNet = jointBayesNet(js,function);
+
+ return boost::make_shared >(*bayesNet);
+ }
/* ************************************************************************* */
template
diff --git a/gtsam/inference/GenericSequentialSolver.h b/gtsam/inference/GenericSequentialSolver.h
index 5b4d4b41e..5299b1afe 100644
--- a/gtsam/inference/GenericSequentialSolver.h
+++ b/gtsam/inference/GenericSequentialSolver.h
@@ -51,10 +51,8 @@ namespace gtsam {
protected:
typedef boost::shared_ptr > sharedFactorGraph;
-
- typedef std::pair<
- boost::shared_ptr,
- boost::shared_ptr > EliminationResult;
+ typedef typename FACTOR::ConditionalType Conditional;
+ typedef std::pair, boost::shared_ptr > EliminationResult;
typedef boost::function&, size_t)> Eliminate;
/** Store the original factors for computing marginals
@@ -117,20 +115,29 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
- typename boost::shared_ptr > eliminate(Eliminate function) const;
+ typename boost::shared_ptr >
+ eliminate(Eliminate function) const;
- /**
- * Compute the marginal joint over a set of variables, by integrating out
- * all of the other variables. Returns the result as a factor graph.
- */
- typename FactorGraph::shared_ptr jointFactorGraph(
- const std::vector& js, Eliminate function) const;
+ /**
+ * Compute the marginal joint over a set of variables, by integrating out
+ * all of the other variables. Returns the result as a Bayes net
+ */
+ typename BayesNet::shared_ptr
+ jointBayesNet(const std::vector& js, Eliminate function) const;
+
+ /**
+ * Compute the marginal joint over a set of variables, by integrating out
+ * all of the other variables. Returns the result as a factor graph.
+ */
+ typename FactorGraph::shared_ptr
+ jointFactorGraph(const std::vector& js, Eliminate function) const;
/**
* Compute the marginal Gaussian density over a variable, by integrating out
* all of the other variables. This function returns the result as a factor.
*/
- typename boost::shared_ptr marginalFactor(Index j, Eliminate function) const;
+ typename boost::shared_ptr
+ marginalFactor(Index j, Eliminate function) const;
/// @}
diff --git a/gtsam/inference/SymbolicSequentialSolver.h b/gtsam/inference/SymbolicSequentialSolver.h
index c114261f0..f023acbaf 100644
--- a/gtsam/inference/SymbolicSequentialSolver.h
+++ b/gtsam/inference/SymbolicSequentialSolver.h
@@ -52,13 +52,22 @@ namespace gtsam {
* Eliminate the factor graph sequentially. Uses a column elimination tree
* to recursively eliminate.
*/
- SymbolicBayesNet::shared_ptr eliminate() const { return Base::eliminate(&EliminateSymbolic); };
+ SymbolicBayesNet::shared_ptr eliminate() const
+ { return Base::eliminate(&EliminateSymbolic); };
+
+ /**
+ * Compute the marginal joint over a set of variables, by integrating out
+ * all of the other variables. Returns the result as a Bayes net.
+ */
+ SymbolicBayesNet::shared_ptr jointBayesNet(const std::vector& js) const
+ { return Base::jointBayesNet(js, &EliminateSymbolic); };
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. Returns the result as a factor graph.
*/
- SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const { return Base::jointFactorGraph(js, &EliminateSymbolic); };
+ SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector& js) const
+ { return Base::jointFactorGraph(js, &EliminateSymbolic); };
/**
* Compute the marginal Gaussian density over a variable, by integrating out
diff --git a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp
index 2397180bb..a1c637dcf 100644
--- a/gtsam/inference/tests/testSymbolicSequentialSolver.cpp
+++ b/gtsam/inference/tests/testSymbolicSequentialSolver.cpp
@@ -11,72 +11,105 @@
/**
* @file testSymbolicSequentialSolver.cpp
- * @brief Unit tests for a symbolic IndexFactor Graph
+ * @brief Unit tests for a symbolic sequential solver routines
* @author Frank Dellaert
* @date Sept 16, 2012
*/
-#include // for operator +=
-using namespace boost::assign;
+#include
#include
-#include
-#include
-#include
-#include
-#include
+#include // for operator +=
+using namespace boost::assign;
using namespace std;
using namespace gtsam;
-static const Index vx2 = 0;
-static const Index vx1 = 1;
-static const Index vl1 = 2;
-
/* ************************************************************************* */
-TEST( SymbolicSequentialSolver, SymbolicSequentialSolver )
-{
- // create factor graph
- SymbolicFactorGraph g;
- g.push_factor(vx2, vx1, vl1);
- g.push_factor(vx1, vl1);
- g.push_factor(vx1);
- // test solver is Testable
- SymbolicSequentialSolver solver(g);
+
+TEST( SymbolicSequentialSolver, SymbolicSequentialSolver ) {
+ // create factor graph
+ SymbolicFactorGraph g;
+ g.push_factor(2, 2, 0);
+ g.push_factor(2, 0);
+ g.push_factor(2);
+ // test solver is Testable
+ SymbolicSequentialSolver solver(g);
// GTSAM_PRINT(solver);
- EXPECT(assert_equal(solver,solver));
+ EXPECT(assert_equal(solver,solver));
}
/* ************************************************************************* */
-TEST( SymbolicSequentialSolver, eliminate )
-{
- // create expected Chordal bayes Net
+
+TEST( SymbolicSequentialSolver, inference ) {
+ // Create factor graph
+ SymbolicFactorGraph fg;
+ 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);
+
+ // 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));
+ 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));
- // Create factor graph
- SymbolicFactorGraph fg;
- 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);
+ {
+ // 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));
- // eliminate
- SymbolicSequentialSolver solver(fg);
- SymbolicBayesNet::shared_ptr actual = solver.eliminate();
+ // 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)));
+ }
- CHECK(assert_equal(expected,*actual));
+ {
+ // jointBayesNet
+ vector js;
+ js.push_back(0);
+ js.push_back(3);
+ js.push_back(4);
+ 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)));
+ }
}
/* ************************************************************************* */
int main() {
- TestResult tr;
- return TestRegistry::runAllTests(tr);
+ TestResult tr;
+ return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */