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

34
tests/testGaussianBayesTreeB.cpp
View File

@ -112,8 +112,7 @@ TEST( GaussianBayesTree, linear_smoother_shortcuts )
C4 x7 : x6 C4 x7 : x6
************************************************************************* */ ************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_marginals ) TEST(GaussianBayesTree, balanced_smoother_marginals) {
{
// Create smoother with 7 nodes // Create smoother with 7 nodes
GaussianFactorGraph smoother = createSmoother(7); GaussianFactorGraph smoother = createSmoother(7);
@ -126,40 +125,37 @@ TEST( GaussianBayesTree, balanced_smoother_marginals )
VectorValues expectedSolution = VectorValues::Zero(actualSolution); 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 // Check marginal on x1
JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), Z_2x1, sigmax1);
JacobianFactor actual1 = *bayesTree.marginalFactor(X(1)); JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
Matrix expectedCovarianceX1 = I_2x2 * (sigmax1 * sigmax1); Matrix expectedCovX1 = I_2x2 * (sigmax1 * sigmax1);
Matrix actualCovarianceX1; auto m = bayesTree.marginalFactor(X(1), EliminateCholesky);
GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky); Matrix actualCovarianceX1 = m->information().inverse();
actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse(); EXPECT(assert_equal(expectedCovX1, actualCovarianceX1, tol));
EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
EXPECT(assert_equal(expected1,actual1,tol));
// Check marginal on x2 // Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically double sigmax2 = 0.68712938; // FIXME: this should be corrected analytically
JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), Z_2x1, sigx2);
JacobianFactor actual2 = *bayesTree.marginalFactor(X(2)); 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 // Check marginal on x3
JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), Z_2x1, sigmax3);
JacobianFactor actual3 = *bayesTree.marginalFactor(X(3)); 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 // Check marginal on x4
JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), Z_2x1, sigmax4);
JacobianFactor actual4 = *bayesTree.marginalFactor(X(4)); 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) // Check marginal on x7 (should be equal to x1)
JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), Z_2x1, sigmax7);
JacobianFactor actual7 = *bayesTree.marginalFactor(X(7)); 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */