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

refactored matrices test and checked my understanding of diagonal

release/4.3a0
Frank Dellaert 2014-02-13 14:58:29 -05:00
parent e471df6a32
commit b048db4296
1 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -117,31 +117,32 @@ TEST(GaussianFactorGraph, matrices) {
gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model); gfg.add(0, (Matrix(2, 3) << 1., 2., 3., 5., 6., 7.), (Vector(2) << 4., 8.), model);
gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model); gfg.add(0, (Matrix(2, 3) << 9.,10., 0., 0., 0., 0.), 1, (Matrix(2, 2) << 11., 12., 14., 15.), (Vector(2) << 13.,16.), model);
Matrix jacobian(4,6); Matrix Ab(4,6);
jacobian << Ab <<
1, 2, 3, 0, 0, 4, 1, 2, 3, 0, 0, 4,
5, 6, 7, 0, 0, 8, 5, 6, 7, 0, 0, 8,
9,10, 0,11,12,13, 9,10, 0,11,12,13,
0, 0, 0,14,15,16; 0, 0, 0,14,15,16;
Matrix expectedJacobian = jacobian; EXPECT(assert_equal(Ab, gfg.augmentedJacobian()));
Matrix expectedHessian = jacobian.transpose() * jacobian; EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian()));
Matrix expectedA = jacobian.leftCols(jacobian.cols()-1);
Vector expectedb = jacobian.col(jacobian.cols()-1);
Matrix expectedL = expectedA.transpose() * expectedA;
Vector expectedeta = expectedA.transpose() * expectedb;
Matrix actualJacobian = gfg.augmentedJacobian(); Matrix A = Ab.leftCols(Ab.cols()-1);
Matrix actualHessian = gfg.augmentedHessian(); Vector b = Ab.col(Ab.cols()-1);
Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian(); Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian();
Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian(); EXPECT(assert_equal(A, actualA));
EXPECT(assert_equal(b, actualb));
EXPECT(assert_equal(expectedJacobian, actualJacobian)); Matrix L = A.transpose() * A;
EXPECT(assert_equal(expectedHessian, actualHessian)); Vector eta = A.transpose() * b;
EXPECT(assert_equal(expectedA, actualA)); Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian();
EXPECT(assert_equal(expectedb, actualb)); EXPECT(assert_equal(L, actualL));
EXPECT(assert_equal(expectedL, actualL)); EXPECT(assert_equal(eta, actualeta));
EXPECT(assert_equal(expectedeta, actualeta));
Vector expectLdiagonal(5); // Make explicit that diagonal is sum-squares of columns
expectLdiagonal << 1+25+81, 4+36+100, 9+49, 121+196, 144+225;
EXPECT(assert_equal(L.diagonal(), expectLdiagonal));
// EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));
} }
/* ************************************************************************* */ /* ************************************************************************* */