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

Checked scatter

release/4.3a0
dellaert 2015-06-21 22:48:36 -07:00
parent a3ed636fce
commit 41d4c48938
1 changed files with 22 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -83,13 +83,33 @@ TEST( GaussianBayesTree, eliminate )
{
GaussianBayesTree bt = *chain.eliminateMultifrontal(chainOrdering);
Scatter scatter(chain);
BOOST_FOREACH(const Scatter::value_type& entry, scatter)
cout << entry.first << " " << entry.second.toString() << endl;
EXPECT_LONGS_EQUAL(4, scatter.size());
EXPECT_LONGS_EQUAL(0, scatter.at(x1).slot);
EXPECT_LONGS_EQUAL(1, scatter.at(x2).slot);
EXPECT_LONGS_EQUAL(2, scatter.at(x3).slot);
EXPECT_LONGS_EQUAL(3, scatter.at(x4).slot);
Matrix two = (Matrix(1, 1) << 2.).finished();
Matrix one = (Matrix(1, 1) << 1.).finished();
GaussianBayesTree bayesTree_expected;
bayesTree_expected.insertRoot(
MakeClique(GaussianConditional(pair_list_of<Key, Matrix>(x3, (Matrix(2, 1) << 2., 0.).finished()) (x4, (Matrix(2, 1) << 2., 2.).finished()), 2, Vector2(2., 2.)), list_of
(MakeClique(GaussianConditional(pair_list_of<Key, Matrix>(x2, (Matrix(2, 1) << -2.*sqrt(2.), 0.).finished()) (x1, (Matrix(2, 1) << -sqrt(2.), -sqrt(2.)).finished()) (x3, (Matrix(2, 1) << -sqrt(2.), sqrt(2.)).finished()), 2, (Vector(2) << -2.*sqrt(2.), 0.).finished())))));
MakeClique(
GaussianConditional(
pair_list_of<Key, Matrix>(x3, (Matrix21() << 2., 0.).finished())(
x4, (Matrix21() << 2., 2.).finished()), 2, Vector2(2., 2.)),
list_of(
MakeClique(
GaussianConditional(
pair_list_of<Key, Matrix>(x2,
(Matrix21() << -2. * sqrt(2.), 0.).finished())(x1,
(Matrix21() << -sqrt(2.), -sqrt(2.)).finished())(x3,
(Matrix21() << -sqrt(2.), sqrt(2.)).finished()), 2,
(Vector(2) << -2. * sqrt(2.), 0.).finished())))));
EXPECT(assert_equal(bayesTree_expected, bt));
}