/* ************************************************************************* */ TEST(GaussianFactorGraph, matrices) { // Create factor graph: // x1 x2 x3 x4 x5 b // 1 2 3 0 0 4 // 5 6 7 0 0 8 // 9 10 0 11 12 13 // 0 0 0 14 15 16 Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished(); Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished(); Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15).finished(); GaussianFactorGraph gfg; SharedDiagonal model = noiseModel::Unit::Create(2); gfg.add(0, A00, Vector2(4., 8.), model); gfg.add(0, A10, 1, A11, Vector2(13., 16.), model); Matrix Ab(4, 6); Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16; // augmented versions EXPECT(assert_equal(Ab, gfg.augmentedJacobian())); EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian())); // jacobian Matrix A = Ab.leftCols(Ab.cols() - 1); Vector b = Ab.col(Ab.cols() - 1); Matrix actualA; Vector actualb; boost::tie(actualA, actualb) = gfg.jacobian(); EXPECT(assert_equal(A, actualA)); EXPECT(assert_equal(b, actualb)); // hessian Matrix L = A.transpose() * A; Vector eta = A.transpose() * b; Matrix actualL; Vector actualeta; boost::tie(actualL, actualeta) = gfg.hessian(); EXPECT(assert_equal(L, actualL)); EXPECT(assert_equal(eta, actualeta)); // hessianBlockDiagonal VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns expectLdiagonal.insert(0, Vector3(1 + 25 + 81, 4 + 36 + 100, 9 + 49)); expectLdiagonal.insert(1, Vector2(121 + 196, 144 + 225)); EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal())); // hessianBlockDiagonal map<Key, Matrix> actualBD = gfg.hessianBlockDiagonal(); LONGS_EQUAL(2, actualBD.size()); EXPECT(assert_equal(A00.transpose() * A00 + A10.transpose() * A10, actualBD[0])); EXPECT(assert_equal(A11.transpose() * A11, actualBD[1])); }
/* ************************************************************************* */ TEST(GaussianFactorGraph, matricesMixed) { GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); Matrix A; Vector b; boost::tie(A, b) = gfg.jacobian(); // incorrect ! Matrix AtA; Vector eta; boost::tie(AtA, eta) = gfg.hessian(); // correct EXPECT(assert_equal(A.transpose() * A, AtA)); Vector expected = -(Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished(); EXPECT(assert_equal(expected, eta)); EXPECT(assert_equal(A.transpose() * b, eta)); }
/* ************************************************************************* */ 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)); Matrix expectedAtA(6, 6); expectedAtA << 125, 0, -25, 0, -100, 0, // 0, 125, 0, -25, 0, -100, // -25, 0, 50, 0, -25, 0, // 0, -25, 0, 50, 0, -25, // -100, 0, -25, 0, 225, 0, // 0, -100, 0, -25, 0, 225; EXPECT(assert_equal(expectedAtA, AtA)); }