From 8ea46857ac69ff8d0dff5c90ad3da8df4e6d1621 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 1 Feb 2018 15:16:50 +0100 Subject: [PATCH] Move cerec_pose.* to mapping/pose_graph. (#874) --- .../{mapping_3d => mapping/pose_graph}/ceres_pose.cc | 8 +++++--- .../{mapping_3d => mapping/pose_graph}/ceres_pose.h | 12 +++++++----- .../mapping_3d/pose_graph/optimization_problem.cc | 5 +++-- .../mapping_3d/scan_matching/ceres_scan_matcher.cc | 4 ++-- 4 files changed, 17 insertions(+), 12 deletions(-) rename cartographer/{mapping_3d => mapping/pose_graph}/ceres_pose.cc (92%) rename cartographer/{mapping_3d => mapping/pose_graph}/ceres_pose.h (84%) diff --git a/cartographer/mapping_3d/ceres_pose.cc b/cartographer/mapping/pose_graph/ceres_pose.cc similarity index 92% rename from cartographer/mapping_3d/ceres_pose.cc rename to cartographer/mapping/pose_graph/ceres_pose.cc index 659e4f3..19a4788 100644 --- a/cartographer/mapping_3d/ceres_pose.cc +++ b/cartographer/mapping/pose_graph/ceres_pose.cc @@ -14,10 +14,11 @@ * limitations under the License. */ -#include "cartographer/mapping_3d/ceres_pose.h" +#include "cartographer/mapping/pose_graph/ceres_pose.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { +namespace pose_graph { CeresPose::CeresPose( const transform::Rigid3d& rigid, @@ -42,5 +43,6 @@ const transform::Rigid3d CeresPose::ToRigid() const { Eigen::Quaterniond(rotation[0], rotation[1], rotation[2], rotation[3])); } -} // namespace mapping_3d +} // namespace pose_graph +} // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_3d/ceres_pose.h b/cartographer/mapping/pose_graph/ceres_pose.h similarity index 84% rename from cartographer/mapping_3d/ceres_pose.h rename to cartographer/mapping/pose_graph/ceres_pose.h index b5f5230..fc0de6a 100644 --- a/cartographer/mapping_3d/ceres_pose.h +++ b/cartographer/mapping/pose_graph/ceres_pose.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ -#define CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_CERES_POSE_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_CERES_POSE_H_ #include #include @@ -25,7 +25,8 @@ #include "ceres/ceres.h" namespace cartographer { -namespace mapping_3d { +namespace mapping { +namespace pose_graph { class CeresPose { public: @@ -49,7 +50,8 @@ class CeresPose { std::shared_ptr data_; }; -} // namespace mapping_3d +} // namespace pose_graph +} // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_3D_CERES_POSE_H_ +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_CERES_POSE_H_ diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc index cb5a545..6316c5b 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc @@ -32,7 +32,7 @@ #include "cartographer/common/time.h" #include "cartographer/internal/mapping_3d/acceleration_cost_function.h" #include "cartographer/internal/mapping_3d/rotation_cost_function.h" -#include "cartographer/mapping_3d/ceres_pose.h" +#include "cartographer/mapping/pose_graph/ceres_pose.h" #include "cartographer/mapping_3d/imu_integration.h" #include "cartographer/mapping_3d/pose_graph/spa_cost_function.h" #include "cartographer/mapping_3d/rotation_parameterization.h" @@ -46,9 +46,10 @@ namespace cartographer { namespace mapping_3d { namespace pose_graph { - namespace { +using ::cartographer::mapping::pose_graph::CeresPose; + // For odometry. std::unique_ptr Interpolate( const sensor::MapByTime& map_by_time, diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc index da77039..a504cc6 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc @@ -23,7 +23,7 @@ #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/make_unique.h" #include "cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h" -#include "cartographer/mapping_3d/ceres_pose.h" +#include "cartographer/mapping/pose_graph/ceres_pose.h" #include "cartographer/mapping_3d/rotation_parameterization.h" #include "cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h" #include "cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h" @@ -75,7 +75,7 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation, transform::Rigid3d* const pose_estimate, ceres::Solver::Summary* const summary) { ceres::Problem problem; - CeresPose ceres_pose( + mapping::pose_graph::CeresPose ceres_pose( initial_pose_estimate, nullptr /* translation_parameterization */, options_.only_optimize_yaw() ? std::unique_ptr(