17 #ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ 18 #define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ 23 #include "Eigen/Geometry" 28 #include "ceres/ceres.h" 29 #include "ceres/jet.h" 32 namespace mapping_3d {
33 namespace sparse_pose_graph {
47 const T*
const c_i_translation,
const T*
const c_j_rotation,
48 const T*
const c_j_translation) {
49 const Eigen::Quaternion<T> R_i_inverse(c_i_rotation[0], -c_i_rotation[1],
50 -c_i_rotation[2], -c_i_rotation[3]);
52 const Eigen::Matrix<T, 3, 1> delta(c_j_translation[0] - c_i_translation[0],
53 c_j_translation[1] - c_i_translation[1],
54 c_j_translation[2] - c_i_translation[2]);
55 const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;
57 const Eigen::Quaternion<T> h_rotation_inverse =
58 Eigen::Quaternion<T>(c_j_rotation[0], -c_j_rotation[1],
59 -c_j_rotation[2], -c_j_rotation[3]) *
60 Eigen::Quaternion<T>(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2],
63 const Eigen::Matrix<T, 3, 1> angle_axis_difference =
65 h_rotation_inverse * zbar_ij.
rotation().cast<T>());
67 return {{T(zbar_ij.
translation().x()) - h_translation[0],
70 angle_axis_difference[0], angle_axis_difference[1],
71 angle_axis_difference[2]}};
78 const T*
const c_i_rotation,
79 const T*
const c_i_translation,
80 const T*
const c_j_rotation,
81 const T*
const c_j_translation, T*
const e) {
82 const std::array<T, 6> e_ij =
84 c_j_rotation, c_j_translation);
85 for (
int ij : {0, 1, 2}) {
88 for (
int ij : {3, 4, 5}) {
94 bool operator()(
const T*
const c_i_rotation,
const T*
const c_i_translation,
95 const T*
const c_j_rotation,
const T*
const c_j_translation,
110 #endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_
SpaCostFunction(const Constraint::Pose &pose)
const Constraint::Pose pose_
static void ComputeScaledError(const Constraint::Pose &pose, const T *const c_i_rotation, const T *const c_i_translation, const T *const c_j_rotation, const T *const c_j_translation, T *const e)
transform::Rigid3d zbar_ij
bool operator()(const T *const c_i_rotation, const T *const c_i_translation, const T *const c_j_rotation, const T *const c_j_translation, T *const e) const
double translation_weight
static std::array< T, 6 > ComputeUnscaledError(const transform::Rigid3d &zbar_ij, const T *const c_i_rotation, const T *const c_i_translation, const T *const c_j_rotation, const T *const c_j_translation)