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

Fixed unit test with different (non-degenerate) Hessian

release/4.3a0
Frank Dellaert 2013-10-29 04:07:44 +00:00
parent 4591835223
commit 23cd20aa9b
1 changed files with 19 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
View File

@ -261,8 +261,13 @@ TEST( GaussianFactorGraph, multiplyHessianAdd )
/* ************************************************************************* */ /* ************************************************************************* */
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
#ifdef LUCA
gfg += HessianFactor(1, 2, 100*ones(2,2), 200*ones(2,2), (Vec(2) << 0.0, 1.0), gfg += HessianFactor(1, 2, 100*ones(2,2), 200*ones(2,2), (Vec(2) << 0.0, 1.0),
400*ones(2,2), (Vec(2) << 1.0, 1.0), 0.0); 400*ones(2,2), (Vec(2) << 1.0, 1.0), 3.0);
#else
gfg += HessianFactor(1, 2, 100*eye(2,2), zeros(2,2), (Vec(2) << 0.0, 1.0),
400*eye(2,2), (Vec(2) << 1.0, 1.0), 3.0);
#endif
return gfg; return gfg;
} }
@ -276,11 +281,17 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 )
(1, (Vec(2) << 3,4)) (1, (Vec(2) << 3,4))
(2, (Vec(2) << 5,6)); (2, (Vec(2) << 5,6));
// expected from matlab: -450 -450 2900 2900 6750 6850
VectorValues expected; VectorValues expected;
#ifdef LUCA
// expected from matlab: -450 -450 2900 2900 6750 6850
expected.insert(0, (Vec(2) << -450, -450)); expected.insert(0, (Vec(2) << -450, -450));
expected.insert(1, (Vec(2) << 2900, 2900)); expected.insert(1, (Vec(2) << 2900, 2900));
expected.insert(2, (Vec(2) << 6750, 6850)); expected.insert(2, (Vec(2) << 6750, 6850));
#else
expected.insert(0, (Vec(2) << -450, -450));
expected.insert(1, (Vec(2) << 300, 400));
expected.insert(2, (Vec(2) << 2950, 3450));
#endif
VectorValues actual; VectorValues actual;
gfg.multiplyHessianAdd(1.0, x, actual); gfg.multiplyHessianAdd(1.0, x, actual);
@ -296,9 +307,11 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 )
TEST( GaussianFactorGraph, matricesMixed ) TEST( GaussianFactorGraph, matricesMixed )
{ {
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian(); Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian(); // incorrect !
Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian(); // correct
EXPECT(assert_equal(A.transpose()*A, AtA)); EXPECT(assert_equal(A.transpose()*A, AtA));
Vector expected = - (Vec(6) << -25, 17.5, 5, -13.5, 29, 4);
EXPECT(assert_equal(expected, eta));
EXPECT(assert_equal(A.transpose()*b, eta)); EXPECT(assert_equal(A.transpose()*b, eta));
} }
@ -310,8 +323,8 @@ TEST( GaussianFactorGraph, gradientAtZero )
VectorValues expected; VectorValues expected;
VectorValues actual = gfg.gradientAtZero(); VectorValues actual = gfg.gradientAtZero();
expected.insert(0, (Vec(2) << -25, 17.5)); expected.insert(0, (Vec(2) << -25, 17.5));
expected.insert(1, (Vec(2) << 5, -12.5)); expected.insert(1, (Vec(2) << 5, -13.5));
expected.insert(2, (Vec(2) << 30, 5)); expected.insert(2, (Vec(2) << 29, 4));
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }