26 #include <ceres/ceres.h>
35 const auto a1 = ceres::cos(
a);
36 const auto a2 = ceres::sin(
a);
39 const auto b1 = ceres::cos(b);
40 const auto b2 = ceres::sin(b);
43 const auto cosSim = a1 * b1 + a2 * b2;
45 const auto angleDiff = ceres::acos(cosSim);
52 const auto derivative = [&
a, &b]() ->
double
54 if (
a == 0. and b == 0.)
60 const double denomSquared =
61 1 - ceres::pow(ceres::sin(
a) * ceres::sin(b) + ceres::cos(
a) * ceres::cos(b), 2);
64 if (denomSquared < 0.001)
69 return (ceres::sin(
a) * ceres::cos(b) - ceres::cos(
a) * ceres::sin(b)) /
71 1 - ceres::pow(ceres::sin(
a) * ceres::sin(b) + ceres::cos(
a) * ceres::cos(b),
76 std::array<double, 2> J;
80 J.at(1) = -derivative;