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

GaussianBayesNet::matrix now properly divides in sigmas

release/4.3a0
Frank Dellaert 2010-01-05 14:14:49 +00:00
parent 22d66df766
commit cd644e75a5
2 changed files with 36 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
cpp/GaussianBayesNet.cpp
View File

@ -156,17 +156,20 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
// find corresponding conditional
GaussianConditional::shared_ptr cg = bn[key];
// get sigmas
Vector sigmas = cg->get_sigmas();
// get RHS and copy to d
const Vector& d_ = cg->get_d();
const size_t n = d_.size();
for (size_t i=0;i<n;i++)
d(I+i) = d_(i);
d(I+i) = d_(i)/sigmas(i);
// get leading R matrix and copy to R
const Matrix& R_ = cg->get_R();
for (size_t i=0;i<n;i++)
for(size_t j=0;j<n;j++)
R(I+i,I+j) = R_(i,j);
R(I+i,I+j) = R_(i,j)/sigmas(i);
// loop over S matrices and copy them into R
GaussianConditional::const_iterator keyS = cg->parentsBegin();
@ -176,7 +179,7 @@ pair<Matrix,Vector> matrix(const GaussianBayesNet& bn) {
const size_t J = mapping[keyS->first]; // find column index
for (size_t i=0;i<m;i++)
for(size_t j=0;j<n;j++)
R(I+i,J+j) = S(i,j);
R(I+i,J+j) = S(i,j)/sigmas(i);
} // keyS
} // keyI

30
cpp/testGaussianFactorGraph.cpp
View File

@ -619,6 +619,36 @@ TEST( GaussianFactorGraph, rhs )
}
/* ************************************************************************* */
// Extra test on elimination prompted by Michael's email to Frank 1/4/2010
TEST( GaussianFactorGraph, elimination )
{
// Create Gaussian Factor Graph
GaussianFactorGraph fg;
Matrix Ap = eye(1), An = eye(1) * -1;
Vector b = Vector_(1, 0.0);
double sigma = 1.0/0.5;
fg.add("x1", An, "x2", Ap, b, sigma);
fg.add("x1", Ap, b, sigma);
fg.add("x2", Ap, b, sigma);
// Eliminate
Ordering ord;
ord += "x1", "x2";
GaussianBayesNet bayesNet = fg.eliminate(ord);
// Check sigma
DOUBLES_EQUAL(1.0/0.612372,bayesNet["x2"]->get_sigmas()(0),1e-5);
// Check matrix
Matrix R;Vector d;
boost::tie(R,d) = matrix(bayesNet);
Matrix expected = Matrix_(2,2,
0.707107, -0.353553,
0.0, 0.612372);
CHECK(assert_equal(expected,R,1e-6));
}
/* ************************************************************************* */
// Tests ported from ConstrainedGaussianFactorGraph
/* ************************************************************************* */
TEST( GaussianFactorGraph, constrained_simple )