From 708e7fc57d6e90edb4e00115c073748fd6368ca3 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Tue, 30 Jan 2018 22:12:25 +0100 Subject: [PATCH] Add a 'cost_helpers' library. (#862) * Add a 'cost_helpers' library. * Change naming and add comments. --- .../mapping/pose_graph/cost_helpers.h | 55 ++++++++++++++ .../mapping/pose_graph/cost_helpers_impl.h | 76 +++++++++++++++++++ .../pose_graph/landmark_cost_function.h | 14 ++-- .../pose_graph/landmark_cost_function_test.cc | 2 +- .../mapping_2d/pose_graph/spa_cost_function.h | 25 +----- .../mapping_3d/pose_graph/spa_cost_function.h | 40 +--------- 6 files changed, 144 insertions(+), 68 deletions(-) create mode 100644 cartographer/mapping/pose_graph/cost_helpers.h create mode 100644 cartographer/mapping/pose_graph/cost_helpers_impl.h diff --git a/cartographer/mapping/pose_graph/cost_helpers.h b/cartographer/mapping/pose_graph/cost_helpers.h new file mode 100644 index 0000000..717e8ef --- /dev/null +++ b/cartographer/mapping/pose_graph/cost_helpers.h @@ -0,0 +1,55 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_H_ + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" + +namespace cartographer { +namespace mapping { +namespace pose_graph { + +// Computes the error between the given relative pose and the difference of +// poses 'start' and 'end' which are both in an arbitrary common frame. +// +// '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); + +// Computes the error between the given relative pose and the difference of +// poses 'start' and 'end' which are both in an arbitrary common frame. +// +// '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); + +} // namespace pose_graph +} // namespace mapping +} // namespace cartographer + +#include "cartographer/mapping/pose_graph/cost_helpers_impl.h" + +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_H_ diff --git a/cartographer/mapping/pose_graph/cost_helpers_impl.h b/cartographer/mapping/pose_graph/cost_helpers_impl.h new file mode 100644 index 0000000..80d0e5b --- /dev/null +++ b/cartographer/mapping/pose_graph/cost_helpers_impl.h @@ -0,0 +1,76 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_IMPL_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_IMPL_H_ + +namespace cartographer { +namespace mapping { +namespace pose_graph { + +template +static std::array ComputeUnscaledError2d( + const transform::Rigid2d& relative_pose, const T* const start, + const T* const end) { + const T cos_theta_i = cos(start[2]); + const T sin_theta_i = sin(start[2]); + const T delta_x = end[0] - start[0]; + const T delta_y = end[1] - start[1]; + const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y, + -sin_theta_i * delta_x + cos_theta_i * delta_y, + end[2] - start[2]}; + return {{T(relative_pose.translation().x()) - h[0], + T(relative_pose.translation().y()) - h[1], + common::NormalizeAngleDifference( + T(relative_pose.rotation().angle()) - h[2])}}; +} + +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) { + const Eigen::Quaternion R_i_inverse(start_rotation[0], -start_rotation[1], + -start_rotation[2], + -start_rotation[3]); + + const Eigen::Matrix delta(end_translation[0] - start_translation[0], + end_translation[1] - start_translation[1], + end_translation[2] - start_translation[2]); + const Eigen::Matrix h_translation = R_i_inverse * delta; + + const Eigen::Quaternion h_rotation_inverse = + Eigen::Quaternion(end_rotation[0], -end_rotation[1], -end_rotation[2], + -end_rotation[3]) * + Eigen::Quaternion(start_rotation[0], start_rotation[1], + start_rotation[2], start_rotation[3]); + + const Eigen::Matrix angle_axis_difference = + transform::RotationQuaternionToAngleAxisVector( + h_rotation_inverse * relative_pose.rotation().cast()); + + return {{T(relative_pose.translation().x()) - h_translation[0], + T(relative_pose.translation().y()) - h_translation[1], + T(relative_pose.translation().z()) - h_translation[2], + angle_axis_difference[0], angle_axis_difference[1], + angle_axis_difference[2]}}; +} + +} // namespace pose_graph +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_IMPL_H_ diff --git a/cartographer/mapping/pose_graph/landmark_cost_function.h b/cartographer/mapping/pose_graph/landmark_cost_function.h index d8cd534..7599b36 100644 --- a/cartographer/mapping/pose_graph/landmark_cost_function.h +++ b/cartographer/mapping/pose_graph/landmark_cost_function.h @@ -19,8 +19,8 @@ #include "Eigen/Core" #include "Eigen/Geometry" +#include "cartographer/mapping/pose_graph/cost_helpers.h" #include "cartographer/mapping/pose_graph_interface.h" -#include "cartographer/mapping_3d/pose_graph/spa_cost_function.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -62,7 +62,7 @@ std::array SlerpQuaternions(const T* const prev_rotation, class LandmarkCostFunction { public: using LandmarkObservation = - mapping::PoseGraph::LandmarkNode::LandmarkObservation; + mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; static ceres::CostFunction* CreateAutoDiffCostFunction( const LandmarkObservation& observation, common::Time prev_node_time, @@ -99,13 +99,9 @@ class LandmarkCostFunction { std::array interpolated_pose_rotation = SlerpQuaternions( prev_node_rotation, next_node_rotation, T(interpolation_parameter_)); - // TODO(pifon2a): Move functions common for all cost functions outside of - // SpaCostFunction scope. - const std::array unscaled_error = - mapping_3d::pose_graph::SpaCostFunction::ComputeUnscaledError( - landmark_to_tracking_transform_, interpolated_pose_rotation.data(), - interpolated_pose_translation, landmark_rotation, - landmark_translation); + 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]; diff --git a/cartographer/mapping/pose_graph/landmark_cost_function_test.cc b/cartographer/mapping/pose_graph/landmark_cost_function_test.cc index bfbc774..85242b0 100644 --- a/cartographer/mapping/pose_graph/landmark_cost_function_test.cc +++ b/cartographer/mapping/pose_graph/landmark_cost_function_test.cc @@ -29,7 +29,7 @@ using ::testing::DoubleEq; using ::testing::ElementsAre; using LandmarkObservation = - mapping::PoseGraph::LandmarkNode::LandmarkObservation; + mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; TEST(LandmarkCostFunctionTest, SmokeTest) { auto* cost_function = LandmarkCostFunction::CreateAutoDiffCostFunction( diff --git a/cartographer/mapping_2d/pose_graph/spa_cost_function.h b/cartographer/mapping_2d/pose_graph/spa_cost_function.h index 3ea80ad..9554316 100644 --- a/cartographer/mapping_2d/pose_graph/spa_cost_function.h +++ b/cartographer/mapping_2d/pose_graph/spa_cost_function.h @@ -23,6 +23,7 @@ #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/pose_graph/cost_helpers.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -50,34 +51,14 @@ class SpaCostFunction { return true; } - // Computes the error between the node-to-submap alignment 'zbar_ij' and the - // difference of submap pose 'c_i' and node pose 'c_j' which are both in an - // arbitrary common frame. - template - static std::array ComputeUnscaledError( - const transform::Rigid2d& zbar_ij, const T* const c_i, - const T* const c_j) { - const T cos_theta_i = cos(c_i[2]); - const T sin_theta_i = sin(c_i[2]); - const T delta_x = c_j[0] - c_i[0]; - const T delta_y = c_j[1] - c_i[1]; - const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y, - -sin_theta_i * delta_x + cos_theta_i * delta_y, - c_j[2] - c_i[2]}; - return {{T(zbar_ij.translation().x()) - h[0], - T(zbar_ij.translation().y()) - h[1], - common::NormalizeAngleDifference(T(zbar_ij.rotation().angle()) - - h[2])}}; - } - // 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 = - ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j); + 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); diff --git a/cartographer/mapping_3d/pose_graph/spa_cost_function.h b/cartographer/mapping_3d/pose_graph/spa_cost_function.h index 6fe0b95..5ebbcd6 100644 --- a/cartographer/mapping_3d/pose_graph/spa_cost_function.h +++ b/cartographer/mapping_3d/pose_graph/spa_cost_function.h @@ -23,6 +23,7 @@ #include "Eigen/Geometry" #include "cartographer/common/math.h" #include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/pose_graph/cost_helpers.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -53,39 +54,6 @@ class SpaCostFunction { return true; } - // Computes the error between the node-to-submap alignment 'zbar_ij' and the - // difference of submap pose 'c_i' and node pose 'c_j' which are both in an - // arbitrary common frame. - template - static std::array 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) { - const Eigen::Quaternion R_i_inverse(c_i_rotation[0], -c_i_rotation[1], - -c_i_rotation[2], -c_i_rotation[3]); - - const Eigen::Matrix delta(c_j_translation[0] - c_i_translation[0], - c_j_translation[1] - c_i_translation[1], - c_j_translation[2] - c_i_translation[2]); - const Eigen::Matrix h_translation = R_i_inverse * delta; - - const Eigen::Quaternion h_rotation_inverse = - Eigen::Quaternion(c_j_rotation[0], -c_j_rotation[1], - -c_j_rotation[2], -c_j_rotation[3]) * - Eigen::Quaternion(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2], - c_i_rotation[3]); - - const Eigen::Matrix angle_axis_difference = - transform::RotationQuaternionToAngleAxisVector( - h_rotation_inverse * zbar_ij.rotation().cast()); - - return {{T(zbar_ij.translation().x()) - h_translation[0], - T(zbar_ij.translation().y()) - h_translation[1], - T(zbar_ij.translation().z()) - h_translation[2], - angle_axis_difference[0], angle_axis_difference[1], - angle_axis_difference[2]}}; - } - // Computes the error scaled by 'translation_weight' and 'rotation_weight', // storing it in 'e'. template @@ -94,9 +62,9 @@ class SpaCostFunction { 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 = - ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation, - c_j_rotation, c_j_translation); + 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); }