6 #include <ceres/ceres.h>
7 #include <ceres/local_parameterization.h>
14 inline std::size_t
GetHash(
const int& x,
const int& y)
16 return ((std::hash<double>()(x) ^ (std::hash<double>()(y) << 1)) >> 1);
27 return angle_radians - two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
38 bool operator()(
const T* theta_radians,
const T* delta_theta_radians, T* theta_radians_plus_delta)
const
40 *theta_radians_plus_delta =
NormalizeAngle(*theta_radians + *delta_theta_radians);
44 static ceres::LocalParameterization*
Create()
46 return (
new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, 1, 1>);
57 const T cos_yaw = ceres::cos(yaw_radians);
58 const T sin_yaw = ceres::sin(yaw_radians);
59 Eigen::Matrix<T, 2, 2> rotation;
60 rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
71 PoseGraph2dErrorTerm(
double x_ab,
double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information)
77 bool operator()(
const T*
const x_a,
const T*
const y_a,
const T*
const yaw_a,
const T*
const x_b,
const T*
const y_b,
const T*
const yaw_b, T* residuals_ptr)
const
79 const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
80 const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
81 Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals_map(residuals_ptr);
82 residuals_map.template head<2>() =
RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) -
p_ab_.cast<T>();
89 static ceres::CostFunction*
Create(
double x_ab,
double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information)
91 return (
new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1, 1, 1>(
new PoseGraph2dErrorTerm(x_ab, y_ab, yaw_ab_radians, sqrt_information)));
94 EIGEN_MAKE_ALIGNED_OPERATOR_NEW