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

Bug fix in BayesTree marginal, re-enabled joint and unit tests

release/4.3a0
Richard Roberts 2010-10-22 22:11:23 +00:00
parent c47893f105
commit 8ff5bf5c7c
7 changed files with 147 additions and 163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
base/FastSet.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* percent.
*/
template<typename VALUE>
class FastSet: public std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > {
class FastSet : public std::set<VALUE, std::less<VALUE>, boost::fast_pool_allocator<VALUE> > {
public:

157
inference/BayesTree-inl.h
View File

@ -108,14 +108,7 @@ namespace gtsam {
cout << "\n";
BOOST_FOREACH(const sharedConditional& conditional, this->conditionals_) {
conditional->print(" " + s + "conditional");
// cout << " " << conditional->key();
}
// if (!separator_.empty()) {
// cout << " :";
// BOOST_FOREACH(Index key, separator_)
// cout << " " << key;
// }
// cout << endl;
}
/* ************************************************************************* */
@ -345,11 +338,11 @@ namespace gtsam {
// for(Index j=0; j<integrands.size(); ++j)
// p_S_R.pop_front();
// Undo the permutation on the shortcut
// Undo the permutation
p_S_R.permuteWithInverse(toBack);
// return the parent shortcut P(Sp|R)
return p_S_R;
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_S_R).eliminate();
}
/* ************************************************************************* */
@ -374,31 +367,33 @@ namespace gtsam {
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_FSR).joint(keys());
}
// /* ************************************************************************* */
// // P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R)
// /* ************************************************************************* */
// template<class CONDITIONAL>
// template<class Factor>
// pair<FactorGraph<Factor>, Ordering>
// BayesTree<CONDITIONAL>::Clique::joint(shared_ptr C2, shared_ptr R) {
// // For now, assume neither is the root
//
// // Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
// sharedBayesNet bn(new BayesNet<CONDITIONAL>);
// if (!isRoot()) bn->push_back(*this); // P(F1|S1)
// if (!isRoot()) bn->push_back(shortcut<Factor>(R)); // P(S1|R)
// if (!C2->isRoot()) bn->push_back(*C2); // P(F2|S2)
// if (!C2->isRoot()) bn->push_back(C2->shortcut<Factor>(R)); // P(S2|R)
// bn->push_back(*R); // P(R)
//
// // Find the keys of both C1 and C2
// Ordering keys12 = keys();
// BOOST_FOREACH(Index key,C2->keys()) keys12.push_back(key);
// keys12.unique();
//
// // Calculate the marginal
// return make_pair(marginalize<Factor,CONDITIONAL>(*bn,keys12), keys12);
// }
/* ************************************************************************* */
// P(C1,C2) = \int_R P(F1|S1) P(S1|R) P(F2|S1) P(S2|R) P(R)
/* ************************************************************************* */
template<class CONDITIONAL>
FactorGraph<typename CONDITIONAL::Factor> BayesTree<CONDITIONAL>::Clique::joint(shared_ptr C2, shared_ptr R) {
// For now, assume neither is the root
// Combine P(F1|S1), P(S1|R), P(F2|S2), P(S2|R), and P(R)
FactorGraph<typename CONDITIONAL::Factor> joint;
if (!isRoot()) joint.push_back(*this); // P(F1|S1)
if (!isRoot()) joint.push_back(shortcut(R)); // P(S1|R)
if (!C2->isRoot()) joint.push_back(*C2); // P(F2|S2)
if (!C2->isRoot()) joint.push_back(C2->shortcut(R)); // P(S2|R)
joint.push_back(*R); // P(R)
// Find the keys of both C1 and C2
vector<Index> keys1(keys());
vector<Index> keys2(C2->keys());
FastSet<Index> keys12;
keys12.insert(keys1.begin(), keys1.end());
keys12.insert(keys2.begin(), keys2.end());
// Calculate the marginal
vector<Index> keys12vector; keys12vector.reserve(keys12.size());
keys12vector.insert(keys12vector.begin(), keys12.begin(), keys12.end());
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(joint).joint(keys12vector);
}
/* ************************************************************************* */
template<class CONDITIONAL>
@ -693,7 +688,7 @@ namespace gtsam {
}
// Now fill in the nodes index
if(subtree->back()->key() > (nodes_.size() - 1))
if(nodes_.size() == 0 || subtree->back()->key() > (nodes_.size() - 1))
nodes_.resize(subtree->back()->key() + 1);
fillNodesIndex(subtree);
}
@ -712,79 +707,45 @@ namespace gtsam {
FactorGraph<typename CONDITIONAL::Factor> cliqueMarginal = clique->marginal(root_);
return GenericSequentialSolver<typename CONDITIONAL::Factor>(cliqueMarginal).marginal(key);
// // Reorder so that only the requested key is not eliminated
// typename FACTORGRAPH::variableindex_type varIndex(cliqueMarginal);
// vector<Index> keyAsVector(1); keyAsVector[0] = key;
// Permutation toBack(Permutation::PushToBack(keyAsVector, varIndex.size()));
// Permutation::shared_ptr toBackInverse(toBack.inverse());
// varIndex.permute(toBack);
// BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) {
// factor->permuteWithInverse(*toBackInverse);
// }
//
// // partially eliminate, remaining factor graph is requested marginal
// SymbolicSequentialSolver::EliminateUntil(cliqueMarginal, varIndex.size()-1, varIndex);
// BOOST_FOREACH(const typename FACTORGRAPH::sharedFactor& factor, cliqueMarginal) {
// if(factor)
// factor->permuteWithInverse(toBack);
// }
// return cliqueMarginal;
}
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::marginalBayesNet(Index key) const {
// calculate marginal as a factor graph
typename FactorGraph<typename CONDITIONAL::Factor>::shared_ptr fg(
new FactorGraph<typename CONDITIONAL::Factor>());
fg->push_back(this->marginal(key));
// calculate marginal as a factor graph
FactorGraph<typename CONDITIONAL::Factor> fg;
fg.push_back(this->marginal(key));
// eliminate further to Bayes net
return GenericSequentialSolver<typename CONDITIONAL::Factor>(*fg).eliminate();
// eliminate factor graph marginal to a Bayes net
return GenericSequentialSolver<typename CONDITIONAL::Factor>(fg).eliminate();
}
// /* ************************************************************************* */
// // Find two cliques, their joint, then marginalizes
// /* ************************************************************************* */
// template<class CONDITIONAL>
// template<class Factor>
// FactorGraph<Factor>
// BayesTree<CONDITIONAL>::joint(Index key1, Index key2) const {
//
// // get clique C1 and C2
// sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
//
// // calculate joint
// Ordering ord;
// FactorGraph<Factor> p_C1C2;
// boost::tie(p_C1C2,ord) = C1->joint<Factor>(C2,root_);
//
// // create an ordering where both requested keys are not eliminated
// ord.remove(key1);
// ord.remove(key2);
//
// // partially eliminate, remaining factor graph is requested joint
// // TODO, make eliminate functional
// eliminate<Factor,CONDITIONAL>(p_C1C2,ord);
// return p_C1C2;
// }
/* ************************************************************************* */
// Find two cliques, their joint, then marginalizes
/* ************************************************************************* */
template<class CONDITIONAL>
typename FactorGraph<typename CONDITIONAL::Factor>::shared_ptr
BayesTree<CONDITIONAL>::joint(Index key1, Index key2) const {
// /* ************************************************************************* */
// template<class CONDITIONAL>
// template<class Factor>
// BayesNet<CONDITIONAL>
// BayesTree<CONDITIONAL>::jointBayesNet(Index key1, Index key2) const {
//
// // calculate marginal as a factor graph
// FactorGraph<Factor> fg = this->joint<Factor>(key1,key2);
//
// // eliminate further to Bayes net
// Ordering ordering;
// ordering += key1, key2;
// return eliminate<Factor,CONDITIONAL>(fg,ordering);
// }
// get clique C1 and C2
sharedClique C1 = (*this)[key1], C2 = (*this)[key2];
// calculate joint
FactorGraph<typename CONDITIONAL::Factor> p_C1C2(C1->joint(C2, root_));
// eliminate remaining factor graph to get requested joint
vector<Index> key12(2); key12[0] = key1; key12[1] = key2;
return GenericSequentialSolver<typename CONDITIONAL::Factor>(p_C1C2).joint(key12);
}
/* ************************************************************************* */
template<class CONDITIONAL>
typename BayesNet<CONDITIONAL>::shared_ptr BayesTree<CONDITIONAL>::jointBayesNet(Index key1, Index key2) const {
// eliminate factor graph marginal to a Bayes net
return GenericSequentialSolver<typename CONDITIONAL::Factor>(*this->joint(key1, key2)).eliminate();
}
/* ************************************************************************* */
template<class CONDITIONAL>

