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

Extra unit test to check that marginals from sequential and multifrontal are the same

release/4.3a0
Richard Roberts 2010-10-22 22:53:33 +00:00
parent 0eaaa12e15
commit fda11d7c15
2 changed files with 20 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
inference/BayesTree-inl.h
View File

@ -342,7 +342,7 @@ namespace gtsam {
p_S_R.permuteWithInverse(toBack);
// return the parent shortcut P(Sp|R)
return *GenericSequentialSolver<typename CONDITIONAL::Factor>(p_S_R).eliminate();
return p_S_R;
}
/* ************************************************************************* */

19
tests/testGaussianISAM.cpp
View File

@ -23,11 +23,13 @@ using namespace boost::assign;
#define GTSAM_MAGIC_KEY
#include <gtsam/geometry/Rot2.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/inference/ISAM-inl.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/slam/smallExample.h>
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);}
/* ************************************************************************* */