17 #ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ 18 #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ 23 #include "Eigen/Geometry" 28 #include "ceres/ceres.h" 29 #include "ceres/jet.h" 32 namespace mapping_2d {
33 namespace sparse_pose_graph {
48 const T cos_theta_i = cos(c_i[2]);
49 const T sin_theta_i = sin(c_i[2]);
50 const T delta_x = c_j[0] - c_i[0];
51 const T delta_y = c_j[1] - c_i[1];
52 const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y,
53 -sin_theta_i * delta_x + cos_theta_i * delta_y,
65 const T*
const c_i,
const T*
const c_j,
67 const std::array<T, 3> e_ij =
75 bool operator()(
const T*
const c_i,
const T*
const c_j, T* e)
const {
88 #endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_
static std::array< T, 3 > ComputeUnscaledError(const transform::Rigid2d &zbar_ij, const T *const c_i, const T *const c_j)
const Constraint::Pose pose_
transform::Rigid3d zbar_ij
T NormalizeAngleDifference(T difference)
bool operator()(const T *const c_i, const T *const c_j, T *e) const
SpaCostFunction(const Constraint::Pose &pose)
double translation_weight
static void ComputeScaledError(const Constraint::Pose &pose, const T *const c_i, const T *const c_j, T *const e)