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

Added marginal function to GaussianISAM, renamed and added comments to bayes tree

release/4.3a0
Alex Cunningham 2011-02-11 22:08:07 +00:00
parent 9f4661544f
commit d8f05f78ff
6 changed files with 60 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/inference/BayesTree-inl.h
View File

@ -382,6 +382,7 @@ namespace gtsam {
template<class CONDITIONAL>
FactorGraph<typename CONDITIONAL::Factor> BayesTree<CONDITIONAL>::Clique::marginal(shared_ptr R) {
// If we are the root, just return this root
// NOTE: immediately cast to a factor graph
if (R.get()==this) return *R;
// Combine P(F|S), P(S|R), and P(R)
@ -389,9 +390,6 @@ namespace gtsam {
p_FSR.push_front(*this);
p_FSR.push_back(*R);
// Find marginal on the keys we are interested in
FactorGraph<typename CONDITIONAL::Factor> p_FSRfg(p_FSR);
assertInvariants();
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_FSR).jointFactorGraph(keys());
}
@ -731,7 +729,7 @@ namespace gtsam {
// First finds clique marginal then marginalizes that
/* ************************************************************************* */
template<class CONDITIONAL>
typename CONDITIONAL::Factor::shared_ptr BayesTree<CONDITIONAL>::marginal(Index key) const {
typename CONDITIONAL::Factor::shared_ptr BayesTree<CONDITIONAL>::marginalFactor(Index key) const {
// get clique containing key
sharedClique clique = (*this)[key];
@ -748,7 +746,7 @@ namespace gtsam {
// calculate marginal as a factor graph
FactorGraph<typename CONDITIONAL::Factor> fg;
fg.push_back(this->marginal(key));
fg.push_back(this->marginalFactor(key));
// eliminate factor graph marginal to a Bayes net
return GenericSequentialSolver<typename CONDITIONAL::Factor>(fg).eliminate();

8
gtsam/inference/BayesTree.h
View File

@ -262,9 +262,13 @@ namespace gtsam {
CliqueData getCliqueData() const;
/** return marginal on any variable */
typename CONDITIONAL::Factor::shared_ptr marginal(Index key) const;
typename CONDITIONAL::Factor::shared_ptr marginalFactor(Index key) const;
/** return marginal on any variable, as a Bayes Net */
/**
* return marginal on any variable, as a Bayes Net
* NOTE: this function calls marginal, and then eliminates it into a Bayes Net
* This is more expensive than the above function
*/
typename BayesNet<CONDITIONAL>::shared_ptr marginalBayesNet(Index key) const;
/** return joint on two variables */

2
gtsam/inference/GenericMultifrontalSolver-inl.h
View File

@ -75,7 +75,7 @@ GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::jointFactorGraph(const std::vec
/* ************************************************************************* */
template<class FACTOR, class JUNCTIONTREE>
typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR, JUNCTIONTREE>::marginalFactor(Index j) const {
return eliminate()->marginal(j);
return eliminate()->marginalFactor(j);
}
}

12
gtsam/linear/GaussianISAM.cpp
View File

@ -24,8 +24,20 @@ using namespace gtsam;
#include <gtsam/inference/ISAM-inl.h>
template class ISAM<GaussianConditional>;
namespace ublas = boost::numeric::ublas;
namespace gtsam {
/* ************************************************************************* */
std::pair<Vector, Matrix> GaussianISAM::marginal(Index j) const {
GaussianFactor::shared_ptr factor = this->marginalFactor(j);
FactorGraph<GaussianFactor> graph;
graph.push_back(factor);
GaussianConditional::shared_ptr conditional = GaussianFactor::CombineAndEliminate(graph,1).first->front();
Matrix R = conditional->get_R();
return make_pair(conditional->get_d(), inverse(ublas::prod(ublas::trans(R), R)));
}
/* ************************************************************************* */
void optimize(const GaussianISAM::sharedClique& clique, VectorValues& result) {
// parents are assumed to already be solved and available in result

3
gtsam/linear/GaussianISAM.h
View File

@ -69,6 +69,9 @@ public:
friend VectorValues optimize(const GaussianISAM&);
/** return marginal on any variable */
std::pair<Vector,Matrix> marginal(Index key) const;
};
// recursively optimize this conditional and all subtrees

40
tests/testGaussianISAM.cpp
View File

@ -195,28 +195,58 @@ TEST( BayesTree, balanced_smoother_marginals )
// Check marginal on x1
GaussianBayesNet expected1 = simpleGaussian(ordering["x1"], zero(2), sigmax1);
GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering["x1"]);
CHECK(assert_equal(expected1,actual1,tol));
Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1);
Vector expectedMeanX1 = zero(2);
Matrix actualCovarianceX1; Vector actualMeanX1;
boost::tie(actualMeanX1, actualCovarianceX1) = bayesTree.marginal(ordering["x1"]);
EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
EXPECT(assert_equal(expectedMeanX1, actualMeanX1, tol));
EXPECT(assert_equal(expected1,actual1,tol));
// Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
GaussianBayesNet expected2 = simpleGaussian(ordering["x2"], zero(2), sigx2);
GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering["x2"]);
CHECK(assert_equal(expected2,actual2,tol)); // FAILS
Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2);
Vector expectedMeanX2 = zero(2);
Matrix actualCovarianceX2; Vector actualMeanX2;
boost::tie(actualMeanX2, actualCovarianceX2) = bayesTree.marginal(ordering["x2"]);
EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol));
EXPECT(assert_equal(expectedMeanX2, actualMeanX2, tol));
EXPECT(assert_equal(expected2,actual2,tol));
// Check marginal on x3
GaussianBayesNet expected3 = simpleGaussian(ordering["x3"], zero(2), sigmax3);
GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering["x3"]);
CHECK(assert_equal(expected3,actual3,tol));
Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3);
Vector expectedMeanX3 = zero(2);
Matrix actualCovarianceX3; Vector actualMeanX3;
boost::tie(actualMeanX3, actualCovarianceX3) = bayesTree.marginal(ordering["x3"]);
EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol));
EXPECT(assert_equal(expectedMeanX3, actualMeanX3, tol));
EXPECT(assert_equal(expected3,actual3,tol));
// Check marginal on x4
GaussianBayesNet expected4 = simpleGaussian(ordering["x4"], zero(2), sigmax4);
GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering["x4"]);
CHECK(assert_equal(expected4,actual4,tol));
Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4);
Vector expectedMeanX4 = zero(2);
Matrix actualCovarianceX4; Vector actualMeanX4;
boost::tie(actualMeanX4, actualCovarianceX4) = bayesTree.marginal(ordering["x4"]);
EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol));
EXPECT(assert_equal(expectedMeanX4, actualMeanX4, tol));
EXPECT(assert_equal(expected4,actual4,tol));
// Check marginal on x7 (should be equal to x1)
GaussianBayesNet expected7 = simpleGaussian(ordering["x7"], zero(2), sigmax7);
GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering["x7"]);
CHECK(assert_equal(expected7,actual7,tol));
Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7);
Vector expectedMeanX7 = zero(2);
Matrix actualCovarianceX7; Vector actualMeanX7;
boost::tie(actualMeanX7, actualCovarianceX7) = bayesTree.marginal(ordering["x7"]);
EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol));
EXPECT(assert_equal(expectedMeanX7, actualMeanX7, tol));
EXPECT(assert_equal(expected7,actual7,tol));
}
/* ************************************************************************* */