17
inference/BayesTree.h
View File

@ -113,9 +113,8 @@ namespace gtsam {
/** return the marginal P(C) of the clique */
FactorGraph<typename CONDITIONAL::Factor> marginal(shared_ptr root);
// /** return the joint P(C1,C2), where C1==this. TODO: not a method? */
// template<class Factor>
// std::pair<FactorGraph<Factor>,Ordering> joint(shared_ptr C2, shared_ptr root);
/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
FactorGraph<typename CONDITIONAL::Factor> joint(shared_ptr C2, shared_ptr root);
};
// typedef for shared pointers to cliques
@ -261,13 +260,11 @@ namespace gtsam {
/** return marginal on any variable, as a Bayes Net */
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index key) const;
// /** return joint on two variables */
// template<class Factor>
// FactorGraph<Factor> joint(Index key1, Index key2) const;
//
// /** return joint on two variables as a BayesNet */
// template<class Factor>
// BayesNet<CONDITIONAL> jointBayesNet(Index key1, Index key2) const;
/** return joint on two variables */
typename FactorGraph<typename CONDITIONAL::Factor>::shared_ptr joint(Index key1, Index key2) const;
/** return joint on two variables as a BayesNet */
typename BayesNet<CONDITIONAL>::shared_ptr jointBayesNet(Index key1, Index key2) const;
/**
* Read only with side effects

19
inference/GenericMultifrontalSolver-inl.h
View File

@ -26,6 +26,8 @@
#include <boost/foreach.hpp>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
@ -42,6 +44,23 @@ GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::eliminate() const {
return bayesTree;
}
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename FactorGraph<FACTOR>::shared_ptr
GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::joint(const std::vector<Index>& js) const {
// We currently have code written only for computing the
if(js.size() != 2)
throw domain_error(
"*MultifrontalSolver::joint(js) currently can only compute joint marginals\n"
"for exactly two variables. You can call marginal to compute the\n"
"marginal for one variable. *SequentialSolver::joint(js) can compute the\n"
"joint marginal over any number of variables, so use that if necessary.\n");
return eliminate()->joint(js[0], js[1]);
}
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::marginal(Index j) const {

7
inference/GenericMultifrontalSolver.h
View File

@ -48,6 +48,13 @@ public:
*/
typename JUNCTIONTREE::BayesTree::shared_ptr eliminate() const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. This function returns the result as a factor
* graph.
*/
typename FactorGraph<FACTOR>::shared_ptr joint(const std::vector<Index>& js) 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.

12
inference/GenericSequentialSolver.h
View File

@ -54,12 +54,6 @@ public:
*/
typename BayesNet<typename FACTOR::Conditional>::shared_ptr eliminate() 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 FACTOR::shared_ptr marginal(Index j) const;
/**
* Compute the marginal joint over a set of variables, by integrating out
* all of the other variables. This function returns the result as a factor
@ -67,6 +61,12 @@ public:
*/
typename FactorGraph<FACTOR>::shared_ptr joint(const std::vector<Index>& js) 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 FACTOR::shared_ptr marginal(Index j) const;
};
}

96
tests/testGaussianISAM.cpp
View File

@ -278,66 +278,66 @@ TEST( BayesTree, balanced_smoother_shortcuts )
//}
/* ************************************************************************* */
// SL-FIX TEST( BayesTree, balanced_smoother_joint )
//{
// // Create smoother with 7 nodes
// GaussianFactorGraph smoother = createSmoother(7);
// Ordering ordering;
// ordering += "x1","x3","x5","x7","x2","x6","x4";
//
// // Create the Bayes tree, expected to look like:
// // x5 x6 x4
// // x3 x2 : x4
// // x1 : x2
// // x7 : x6
// GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
// GaussianISAM bayesTree(chordalBayesNet);
//
// // Conditional density elements reused by both tests
// const Vector sigma = ones(2);
// const Matrix I = eye(2), A = -0.00429185*I;
//
// // Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
// GaussianBayesNet expected1;
// // Why does the sign get flipped on the prior?
// GaussianConditional::shared_ptr
// parent1(new GaussianConditional("x7", zero(2), -1*I/sigmax7, ones(2)));
// expected1.push_front(parent1);
// push_front(expected1,"x1", zero(2), I/sigmax7, "x7", A/sigmax7, sigma);
// GaussianBayesNet actual1 = bayesTree.jointBayesNet<GaussianFactor>("x1","x7");
// CHECK(assert_equal(expected1,actual1,tol));
//
TEST( BayesTree, balanced_smoother_joint )
{
// Create smoother with 7 nodes
Ordering ordering;
ordering += "x1","x3","x5","x7","x2","x6","x4";
GaussianFactorGraph smoother = createSmoother(7, ordering).first;
// Create the Bayes tree, expected to look like:
// x5 x6 x4
// x3 x2 : x4
// x1 : x2
// x7 : x6
GaussianBayesNet chordalBayesNet = *GaussianSequentialSolver(smoother).eliminate();
GaussianISAM bayesTree(chordalBayesNet);
// Conditional density elements reused by both tests
const Vector sigma = ones(2);
const Matrix I = eye(2), A = -0.00429185*I;
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNet expected1;
// Why does the sign get flipped on the prior?
GaussianConditional::shared_ptr
parent1(new GaussianConditional(ordering["x7"], zero(2), -1*I/sigmax7, ones(2)));
expected1.push_front(parent1);
push_front(expected1,ordering["x1"], zero(2), I/sigmax7, ordering["x7"], A/sigmax7, sigma);
GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering["x1"],ordering["x7"]);
CHECK(assert_equal(expected1,actual1,tol));
// // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
// GaussianBayesNet expected2;
// GaussianConditional::shared_ptr
// parent2(new GaussianConditional("x1", zero(2), -1*I/sigmax1, ones(2)));
// parent2(new GaussianConditional(ordering["x1"], zero(2), -1*I/sigmax1, ones(2)));
// expected2.push_front(parent2);
// push_front(expected2,"x7", zero(2), I/sigmax1, "x1", A/sigmax1, sigma);
// GaussianBayesNet actual2 = bayesTree.jointBayesNet<GaussianFactor>("x7","x1");
// push_front(expected2,ordering["x7"], zero(2), I/sigmax1, ordering["x1"], A/sigmax1, sigma);
// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering["x7"],ordering["x1"]);
// CHECK(assert_equal(expected2,actual2,tol));
//
// // Check the joint density P(x1,x4), i.e. with a root variable
// GaussianBayesNet expected3;
// GaussianConditional::shared_ptr
// parent3(new GaussianConditional("x4", zero(2), I/sigmax4, ones(2)));
// expected3.push_front(parent3);
// double sig14 = 0.784465;
// Matrix A14 = -0.0769231*I;
// push_front(expected3,"x1", zero(2), I/sig14, "x4", A14/sig14, sigma);
// GaussianBayesNet actual3 = bayesTree.jointBayesNet<GaussianFactor>("x1","x4");
// CHECK(assert_equal(expected3,actual3,tol));
//
// Check the joint density P(x1,x4), i.e. with a root variable
GaussianBayesNet expected3;
GaussianConditional::shared_ptr
parent3(new GaussianConditional(ordering["x4"], zero(2), I/sigmax4, ones(2)));
expected3.push_front(parent3);
double sig14 = 0.784465;
Matrix A14 = -0.0769231*I;
push_front(expected3,ordering["x1"], zero(2), I/sig14, ordering["x4"], A14/sig14, sigma);
GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering["x1"],ordering["x4"]);
CHECK(assert_equal(expected3,actual3,tol));
// // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
// GaussianBayesNet expected4;
// GaussianConditional::shared_ptr
// parent4(new GaussianConditional("x1", zero(2), -1.0*I/sigmax1, ones(2)));
// parent4(new GaussianConditional(ordering["x1"], zero(2), -1.0*I/sigmax1, ones(2)));
// expected4.push_front(parent4);
// double sig41 = 0.668096;
// Matrix A41 = -0.055794*I;
// push_front(expected4,"x4", zero(2), I/sig41, "x1", A41/sig41, sigma);
// GaussianBayesNet actual4 = bayesTree.jointBayesNet<GaussianFactor>("x4","x1");
// push_front(expected4,ordering["x4"], zero(2), I/sig41, ordering["x1"], A41/sig41, sigma);
// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering["x4"],ordering["x1"]);
// CHECK(assert_equal(expected4,actual4,tol));
//}
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}