diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index 1ad2ae97f..f0156e0ff 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -342,7 +342,7 @@ namespace gtsam { p_S_R.permuteWithInverse(toBack); // return the parent shortcut P(Sp|R) - return *GenericSequentialSolver(p_S_R).eliminate(); + return p_S_R; } /* ************************************************************************* */ diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index c676f1695..fb6f0c568 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -23,11 +23,13 @@ using namespace boost::assign; #define GTSAM_MAGIC_KEY +#include #include #include #include #include #include +#include #include using namespace std; @@ -339,6 +341,23 @@ TEST( BayesTree, balanced_smoother_joint ) // CHECK(assert_equal(expected4,actual4,tol)); } +/* ************************************************************************* */ +TEST(BayesTree, simpleMarginal) +{ + GaussianFactorGraph gfg; + + Matrix A12 = Rot2::fromDegrees(45.0).matrix(); + + gfg.add(0, eye(2), zero(2), sharedSigma(2, 1.0)); + gfg.add(0, -eye(2), 1, eye(2), ones(2), sharedSigma(2, 1.0)); + gfg.add(1, -eye(2), 2, A12, ones(2), sharedSigma(2, 1.0)); + + Matrix expected(GaussianSequentialSolver(gfg).marginalStandard(2).second); + Matrix actual(GaussianMultifrontalSolver(gfg).marginalStandard(2).second); + + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */