jointBayesNet function avoids conversion to factorgraph (which was converted back to a BayesNet in shortcut calculation)
Frank Dellaert
parent
3f194bebff
commit
db57f1872a
|
|
@ -849,10 +849,10 @@
|
|||
</target>
|
||||
<target name="testSymbolicSequentialSolver.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildArguments>-j1</buildArguments>
|
||||
<buildTarget>testSymbolicSequentialSolver.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testEliminationTree.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
|
|
|
|||
|
|
@ -82,45 +82,54 @@ namespace gtsam {
|
|||
return eliminationTree_->eliminate(function);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename FactorGraph<FACTOR>::shared_ptr //
|
||||
GenericSequentialSolver<FACTOR>::jointFactorGraph(
|
||||
const std::vector<Index>& js, Eliminate function) const {
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr //
|
||||
GenericSequentialSolver<FACTOR>::jointBayesNet(
|
||||
const std::vector<Index>& 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>& 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>& factor, *factors_)
|
||||
if (factor) factor->permuteWithInverse(*permutationInverse);
|
||||
|
||||
// Eliminate all variables
|
||||
typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr
|
||||
bayesNet(EliminationTree<FACTOR>::Create(*factors_)->eliminate(function));
|
||||
// Eliminate all variables
|
||||
typename BayesNet<Conditional>::shared_ptr
|
||||
bayesNet(EliminationTree<FACTOR>::Create(*factors_)->eliminate(function));
|
||||
|
||||
// Undo the permuation on the original factors and on the structure.
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& 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>& factor, *factors_)
|
||||
if (factor) factor->permuteWithInverse(*permutation);
|
||||
|
||||
// Take the joint marginal from the Bayes net.
|
||||
sharedFactorGraph joint(new FactorGraph<FACTOR> );
|
||||
joint->reserve(js.size());
|
||||
typename BayesNet<typename FACTOR::ConditionalType>::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;i<nrMarginalizedOut;i++)
|
||||
bayesNet->pop_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<Conditional>& c, *bayesNet)
|
||||
c->permuteWithInverse(*permutation);
|
||||
|
||||
// Undo the permutation on the eliminated joint marginal factors
|
||||
BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *joint)
|
||||
factor->permuteWithInverse(*permutation);
|
||||
return bayesNet;
|
||||
}
|
||||
|
||||
return joint;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
typename FactorGraph<FACTOR>::shared_ptr //
|
||||
GenericSequentialSolver<FACTOR>::jointFactorGraph(
|
||||
const std::vector<Index>& js, Eliminate function) const {
|
||||
|
||||
// Eliminate all variables
|
||||
typename BayesNet<Conditional>::shared_ptr
|
||||
bayesNet = jointBayesNet(js,function);
|
||||
|
||||
return boost::make_shared<FactorGraph<FACTOR> >(*bayesNet);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<class FACTOR>
|
||||
|
|
|
|||
|
|
@ -51,10 +51,8 @@ namespace gtsam {
|
|||
protected:
|
||||
|
||||
typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
|
||||
|
||||
typedef std::pair<
|
||||
boost::shared_ptr<typename FACTOR::ConditionalType>,
|
||||
boost::shared_ptr<FACTOR> > EliminationResult;
|
||||
typedef typename FACTOR::ConditionalType Conditional;
|
||||
typedef std::pair<boost::shared_ptr<Conditional>, boost::shared_ptr<FACTOR> > EliminationResult;
|
||||
typedef boost::function<EliminationResult(const FactorGraph<FACTOR>&, 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<BayesNet<typename FACTOR::ConditionalType> > eliminate(Eliminate function) const;
|
||||
typename boost::shared_ptr<BayesNet<Conditional> >
|
||||
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<FACTOR>::shared_ptr jointFactorGraph(
|
||||
const std::vector<Index>& 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<Conditional>::shared_ptr
|
||||
jointBayesNet(const std::vector<Index>& 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<FACTOR>::shared_ptr
|
||||
jointFactorGraph(const std::vector<Index>& 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<FACTOR> marginalFactor(Index j, Eliminate function) const;
|
||||
typename boost::shared_ptr<FACTOR>
|
||||
marginalFactor(Index j, Eliminate function) const;
|
||||
|
||||
/// @}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<Index>& 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<Index>& js) const { return Base::jointFactorGraph(js, &EliminateSymbolic); };
|
||||
SymbolicFactorGraph::shared_ptr jointFactorGraph(const std::vector<Index>& js) const
|
||||
{ return Base::jointFactorGraph(js, &EliminateSymbolic); };
|
||||
|
||||
/**
|
||||
* Compute the marginal Gaussian density over a variable, by integrating out
|
||||
|
|
|
|||
|
|
@ -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 <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
#include <gtsam/inference/SymbolicSequentialSolver.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/inference/SymbolicFactorGraph.h>
|
||||
#include <gtsam/inference/BayesNet-inl.h>
|
||||
#include <gtsam/inference/IndexFactor.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/SymbolicSequentialSolver.h>
|
||||
#include <boost/assign/std/list.hpp> // 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<IndexConditional>(4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(3,4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(2,4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(1,2,4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(0,1,2));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(3, 4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(2, 4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(1, 2, 4));
|
||||
expected.push_front(boost::make_shared<IndexConditional>(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<Index> 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<IndexConditional>(3));
|
||||
expectedBN.push_front(boost::make_shared<IndexConditional>(4, 3));
|
||||
expectedBN.push_front(boost::make_shared<IndexConditional>(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<Index> 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<IndexConditional>(3));
|
||||
expectedBN.push_front(boost::make_shared<IndexConditional>(4, 3));
|
||||
expectedBN.push_front(boost::make_shared<IndexConditional>(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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue