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

Fix test that used FromMeandAndStddev

release/4.3a0
Frank Dellaert 2022-02-06 20:47:56 -05:00
parent 13d370f61b
commit 325613fc8e
1 changed files with 18 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
tests/testGaussianBayesTreeB.cpp
View File

@ -112,54 +112,50 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
C4 x7 : x6
************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_marginals )
{
TEST(GaussianBayesTree, balanced_smoother_marginals) {
// Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7);
// Create the Bayes tree
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
ordering += X(1), X(3), X(5), X(7), X(2), X(6), X(4);
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
VectorValues actualSolution = bayesTree.optimize();
VectorValues expectedSolution = VectorValues::Zero(actualSolution);
EXPECT(assert_equal(expectedSolution,actualSolution,tol));
EXPECT(assert_equal(expectedSolution, actualSolution, tol));
LONGS_EQUAL(4, (long)bayesTree.size());
LONGS_EQUAL(4, bayesTree.size());
double tol=1e-5;
double tol = 1e-5;
// Check marginal on x1
JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1);
Matrix actualCovarianceX1;
GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky);
actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
EXPECT(assert_equal(expected1,actual1,tol));
Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1);
auto m = bayesTree.marginalFactor(X(1), EliminateCholesky);
Matrix actualCovarianceX1 = m->information().inverse();
EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol));
// Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
double sigmax2 = 0.68712938; // FIXME: this should be corrected analytically
JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
EXPECT(assert_equal(expected2,actual2,tol));
Matrix expectedCovX2 = I_2x2 * (sigmax2 * sigmax2);
EXPECT(assert_equal(expectedCovX2, actual2.information().inverse(), tol));
// Check marginal on x3
JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
EXPECT(assert_equal(expected3,actual3,tol));
Matrix expectedCovX3 = I_2x2 * (sigmax3 * sigmax3);
EXPECT(assert_equal(expectedCovX3, actual3.information().inverse(), tol));
// Check marginal on x4
JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
EXPECT(assert_equal(expected4,actual4,tol));
Matrix expectedCovX4 = I_2x2 * (sigmax4 * sigmax4);
EXPECT(assert_equal(expectedCovX4, actual4.information().inverse(), tol));
// Check marginal on x7 (should be equal to x1)
JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
EXPECT(assert_equal(expected7,actual7,tol));
Matrix expectedCovX7 = I_2x2 * (sigmax7 * sigmax7);
EXPECT(assert_equal(expectedCovX7, actual7.information().inverse(), tol));
}
/* ************************************************************************* */