/* ************************************************************************* */ TEST(GaussianFactorGraph, multiplyHessianAdd2) { GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor(); // brute force Matrix AtA; Vector eta; boost::tie(AtA, eta) = gfg.hessian(); Vector X(6); X << 1, 2, 3, 4, 5, 6; Vector Y(6); Y << -450, -450, 300, 400, 2950, 3450; EXPECT(assert_equal(Y, AtA * X)); VectorValues x = map_list_of<Key, Vector>(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6)); VectorValues expected; expected.insert(0, Vector2(-450, -450)); expected.insert(1, Vector2(300, 400)); expected.insert(2, Vector2(2950, 3450)); VectorValues actual; gfg.multiplyHessianAdd(1.0, x, actual); EXPECT(assert_equal(expected, actual)); // now, do it with non-zero y gfg.multiplyHessianAdd(1.0, x, actual); EXPECT(assert_equal(2 * expected, actual)); }
/* ************************************************************************* */ TEST(GaussianFactorGraph, multiplyHessianAdd) { GaussianFactorGraph gfg = createSimpleGaussianFactorGraph(); VectorValues x = map_list_of<Key, Vector>(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6)); VectorValues expected; expected.insert(0, Vector2(-450, -450)); expected.insert(1, Vector2(0, 0)); expected.insert(2, Vector2(950, 1050)); VectorValues actual; gfg.multiplyHessianAdd(1.0, x, actual); EXPECT(assert_equal(expected, actual)); // now, do it with non-zero y gfg.multiplyHessianAdd(1.0, x, actual); EXPECT(assert_equal(2 * expected, actual)); }