diff --git a/cartographer/mapping/pose_graph/cost_helpers.h b/cartographer/mapping/pose_graph/cost_helpers.h index 717e8ef..3238f92 100644 --- a/cartographer/mapping/pose_graph/cost_helpers.h +++ b/cartographer/mapping/pose_graph/cost_helpers.h @@ -31,9 +31,11 @@ namespace pose_graph { // // 'start' and 'end' poses have the format [x, y, rotation]. template -static std::array ComputeUnscaledError2d( - const transform::Rigid2d& relative_pose, const T* const start, - const T* const end); +std::array ComputeUnscaledError(const transform::Rigid2d& relative_pose, + const T* const start, const T* const end); +template +std::array ScaleError(std::array error, T translation_weight, + T rotation_weight); // Computes the error between the given relative pose and the difference of // poses 'start' and 'end' which are both in an arbitrary common frame. @@ -41,11 +43,22 @@ static std::array ComputeUnscaledError2d( // 'start' and 'end' translation has the format [x, y, z]. // 'start' and 'end' rotation are quaternions in the format [w, n_1, n_2, n_3]. template -static std::array ComputeUnscaledError3d( - const transform::Rigid3d& relative_pose, const T* const start_rotation, - const T* const start_translation, const T* const end_rotation, - const T* const end_translation); +std::array ComputeUnscaledError(const transform::Rigid3d& relative_pose, + const T* const start_rotation, + const T* const start_translation, + const T* const end_rotation, + const T* const end_translation); +template +std::array ScaleError(std::array error, T translation_weight, + T rotation_weight); + +// Computes spherical linear interpolation of unit quaternions. +// +// 'start' and 'end' are quaternions in the format [w, n_1, n_2, n_3]. +template +std::array SlerpQuaternions(const T* const start, const T* const end, + T factor); } // namespace pose_graph } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/pose_graph/cost_helpers_impl.h b/cartographer/mapping/pose_graph/cost_helpers_impl.h index 80d0e5b..c0c9265 100644 --- a/cartographer/mapping/pose_graph/cost_helpers_impl.h +++ b/cartographer/mapping/pose_graph/cost_helpers_impl.h @@ -22,7 +22,7 @@ namespace mapping { namespace pose_graph { template -static std::array ComputeUnscaledError2d( +static std::array ComputeUnscaledError( const transform::Rigid2d& relative_pose, const T* const start, const T* const end) { const T cos_theta_i = cos(start[2]); @@ -39,7 +39,17 @@ static std::array ComputeUnscaledError2d( } template -static std::array ComputeUnscaledError3d( +std::array ScaleError(std::array error, T translation_weight, + T rotation_weight) { + std::array scaled_error(std::move(error)); + scaled_error[0] *= translation_weight; + scaled_error[1] *= translation_weight; + scaled_error[2] *= rotation_weight; + return scaled_error; +} + +template +static std::array ComputeUnscaledError( const transform::Rigid3d& relative_pose, const T* const start_rotation, const T* const start_translation, const T* const end_rotation, const T* const end_translation) { @@ -69,6 +79,46 @@ static std::array ComputeUnscaledError3d( angle_axis_difference[2]}}; } +template +std::array ScaleError(std::array error, T translation_weight, + T rotation_weight) { + std::array scaled_error(std::move(error)); + scaled_error[0] *= translation_weight; + scaled_error[1] *= translation_weight; + scaled_error[2] *= translation_weight; + scaled_error[3] *= rotation_weight; + scaled_error[4] *= rotation_weight; + scaled_error[5] *= rotation_weight; + return scaled_error; +} + +// Eigen implementation of slerp is not compatible with Ceres on all supported +// platforms. Our own implementation is used instead. +template +std::array SlerpQuaternions(const T* const start, const T* const end, + T factor) { + // Angle 'theta' is the half-angle "between" quaternions. It can be computed + // as the arccosine of their dot product. + const T cos_theta = start[0] * end[0] + start[1] * end[1] + + start[2] * end[2] + start[3] * end[3]; + const T abs_cos_theta = abs(cos_theta); + // If numerical error brings 'cos_theta' outside [-1 + epsilon, 1 - epsilon] + // interval, then the quaternions are likely to be collinear. + T prev_scale = T(1.) - factor; + T next_scale = factor; + if (abs_cos_theta < T(1. - 1e-5)) { + const T theta = acos(abs_cos_theta); + const T sin_theta = sin(theta); + prev_scale = sin(prev_scale * theta) / sin_theta; + next_scale = sin(next_scale * theta) / sin_theta; + } + if (cos_theta < T(0.)) next_scale = -next_scale; + return {{prev_scale * start[0] + next_scale * end[0], + prev_scale * start[1] + next_scale * end[1], + prev_scale * start[2] + next_scale * end[2], + prev_scale * start[3] + next_scale * end[3]}}; +} + } // namespace pose_graph } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/pose_graph/landmark_cost_function.h b/cartographer/mapping/pose_graph/landmark_cost_function.h index 7599b36..ab3736d 100644 --- a/cartographer/mapping/pose_graph/landmark_cost_function.h +++ b/cartographer/mapping/pose_graph/landmark_cost_function.h @@ -30,33 +30,6 @@ namespace cartographer { namespace mapping { namespace pose_graph { -template -std::array SlerpQuaternions(const T* const prev_rotation, - const T* const next_rotation, T factor) { - // Angle 'theta' is the half-angle "between" quaternions. It can be computed - // as the arccosine of their dot product. - const T cos_theta = prev_rotation[0] * next_rotation[0] + - prev_rotation[1] * next_rotation[1] + - prev_rotation[2] * next_rotation[2] + - prev_rotation[3] * next_rotation[3]; - // If numerical error brings 'cos_theta' outside of [-1., 1.] interval, then - // the quaternions are likely to be collinear. - if (cos_theta >= T(1.0) || cos_theta <= T(-1.0)) { - return {{next_rotation[0], next_rotation[1], next_rotation[2], - next_rotation[3]}}; - } - const T theta = acos(abs(cos_theta)); - const T sin_theta = sin(theta); - const T prev_scale = sin((T(1.0) - factor) * theta) / sin_theta; - const T next_scale = - sin(factor * theta) * (cos_theta < T(0) ? T(-1.0) : T(1.0)) / sin_theta; - - return {{prev_scale * prev_rotation[0] + next_scale * next_rotation[0], - prev_scale * prev_rotation[1] + next_scale * next_rotation[1], - prev_scale * prev_rotation[2] + next_scale * next_rotation[2], - prev_scale * prev_rotation[3] + next_scale * next_rotation[3]}}; -} - // Cost function measuring the weighted error between the observed pose given by // the landmark measurement and the linearly interpolated pose. class LandmarkCostFunction { @@ -85,30 +58,26 @@ class LandmarkCostFunction { const T* const next_node_translation, const T* const landmark_rotation, const T* const landmark_translation, T* const e) const { - const T interpolated_pose_translation[3] = { - prev_node_translation[0] + - interpolation_parameter_ * - (next_node_translation[0] - prev_node_translation[0]), - prev_node_translation[1] + - interpolation_parameter_ * - (next_node_translation[1] - prev_node_translation[1]), - prev_node_translation[2] + - interpolation_parameter_ * - (next_node_translation[2] - prev_node_translation[2])}; - - std::array interpolated_pose_rotation = SlerpQuaternions( + const std::array interpolated_pose_translation{ + {prev_node_translation[0] + + interpolation_parameter_ * + (next_node_translation[0] - prev_node_translation[0]), + prev_node_translation[1] + + interpolation_parameter_ * + (next_node_translation[1] - prev_node_translation[1]), + prev_node_translation[2] + + interpolation_parameter_ * + (next_node_translation[2] - prev_node_translation[2])}}; + const std::array interpolated_pose_rotation = SlerpQuaternions( prev_node_rotation, next_node_rotation, T(interpolation_parameter_)); - const std::array unscaled_error = ComputeUnscaledError3d( - landmark_to_tracking_transform_, interpolated_pose_rotation.data(), - interpolated_pose_translation, landmark_rotation, landmark_translation); - - e[0] = T(translation_weight_) * unscaled_error[0]; - e[1] = T(translation_weight_) * unscaled_error[1]; - e[2] = T(translation_weight_) * unscaled_error[2]; - e[3] = T(rotation_weight_) * unscaled_error[3]; - e[4] = T(rotation_weight_) * unscaled_error[4]; - e[5] = T(rotation_weight_) * unscaled_error[5]; + const std::array error = ScaleError( + ComputeUnscaledError(landmark_to_tracking_transform_, + interpolated_pose_rotation.data(), + interpolated_pose_translation.data(), + landmark_rotation, landmark_translation), + T(translation_weight_), T(rotation_weight_)); + std::copy(std::begin(error), std::end(error), e); return true; } diff --git a/cartographer/mapping_2d/pose_graph/spa_cost_function.h b/cartographer/mapping_2d/pose_graph/spa_cost_function.h index 9554316..25ee9d6 100644 --- a/cartographer/mapping_2d/pose_graph/spa_cost_function.h +++ b/cartographer/mapping_2d/pose_graph/spa_cost_function.h @@ -47,21 +47,14 @@ class SpaCostFunction { template bool operator()(const T* const c_i, const T* const c_j, T* e) const { - ComputeScaledError(pose_, c_i, c_j, e); - return true; - } + using mapping::pose_graph::ComputeUnscaledError; + using mapping::pose_graph::ScaleError; - // Computes the error scaled by 'translation_weight' and 'rotation_weight', - // storing it in 'e'. - template - static void ComputeScaledError(const Constraint::Pose& pose, - const T* const c_i, const T* const c_j, - T* const e) { - const std::array e_ij = mapping::pose_graph::ComputeUnscaledError2d( - transform::Project2D(pose.zbar_ij), c_i, c_j); - e[0] = e_ij[0] * T(pose.translation_weight); - e[1] = e_ij[1] * T(pose.translation_weight); - e[2] = e_ij[2] * T(pose.rotation_weight); + const std::array error = ScaleError( + ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j), + T(pose_.translation_weight), T(pose_.rotation_weight)); + std::copy(std::begin(error), std::end(error), e); + return true; } private: diff --git a/cartographer/mapping_3d/pose_graph/spa_cost_function.h b/cartographer/mapping_3d/pose_graph/spa_cost_function.h index 5ebbcd6..da3ca5d 100644 --- a/cartographer/mapping_3d/pose_graph/spa_cost_function.h +++ b/cartographer/mapping_3d/pose_graph/spa_cost_function.h @@ -49,28 +49,15 @@ class SpaCostFunction { 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 { - ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation, - c_j_translation, e); - return true; - } + using mapping::pose_graph::ComputeUnscaledError; + using mapping::pose_graph::ScaleError; - // Computes the error scaled by 'translation_weight' and 'rotation_weight', - // storing it in 'e'. - template - 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) { - const std::array e_ij = mapping::pose_graph::ComputeUnscaledError3d( - pose.zbar_ij, c_i_rotation, c_i_translation, c_j_rotation, - c_j_translation); - for (int ij : {0, 1, 2}) { - e[ij] = e_ij[ij] * T(pose.translation_weight); - } - for (int ij : {3, 4, 5}) { - e[ij] = e_ij[ij] * T(pose.rotation_weight); - } + const std::array error = ScaleError( + ComputeUnscaledError(pose_.zbar_ij, c_i_rotation, c_i_translation, + c_j_rotation, c_j_translation), + T(pose_.translation_weight), T(pose_.rotation_weight)); + std::copy(std::begin(error), std::end(error), e); + return true; } private: