From 4d361abe418d98441b1429db180a802e1eb847ef Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Mon, 17 Oct 2016 16:14:24 +0200 Subject: [PATCH] Extract SpaCostFunctions into their own files. (#71) --- .../sparse_pose_graph/CMakeLists.txt | 12 ++ .../sparse_pose_graph/optimization_problem.cc | 1 + .../sparse_pose_graph/optimization_problem.h | 58 ---------- .../sparse_pose_graph/spa_cost_function.h | 85 ++++++++++++++ .../sparse_pose_graph/CMakeLists.txt | 12 ++ .../sparse_pose_graph/optimization_problem.cc | 57 +--------- .../sparse_pose_graph/optimization_problem.h | 28 ----- .../sparse_pose_graph/spa_cost_function.h | 107 ++++++++++++++++++ cartographer/sensor/CMakeLists.txt | 1 + 9 files changed, 219 insertions(+), 142 deletions(-) create mode 100644 cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h create mode 100644 cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h diff --git a/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt b/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt index 6da6c6f..3d7784e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt +++ b/cartographer/mapping_2d/sparse_pose_graph/CMakeLists.txt @@ -56,8 +56,20 @@ google_library(mapping_2d_sparse_pose_graph_optimization_problem common_lua_parameter_dictionary common_math common_port + mapping_2d_sparse_pose_graph_spa_cost_function mapping_2d_submaps mapping_sparse_pose_graph mapping_sparse_pose_graph_proto_optimization_problem_options transform_transform ) + +google_library(mapping_2d_sparse_pose_graph_spa_cost_function + USES_CERES + USES_EIGEN + HDRS + spa_cost_function.h + DEPENDS + common_math + mapping_sparse_pose_graph + transform_rigid_transform +) diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 69ed7ec..e5bf470 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -26,6 +26,7 @@ #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" +#include "cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "glog/logging.h" diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index e58bf03..f928fb8 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -38,52 +38,6 @@ class OptimizationProblem { public: using Constraint = mapping::SparsePoseGraph::Constraint2D; - class SpaCostFunction { - public: - explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} - - // Computes the error in the proposed change to the scan-to-submap alignment - // 'zbar_ij' where submap pose 'c_i' and scan pose 'c_j' 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])}}; - } - - // Scales the result of ComputeUnscaledError scaled by 'sqrt_Lambda_ij' and - // stores it in 'e'. - template - static void ComputeScaledError(const Constraint::Pose& pose, - const T* const c_i, const T* const c_j, - T* const e) { - std::array e_ij = ComputeUnscaledError(pose.zbar_ij, c_i, c_j); - (Eigen::Map>(e)) = - pose.sqrt_Lambda_ij.cast() * - Eigen::Map>(e_ij.data()); - } - - 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; - } - - private: - const Constraint::Pose pose_; - }; - explicit OptimizationProblem( const mapping::sparse_pose_graph::proto::OptimizationProblemOptions& options); @@ -104,18 +58,6 @@ class OptimizationProblem { std::vector* submap_transforms); private: - class TieToPoseCostFunction { - public: - explicit TieToPoseCostFunction(const Constraint::Pose& pose) - : pose_(pose) {} - - template - bool operator()(const T* const c_j, T* e) const; - - private: - const Constraint::Pose pose_; - }; - mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; }; diff --git a/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h b/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h new file mode 100644 index 0000000..5a5ddd0 --- /dev/null +++ b/cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h @@ -0,0 +1,85 @@ +/* + * Copyright 2016 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_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ +#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/transform/rigid_transform.h" +#include "ceres/ceres.h" +#include "ceres/jet.h" + +namespace cartographer { +namespace mapping_2d { +namespace sparse_pose_graph { + +class SpaCostFunction { + public: + using Constraint = mapping::SparsePoseGraph::Constraint2D; + + explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} + + // Computes the error between the scan-to-submap alignment 'zbar_ij' and the + // difference of submap pose 'c_i' and scan 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 'sqrt_Lambda_ij', 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) { + std::array e_ij = ComputeUnscaledError(pose.zbar_ij, c_i, c_j); + (Eigen::Map>(e)) = + pose.sqrt_Lambda_ij.cast() * + Eigen::Map>(e_ij.data()); + } + + 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; + } + + private: + const Constraint::Pose pose_; +}; + +} // namespace sparse_pose_graph +} // namespace mapping_2d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ diff --git a/cartographer/mapping_3d/sparse_pose_graph/CMakeLists.txt b/cartographer/mapping_3d/sparse_pose_graph/CMakeLists.txt index 5e10133..0606f4b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/CMakeLists.txt +++ b/cartographer/mapping_3d/sparse_pose_graph/CMakeLists.txt @@ -62,11 +62,23 @@ google_library(mapping_3d_sparse_pose_graph_optimization_problem mapping_3d_ceres_pose mapping_3d_imu_integration mapping_3d_rotation_cost_function + mapping_3d_sparse_pose_graph_spa_cost_function mapping_3d_submaps mapping_sparse_pose_graph_proto_optimization_problem_options transform_transform ) +google_library(mapping_3d_sparse_pose_graph_spa_cost_function + USES_CERES + USES_EIGEN + HDRS + spa_cost_function.h + DEPENDS + common_math + mapping_sparse_pose_graph + transform_rigid_transform +) + google_test(mapping_3d_sparse_pose_graph_optimization_problem_test USES_EIGEN USES_GLOG diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 860fa35..48a7eaf 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -32,6 +32,7 @@ #include "cartographer/mapping_3d/ceres_pose.h" #include "cartographer/mapping_3d/imu_integration.h" #include "cartographer/mapping_3d/rotation_cost_function.h" +#include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "ceres/jet.h" @@ -76,62 +77,6 @@ OptimizationProblem::OptimizationProblem( OptimizationProblem::~OptimizationProblem() {} -template -std::array OptimizationProblem::SpaCostFunction::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]}}; -} - -template -void OptimizationProblem::SpaCostFunction::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) { - std::array e_ij = - ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation, - c_j_rotation, c_j_translation); - // Matrix-vector product of sqrt_Lambda_ij * e_ij - for (int s = 0; s != 6; ++s) { - e[s] = T(0.); - for (int t = 0; t != 6; ++t) { - e[s] += pose.sqrt_Lambda_ij(s, t) * e_ij[t]; - } - } -} - -template -bool OptimizationProblem::SpaCostFunction::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* e) const { - ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation, - c_j_translation, e); - return true; -} - void OptimizationProblem::AddImuData(common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index a97edb5..4ac4f5f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -73,34 +73,6 @@ class OptimizationProblem { const std::vector& node_data() const; private: - class SpaCostFunction { - public: - explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} - - // Compute the error (linear offset and rotational error) without scaling - // it by the covariance. - 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); - - // Computes the error scaled by 'sqrt_Lambda_ij', 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); - template - 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; - - private: - const Constraint::Pose pose_; - }; - mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; std::deque imu_data_; std::vector node_data_; diff --git a/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h new file mode 100644 index 0000000..14e022e --- /dev/null +++ b/cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h @@ -0,0 +1,107 @@ +/* + * Copyright 2016 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_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ +#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/transform/rigid_transform.h" +#include "ceres/ceres.h" +#include "ceres/jet.h" + +namespace cartographer { +namespace mapping_3d { +namespace sparse_pose_graph { + +class SpaCostFunction { + public: + using Constraint = mapping::SparsePoseGraph::Constraint3D; + + explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {} + + // Computes the error between the scan-to-submap alignment 'zbar_ij' and the + // difference of submap pose 'c_i' and scan 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 'sqrt_Lambda_ij', 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) { + std::array e_ij = + ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation, + c_j_rotation, c_j_translation); + (Eigen::Map>(e)) = + pose.sqrt_Lambda_ij.cast() * + Eigen::Map>(e_ij.data()); + } + + template + 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; + } + + private: + const Constraint::Pose pose_; +}; + +} // namespace sparse_pose_graph +} // namespace mapping_3d +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_ diff --git a/cartographer/sensor/CMakeLists.txt b/cartographer/sensor/CMakeLists.txt index ecca7e6..72a9597 100644 --- a/cartographer/sensor/CMakeLists.txt +++ b/cartographer/sensor/CMakeLists.txt @@ -23,6 +23,7 @@ google_library(sensor_collator common_make_unique common_ordered_multi_queue common_time + sensor_data sensor_sensor_packet_period_histogram_builder )