/* ************************************************************************* */
TEST(GaussianFactorGraph, gradient) {
GaussianFactorGraph fg = createSimpleGaussianFactorGraph();
// Construct expected gradient
// 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 +
// 25*(l1-x2-[-0.2;0.3])^2
// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
VectorValues expected = map_list_of<Key, Vector>(1, Vector2(5.0, -12.5))(2, Vector2(30.0, 5.0))(
0, Vector2(-25.0, 17.5));
// Check the gradient at delta=0
VectorValues zero = VectorValues::Zero(expected);
VectorValues actual = fg.gradient(zero);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(expected, fg.gradientAtZero()));
// Check the gradient at the solution (should be zero)
VectorValues solution = fg.optimize();
VectorValues actual2 = fg.gradient(solution);
EXPECT(assert_equal(VectorValues::Zero(solution), actual2));
}
