Function fd_test

tests/costs.cpp:25–47

using context::VectorSpace;

void fd_test(VectorXs x0, VectorXs u0, MatrixXs weights,
             QuadraticResidualCost qres, shared_ptr<CostData> data) {

  const xyz::polymorphic<StageFunctionTpl<T>> fun = qres.residual_;
  const auto fd = fun->createData();
  const auto ndx = fd->ndx1;
  const auto nu = fd->nu;
  qres.evaluate(x0, u0, *data);
  qres.computeGradients(x0, u0, *data);
  qres.computeHessians(x0, u0, *data);

  // analytical formula
  fun->evaluate(x0, u0, *fd);
  fun->computeJacobians(x0, u0, *fd);

  auto n = (long)(ndx + nu);
  auto J = fd->jac_buffer_.leftCols(n);

  auto grad_ref = J.transpose() * weights * fd->value_;
  auto hess_ref = J.transpose() * weights * J;
  REQUIRE(grad_ref.isApprox(data->grad_));
  REQUIRE(hess_ref.isApprox(data->hess_));
}

#ifdef ALIGATOR_WITH_PINOCCHIO
TEST_CASE("quad_state_se2", "[costs]") {