Function TEST

okvis_ros/okvis/okvis_ceres/test/TestJacobians.cpp:10–156

const double jacobianTolerance = 1.0e-3;

TEST(okvisTestSuite, JacobianTest) {
  // initialize random number generator
  srand((unsigned int)time(0));  // disabled: make unit tests deterministic...

  OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error);

  okvis::kinematics::Transformation T_WS;
  T_WS.setRandom();
  okvis::Time time(0.1);
  okvis::ceres::PoseParameterBlock poseParameterBlock(T_WS, 0, time);  // ground truth

  double* x = poseParameterBlock.parameters();

  Eigen::Matrix<double, 7, 6, Eigen::RowMajor> Jplus;
  Eigen::Matrix<double, 7, 6, Eigen::RowMajor> Jplus_numDiff;

  okvis::ceres::PoseManifold::plusJacobian(x, Jplus.data());

  double dx = 1e-9;

  for (size_t i = 0; i < 6; ++i) {
    Eigen::Matrix<double, 7, 1> xp;
    Eigen::Matrix<double, 7, 1> xm;
    Eigen::Matrix<double, 6, 1> delta;
    delta.setZero();
    delta[i] = dx;
    okvis::ceres::PoseManifold::plus(x, delta.data(), xp.data());
    delta[i] = -dx;
    okvis::ceres::PoseManifold::plus(x, delta.data(), xm.data());
    Jplus_numDiff.col(i) = (xp - xm) / (2 * dx);
  }

  okvis::ceres::PoseManifold pose_manifold;
  OKVIS_ASSERT_TRUE(Exception, pose_manifold.verify(x, 1e-6), "Pose manifold verification failed.");

  // x = poseParameterBlock.parameters();
  Eigen::Matrix<double, 6, 7, Eigen::RowMajor> Jminus;
  Eigen::Matrix<double, 6, 7, Eigen::RowMajor> Jminus_numDiff;

  okvis::ceres::PoseManifold::minusJacobian(x, Jminus.data());

  Eigen::Matrix<double, 7, 1> xp, xm, x_vec(x);

  for (size_t i = 0; i < 7; ++i) {
    Eigen::Matrix<double, 6, 1> delta_p, delta_m;

    xp = x_vec;
    xm = x_vec;
    xp[i] = x[i] + dx;
    xm[i] = x[i] - dx;

    okvis::ceres::PoseManifold::minus(xp.data(), x, delta_p.data());
    okvis::ceres::PoseManifold::minus(xm.data(), x, delta_m.data());
    Jminus_numDiff.col(i) = (delta_p - delta_m) / (2 * dx);
  }

  OKVIS_ASSERT_TRUE(Exception,
                    (Jminus - Jminus_numDiff).norm() < jacobianTolerance,