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

Failed unit test, trace back to updateATA ?

release/4.3a0
Frank Dellaert 2013-10-29 01:10:41 +00:00
parent 788f69c00e
commit 3c65fcfa19
1 changed files with 46 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -173,6 +173,7 @@ TEST( GaussianFactorGraph, gradient )
VectorValues zero = VectorValues::Zero(expected); VectorValues zero = VectorValues::Zero(expected);
VectorValues actual = fg.gradient(zero); VectorValues actual = fg.gradient(zero);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(expected, fg.gradientAtZero()));
// Check the gradient at the solution (should be zero) // Check the gradient at the solution (should be zero)
VectorValues solution = fg.optimize(); VectorValues solution = fg.optimize();
@ -221,10 +222,21 @@ TEST(GaussianFactorGraph, eliminate_empty )
EXPECT(assert_equal(*remainingGFG, expectedLF)); EXPECT(assert_equal(*remainingGFG, expectedLF));
} }
/* ************************************************************************* */
TEST( GaussianFactorGraph, matrices2 )
{
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian();
Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian();
EXPECT(assert_equal(A.transpose()*A, AtA));
EXPECT(assert_equal(A.transpose()*b, eta));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, multiplyHessianAdd ) TEST( GaussianFactorGraph, multiplyHessianAdd )
{ {
GaussianFactorGraph A = createSimpleGaussianFactorGraph(); GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
VectorValues x = map_list_of VectorValues x = map_list_of
(0, (Vec(2) << 1,2)) (0, (Vec(2) << 1,2))
@ -237,26 +249,26 @@ TEST( GaussianFactorGraph, multiplyHessianAdd )
expected.insert(2, (Vec(2) << 950, 1050)); expected.insert(2, (Vec(2) << 950, 1050));
VectorValues actual; VectorValues actual;
A.multiplyHessianAdd(1.0, x, actual); gfg.multiplyHessianAdd(1.0, x, actual);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
// now, do it with non-zero y // now, do it with non-zero y
A.multiplyHessianAdd(1.0, x, actual); gfg.multiplyHessianAdd(1.0, x, actual);
EXPECT(assert_equal(2*expected, actual)); EXPECT(assert_equal(2*expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() {
GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
fg += 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), 0.0);
return fg; return gfg;
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, multiplyHessianAdd2 ) TEST( GaussianFactorGraph, multiplyHessianAdd2 )
{ {
GaussianFactorGraph A = createGaussianFactorGraphWithHessianFactor(); GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
VectorValues x = map_list_of VectorValues x = map_list_of
(0, (Vec(2) << 1,2)) (0, (Vec(2) << 1,2))
@ -270,15 +282,39 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 )
expected.insert(2, (Vec(2) << 6750, 6850)); expected.insert(2, (Vec(2) << 6750, 6850));
VectorValues actual; VectorValues actual;
A.multiplyHessianAdd(1.0, x, actual); gfg.multiplyHessianAdd(1.0, x, actual);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
// now, do it with non-zero y // now, do it with non-zero y
A.multiplyHessianAdd(1.0, x, actual); gfg.multiplyHessianAdd(1.0, x, actual);
EXPECT(assert_equal(2*expected, actual)); EXPECT(assert_equal(2*expected, actual));
} }
/* ************************************************************************* */
TEST( GaussianFactorGraph, matricesMixed )
{
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
Matrix A; Vector b; boost::tie(A,b) = gfg.jacobian();
Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian();
EXPECT(assert_equal(A.transpose()*A, AtA));
EXPECT(assert_equal(A.transpose()*b, eta));
}
/* ************************************************************************* */
TEST( GaussianFactorGraph, gradientAtZero )
{
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
VectorValues expected;
VectorValues actual = gfg.gradientAtZero();
expected.insert(0, (Vec(2) << -25, 17.5));
expected.insert(1, (Vec(2) << 5, -12.5));
expected.insert(2, (Vec(2) << 30, 5));
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */