From 4aa2af23dedb824e0e2f9d4db7dba2f238d98e44 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Fri, 20 Apr 2018 20:52:17 +0200 Subject: [PATCH] Move files related to optimization to optimization/. (#1102) --- .../mapping/internal/2d/pose_graph_2d.cc | 15 +++++++------- .../mapping/internal/2d/pose_graph_2d.h | 10 +++++----- .../mapping/internal/2d/pose_graph_2d_test.cc | 2 +- .../mapping/internal/3d/pose_graph_3d.cc | 12 +++++------ .../mapping/internal/3d/pose_graph_3d.h | 10 +++++----- .../mapping/internal/3d/pose_graph_3d_test.cc | 8 ++++---- .../3d/scan_matching/ceres_scan_matcher_3d.cc | 4 ++-- .../ceres_pose.cc | 6 +++--- .../{pose_graph => optimization}/ceres_pose.h | 10 +++++----- .../acceleration_cost_function_3d.h | 6 +++--- .../cost_functions}/cost_helpers.h | 12 +++++------ .../cost_functions}/cost_helpers_impl.h | 10 +++++----- .../landmark_cost_function_2d.h | 14 ++++++------- .../landmark_cost_function_2d_test.cc | 6 +++--- .../landmark_cost_function_3d.h | 14 ++++++------- .../landmark_cost_function_3d_test.cc | 6 +++--- .../rotation_cost_function_3d.h | 20 +++++++++---------- .../cost_functions}/spa_cost_function_2d.h | 16 +++++++-------- .../cost_functions}/spa_cost_function_3d.h | 12 +++++------ .../optimization_problem_2d.cc | 15 +++++++------- .../optimization_problem_2d.h | 16 +++++++-------- .../optimization_problem_3d.cc | 20 +++++++++---------- .../optimization_problem_3d.h | 16 +++++++-------- .../optimization_problem_3d_test.cc | 12 +++++------ .../optimization_problem_interface.h | 10 +++++----- .../optimization_problem_options.cc | 6 +++--- .../optimization_problem_options.h | 10 +++++----- cartographer/mapping/map_builder.cc | 4 ++-- cartographer/mapping/pose_graph.cc | 4 ++-- .../proto/optimization_problem_options.proto | 2 +- .../mapping/proto/pose_graph_options.proto | 2 +- 31 files changed, 155 insertions(+), 155 deletions(-) rename cartographer/mapping/internal/{pose_graph => optimization}/ceres_pose.cc (93%) rename cartographer/mapping/internal/{pose_graph => optimization}/ceres_pose.h (86%) rename cartographer/mapping/internal/{3d => optimization/cost_functions}/acceleration_cost_function_3d.h (93%) rename cartographer/mapping/internal/{pose_graph => optimization/cost_functions}/cost_helpers.h (89%) rename cartographer/mapping/internal/{pose_graph => optimization/cost_functions}/cost_helpers_impl.h (96%) rename cartographer/mapping/internal/{2d/pose_graph => optimization/cost_functions}/landmark_cost_function_2d.h (88%) rename cartographer/mapping/internal/{2d/pose_graph => optimization/cost_functions}/landmark_cost_function_2d_test.cc (94%) rename cartographer/mapping/internal/{3d/pose_graph => optimization/cost_functions}/landmark_cost_function_3d.h (87%) rename cartographer/mapping/internal/{3d/pose_graph => optimization/cost_functions}/landmark_cost_function_3d_test.cc (94%) rename cartographer/mapping/internal/{3d => optimization/cost_functions}/rotation_cost_function_3d.h (75%) rename cartographer/mapping/internal/{2d/pose_graph => optimization/cost_functions}/spa_cost_function_2d.h (79%) rename cartographer/mapping/internal/{3d/pose_graph => optimization/cost_functions}/spa_cost_function_3d.h (83%) rename cartographer/mapping/internal/{2d/pose_graph => optimization}/optimization_problem_2d.cc (96%) rename cartographer/mapping/internal/{2d/pose_graph => optimization}/optimization_problem_2d.h (88%) rename cartographer/mapping/internal/{3d/pose_graph => optimization}/optimization_problem_3d.cc (97%) rename cartographer/mapping/internal/{3d/pose_graph => optimization}/optimization_problem_3d.h (90%) rename cartographer/mapping/internal/{3d/pose_graph => optimization}/optimization_problem_3d_test.cc (95%) rename cartographer/mapping/internal/{pose_graph => optimization}/optimization_problem_interface.h (91%) rename cartographer/mapping/internal/{pose_graph => optimization}/optimization_problem_options.cc (94%) rename cartographer/mapping/internal/{pose_graph => optimization}/optimization_problem_options.h (76%) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 234f800..74ecf75 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -41,7 +41,7 @@ namespace mapping { PoseGraph2D::PoseGraph2D( const proto::PoseGraphOptions& options, - std::unique_ptr optimization_problem, + std::unique_ptr optimization_problem, common::ThreadPool* thread_pool) : options_(options), optimization_problem_(std::move(optimization_problem)), @@ -250,8 +250,9 @@ void PoseGraph2D::ComputeConstraintsForNode( local_pose_2d; optimization_problem_->AddTrajectoryNode( matching_id.trajectory_id, - pose_graph::NodeSpec2D{constant_data->time, local_pose_2d, global_pose_2d, - constant_data->gravity_alignment}); + optimization::NodeSpec2D{constant_data->time, local_pose_2d, + global_pose_2d, + constant_data->gravity_alignment}); for (size_t i = 0; i < insertion_submaps.size(); ++i) { const SubmapId submap_id = submap_ids[i]; // Even if this was the last node added to 'submap_id', the submap will @@ -446,8 +447,8 @@ void PoseGraph2D::AddSubmapFromProto( submap_data_.Insert(submap_id, InternalSubmapData()); submap_data_.at(submap_id).submap = submap_ptr; // Immediately show the submap at the 'global_submap_pose'. - global_submap_poses_.Insert(submap_id, - pose_graph::SubmapSpec2D{global_submap_pose_2d}); + global_submap_poses_.Insert( + submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) { submap_data_.at(submap_id).state = SubmapState::kFinished; optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); @@ -471,7 +472,7 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, constant_data->gravity_alignment.inverse()); optimization_problem_->InsertTrajectoryNode( node_id, - pose_graph::NodeSpec2D{ + optimization::NodeSpec2D{ constant_data->time, transform::Project2D(constant_data->local_pose * gravity_alignment_inverse), @@ -746,7 +747,7 @@ PoseGraph2D::GetAllSubmapPoses() { } transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform( - const MapById& global_submap_poses, + const MapById& global_submap_poses, const int trajectory_id) const { auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index a5ee8ad..971683e 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -34,7 +34,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/2d/submap_2d.h" #include "cartographer/mapping/internal/2d/pose_graph/constraint_builder_2d.h" -#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph_trimmer.h" @@ -61,7 +61,7 @@ class PoseGraph2D : public PoseGraph { public: PoseGraph2D( const proto::PoseGraphOptions& options, - std::unique_ptr optimization_problem, + std::unique_ptr optimization_problem, common::ThreadPool* thread_pool); ~PoseGraph2D() override; @@ -201,7 +201,7 @@ class PoseGraph2D : public PoseGraph { // Computes the local to global map frame transform based on the given // 'global_submap_poses'. transform::Rigid3d ComputeLocalToGlobalTransform( - const MapById& global_submap_poses, + const MapById& global_submap_poses, int trajectory_id) const REQUIRES(mutex_); SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) REQUIRES(mutex_); @@ -239,7 +239,7 @@ class PoseGraph2D : public PoseGraph { void DispatchOptimization() REQUIRES(mutex_); // Current optimization problem. - std::unique_ptr optimization_problem_; + std::unique_ptr optimization_problem_; pose_graph::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_); std::vector constraints_ GUARDED_BY(mutex_); @@ -252,7 +252,7 @@ class PoseGraph2D : public PoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Global submap poses currently used for displaying data. - MapById global_submap_poses_ + MapById global_submap_poses_ GUARDED_BY(mutex_); // Global landmark poses with all observations. diff --git a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc index d6b5a56..3c60f55 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d_test.cc @@ -137,7 +137,7 @@ class PoseGraph2DTest : public ::testing::Test { auto options = CreatePoseGraphOptions(parameter_dictionary.get()); pose_graph_ = common::make_unique( options, - common::make_unique( + common::make_unique( options.optimization_problem_options()), &thread_pool_); } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index a19e446..ce9eaa9 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -41,7 +41,7 @@ namespace mapping { PoseGraph3D::PoseGraph3D( const proto::PoseGraphOptions& options, - std::unique_ptr optimization_problem, + std::unique_ptr optimization_problem, common::ThreadPool* thread_pool) : options_(options), optimization_problem_(std::move(optimization_problem)), @@ -267,7 +267,7 @@ void PoseGraph3D::ComputeConstraintsForNode( insertion_submaps.front()->local_pose().inverse() * local_pose; optimization_problem_->AddTrajectoryNode( matching_id.trajectory_id, - pose_graph::NodeSpec3D{constant_data->time, local_pose, global_pose}); + optimization::NodeSpec3D{constant_data->time, local_pose, global_pose}); for (size_t i = 0; i < insertion_submaps.size(); ++i) { const SubmapId submap_id = submap_ids[i]; // Even if this was the last node added to 'submap_id', the submap will only @@ -461,7 +461,7 @@ void PoseGraph3D::AddSubmapFromProto( submap_data_.at(submap_id).submap = submap_ptr; // Immediately show the submap at the 'global_submap_pose'. global_submap_poses_.Insert(submap_id, - pose_graph::SubmapSpec3D{global_submap_pose}); + optimization::SubmapSpec3D{global_submap_pose}); AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) { submap_data_.at(submap_id).state = SubmapState::kFinished; optimization_problem_->InsertSubmap(submap_id, global_submap_pose); @@ -483,8 +483,8 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose, const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; optimization_problem_->InsertTrajectoryNode( node_id, - pose_graph::NodeSpec3D{constant_data->time, constant_data->local_pose, - global_pose}); + optimization::NodeSpec3D{constant_data->time, constant_data->local_pose, + global_pose}); }); } @@ -778,7 +778,7 @@ PoseGraph3D::GetAllSubmapPoses() { } transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform( - const MapById& global_submap_poses, + const MapById& global_submap_poses, const int trajectory_id) const { auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index f0cba44..cf51dd9 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -34,7 +34,7 @@ #include "cartographer/common/time.h" #include "cartographer/mapping/3d/submap_3d.h" #include "cartographer/mapping/internal/3d/pose_graph/constraint_builder_3d.h" -#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph_trimmer.h" @@ -60,7 +60,7 @@ class PoseGraph3D : public PoseGraph { public: PoseGraph3D( const proto::PoseGraphOptions& options, - std::unique_ptr optimization_problem, + std::unique_ptr optimization_problem, common::ThreadPool* thread_pool); ~PoseGraph3D() override; @@ -200,7 +200,7 @@ class PoseGraph3D : public PoseGraph { // Computes the local to global map frame transform based on the given // 'global_submap_poses'. transform::Rigid3d ComputeLocalToGlobalTransform( - const MapById& global_submap_poses, + const MapById& global_submap_poses, int trajectory_id) const REQUIRES(mutex_); PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) @@ -243,7 +243,7 @@ class PoseGraph3D : public PoseGraph { void DispatchOptimization() REQUIRES(mutex_); // Current optimization problem. - std::unique_ptr optimization_problem_; + std::unique_ptr optimization_problem_; pose_graph::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_); std::vector constraints_ GUARDED_BY(mutex_); @@ -256,7 +256,7 @@ class PoseGraph3D : public PoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Global submap poses currently used for displaying data. - MapById global_submap_poses_ + MapById global_submap_poses_ GUARDED_BY(mutex_); // Global landmark poses with all observations. diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index a2ea75e..6f1b10b 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc @@ -28,8 +28,8 @@ namespace cartographer { namespace mapping { namespace { -using ::cartographer::mapping::pose_graph::OptimizationProblem3D; -using ::cartographer::mapping::pose_graph::proto::OptimizationProblemOptions; +using ::cartographer::mapping::optimization::OptimizationProblem3D; +using ::cartographer::mapping::optimization::proto::OptimizationProblemOptions; using ::cartographer::transform::Rigid3d; class MockOptimizationProblem3D : public OptimizationProblem3D { @@ -46,7 +46,7 @@ class PoseGraph3DForTesting : public PoseGraph3D { public: PoseGraph3DForTesting( const proto::PoseGraphOptions& options, - std::unique_ptr optimization_problem, + std::unique_ptr optimization_problem, common::ThreadPool* thread_pool) : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {} @@ -68,7 +68,7 @@ class PoseGraph3DTest : public ::testing::Test { void BuildPoseGraph() { auto optimization_problem = - common::make_unique( + common::make_unique( pose_graph_options_.optimization_problem_options()); pose_graph_ = common::make_unique( pose_graph_options_, std::move(optimization_problem), diff --git a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc index cc889f6..ea8892c 100644 --- a/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc +++ b/cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc @@ -26,7 +26,7 @@ #include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h" #include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h" -#include "cartographer/mapping/internal/pose_graph/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -76,7 +76,7 @@ void CeresScanMatcher3D::Match( transform::Rigid3d* const pose_estimate, ceres::Solver::Summary* const summary) { ceres::Problem problem; - pose_graph::CeresPose ceres_pose( + optimization::CeresPose ceres_pose( initial_pose_estimate, nullptr /* translation_parameterization */, options_.only_optimize_yaw() ? std::unique_ptr( diff --git a/cartographer/mapping/internal/pose_graph/ceres_pose.cc b/cartographer/mapping/internal/optimization/ceres_pose.cc similarity index 93% rename from cartographer/mapping/internal/pose_graph/ceres_pose.cc rename to cartographer/mapping/internal/optimization/ceres_pose.cc index 7063a47..4045fa5 100644 --- a/cartographer/mapping/internal/pose_graph/ceres_pose.cc +++ b/cartographer/mapping/internal/optimization/ceres_pose.cc @@ -14,11 +14,11 @@ * limitations under the License. */ -#include "cartographer/mapping/internal/pose_graph/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { CeresPose::CeresPose( const transform::Rigid3d& rigid, @@ -40,6 +40,6 @@ const transform::Rigid3d CeresPose::ToRigid() const { return transform::Rigid3d::FromArrays(data_->rotation, data_->translation); } -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/pose_graph/ceres_pose.h b/cartographer/mapping/internal/optimization/ceres_pose.h similarity index 86% rename from cartographer/mapping/internal/pose_graph/ceres_pose.h rename to cartographer/mapping/internal/optimization/ceres_pose.h index fdd6d2e..9e617b6 100644 --- a/cartographer/mapping/internal/pose_graph/ceres_pose.h +++ b/cartographer/mapping/internal/optimization/ceres_pose.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_ #include #include @@ -26,7 +26,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { class CeresPose { public: @@ -53,8 +53,8 @@ class CeresPose { std::shared_ptr data_; }; -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_CERES_POSE_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_CERES_POSE_H_ diff --git a/cartographer/mapping/internal/3d/acceleration_cost_function_3d.h b/cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h similarity index 93% rename from cartographer/mapping/internal/3d/acceleration_cost_function_3d.h rename to cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h index b90293d..20325f1 100644 --- a/cartographer/mapping/internal/3d/acceleration_cost_function_3d.h +++ b/cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_ #include "Eigen/Core" #include "Eigen/Geometry" @@ -101,4 +101,4 @@ class AccelerationCostFunction3D { } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_ACCELERATION_COST_FUNCTION_3D_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ACCELERATION_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers.h b/cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h similarity index 89% rename from cartographer/mapping/internal/pose_graph/cost_helpers.h rename to cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h index 9329d4a..8b54e29 100644 --- a/cartographer/mapping/internal/pose_graph/cost_helpers.h +++ b/cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_ #include "Eigen/Core" #include "Eigen/Geometry" @@ -24,7 +24,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { // Computes the error between the given relative pose and the difference of // poses 'start' and 'end' which are both in an arbitrary common frame. @@ -80,10 +80,10 @@ InterpolateNodes2D(const T* const prev_node_pose, const Eigen::Quaterniond& next_node_gravity_alignment, const double interpolation_parameter); -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer -#include "cartographer/mapping/internal/pose_graph/cost_helpers_impl.h" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h" -#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_H_ diff --git a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h b/cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h similarity index 96% rename from cartographer/mapping/internal/pose_graph/cost_helpers_impl.h rename to cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h index eee01cf..6f20889 100644 --- a/cartographer/mapping/internal/pose_graph/cost_helpers_impl.h +++ b/cartographer/mapping/internal/optimization/cost_functions/cost_helpers_impl.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_ #include "Eigen/Core" #include "cartographer/transform/rigid_transform.h" @@ -23,7 +23,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { template static std::array ComputeUnscaledError( @@ -190,8 +190,8 @@ InterpolateNodes2D(const T* const prev_node_pose, T(0)}}); } -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_COST_HELPERS_IMPL_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_COST_HELPERS_IMPL_H_ diff --git a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h similarity index 88% rename from cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h rename to cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h index 1c6a47f..d5e0719 100644 --- a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h +++ b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h @@ -14,13 +14,13 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_ #include "Eigen/Core" #include "Eigen/Geometry" -#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h" -#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -29,7 +29,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { // Cost function measuring the weighted error between the observed pose given by // the landmark measurement and the linearly interpolated pose of embedded in 3D @@ -91,8 +91,8 @@ class LandmarkCostFunction2D { const double interpolation_parameter_; }; -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_LANDMARK_COST_FUNCTION_2D_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_2D_H_ diff --git a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc similarity index 94% rename from cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc rename to cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc index 9a0e505..e5d4c2a 100644 --- a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h" #include @@ -24,7 +24,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { namespace { using ::testing::DoubleEq; @@ -72,6 +72,6 @@ TEST(LandmarkCostFunctionTest, SmokeTest) { } } // namespace -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h similarity index 87% rename from cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h rename to cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h index b871e48..4d297e1 100644 --- a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h +++ b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h @@ -14,13 +14,13 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_ #include "Eigen/Core" #include "Eigen/Geometry" -#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h" -#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -29,7 +29,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { // Cost function measuring the weighted error between the observed pose given by // the landmark measurement and the linearly interpolated pose. @@ -92,8 +92,8 @@ class LandmarkCostFunction3D { const double interpolation_parameter_; }; -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_3D_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_LANDMARK_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc similarity index 94% rename from cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc rename to cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc index a270d3b..a5c620d 100644 --- a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc +++ b/cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d_test.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h" #include @@ -24,7 +24,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { namespace { using ::testing::DoubleEq; @@ -72,6 +72,6 @@ TEST(LandmarkCostFunction3DTest, SmokeTest) { } } // namespace -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/3d/rotation_cost_function_3d.h b/cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h similarity index 75% rename from cartographer/mapping/internal/3d/rotation_cost_function_3d.h rename to cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h index cd61190..53390fc 100644 --- a/cartographer/mapping/internal/3d/rotation_cost_function_3d.h +++ b/cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_ #include "Eigen/Core" #include "Eigen/Geometry" @@ -25,15 +25,15 @@ namespace cartographer { namespace mapping { // Penalizes differences between IMU data and optimized orientations. -class RotationCostFunction { +class RotationCostFunction3D { public: static ceres::CostFunction* CreateAutoDiffCostFunction( const double scaling_factor, const Eigen::Quaterniond& delta_rotation_imu_frame) { return new ceres::AutoDiffCostFunction< - RotationCostFunction, 3 /* residuals */, 4 /* rotation variables */, + RotationCostFunction3D, 3 /* residuals */, 4 /* rotation variables */, 4 /* rotation variables */, 4 /* rotation variables */ - >(new RotationCostFunction(scaling_factor, delta_rotation_imu_frame)); + >(new RotationCostFunction3D(scaling_factor, delta_rotation_imu_frame)); } template @@ -56,13 +56,13 @@ class RotationCostFunction { } private: - RotationCostFunction(const double scaling_factor, - const Eigen::Quaterniond& delta_rotation_imu_frame) + RotationCostFunction3D(const double scaling_factor, + const Eigen::Quaterniond& delta_rotation_imu_frame) : scaling_factor_(scaling_factor), delta_rotation_imu_frame_(delta_rotation_imu_frame) {} - RotationCostFunction(const RotationCostFunction&) = delete; - RotationCostFunction& operator=(const RotationCostFunction&) = delete; + RotationCostFunction3D(const RotationCostFunction3D&) = delete; + RotationCostFunction3D& operator=(const RotationCostFunction3D&) = delete; const double scaling_factor_; const Eigen::Quaterniond delta_rotation_imu_frame_; @@ -71,4 +71,4 @@ class RotationCostFunction { } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_ROTATION_COST_FUNCTION_3D_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_ROTATION_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h similarity index 79% rename from cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h rename to cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h index 97296a2..9020b47 100644 --- a/cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h +++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h @@ -14,15 +14,15 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_ #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" -#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -31,7 +31,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { class SpaCostFunction2D { public: @@ -45,8 +45,8 @@ class SpaCostFunction2D { template bool operator()(const T* const c_i, const T* const c_j, T* e) const { - using pose_graph::ComputeUnscaledError; - using pose_graph::ScaleError; + using optimization::ComputeUnscaledError; + using optimization::ScaleError; const std::array error = ScaleError( ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j), @@ -62,8 +62,8 @@ class SpaCostFunction2D { const PoseGraph::Constraint::Pose pose_; }; -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_SPA_COST_FUNCTION_2D_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_ diff --git a/cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h similarity index 83% rename from cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h rename to cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h index d791bce..d4a400b 100644 --- a/cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h +++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h @@ -14,15 +14,15 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_ #include #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/common/math.h" -#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" #include "cartographer/mapping/pose_graph.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" @@ -31,7 +31,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { class SpaCostFunction3D { public: @@ -62,8 +62,8 @@ class SpaCostFunction3D { const PoseGraph::Constraint::Pose pose_; }; -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_SPA_COST_FUNCTION_3D_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_3D_H_ diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc similarity index 96% rename from cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc rename to cartographer/mapping/internal/optimization/optimization_problem_2d.cc index 954e813..8a91277 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" #include #include @@ -27,10 +27,9 @@ #include "cartographer/common/ceres_solver_options.h" #include "cartographer/common/histogram.h" #include "cartographer/common/math.h" -#include "cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h" -#include "cartographer/mapping/internal/2d/pose_graph/spa_cost_function_2d.h" -#include "cartographer/mapping/internal/pose_graph/ceres_pose.h" -#include "cartographer/mapping/internal/pose_graph/cost_helpers.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_2d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h" #include "cartographer/sensor/odometry_data.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -38,10 +37,10 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { namespace { -using ::cartographer::mapping::pose_graph::CeresPose; +using ::cartographer::mapping::optimization::CeresPose; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; // Converts a pose into the 3 optimization variable format used for Ceres: @@ -361,6 +360,6 @@ OptimizationProblem2D::CalculateOdometryBetweenNodes( return nullptr; } -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h b/cartographer/mapping/internal/optimization/optimization_problem_2d.h similarity index 88% rename from cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h rename to cartographer/mapping/internal/optimization/optimization_problem_2d.h index 5a56dd0..f5094d9 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_ #include #include @@ -28,7 +28,7 @@ #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/mapping/id.h" -#include "cartographer/mapping/internal/pose_graph/optimization_problem_interface.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_interface.h" #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/sensor/imu_data.h" @@ -38,7 +38,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { struct NodeSpec2D { common::Time time; @@ -56,7 +56,7 @@ class OptimizationProblem2D transform::Rigid2d> { public: explicit OptimizationProblem2D( - const pose_graph::proto::OptimizationProblemOptions& options); + const optimization::proto::OptimizationProblemOptions& options); ~OptimizationProblem2D(); OptimizationProblem2D(const OptimizationProblem2D&) = delete; @@ -108,7 +108,7 @@ class OptimizationProblem2D int trajectory_id, const NodeSpec2D& first_node_data, const NodeSpec2D& second_node_data) const; - pose_graph::proto::OptimizationProblemOptions options_; + optimization::proto::OptimizationProblemOptions options_; MapById node_data_; MapById submap_data_; std::map landmark_data_; @@ -116,8 +116,8 @@ class OptimizationProblem2D sensor::MapByTime odometry_data_; }; -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_OPTIMIZATION_PROBLEM_2D_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_2D_H_ diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc similarity index 97% rename from cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc rename to cartographer/mapping/internal/optimization/optimization_problem_3d.cc index baa9089..4bd40f9 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.cc @@ -14,7 +14,7 @@ * limitations under the License. */ -#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" #include #include @@ -30,13 +30,13 @@ #include "cartographer/common/make_unique.h" #include "cartographer/common/math.h" #include "cartographer/common/time.h" -#include "cartographer/mapping/internal/3d/acceleration_cost_function_3d.h" #include "cartographer/mapping/internal/3d/imu_integration.h" -#include "cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h" -#include "cartographer/mapping/internal/3d/pose_graph/spa_cost_function_3d.h" -#include "cartographer/mapping/internal/3d/rotation_cost_function_3d.h" #include "cartographer/mapping/internal/3d/rotation_parameterization.h" -#include "cartographer/mapping/internal/pose_graph/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/ceres_pose.h" +#include "cartographer/mapping/internal/optimization/cost_functions/acceleration_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/landmark_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/rotation_cost_function_3d.h" +#include "cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_3d.h" #include "cartographer/transform/timestamped_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" @@ -46,7 +46,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { namespace { using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; @@ -188,7 +188,7 @@ void AddLandmarkCostFunctions( } // namespace OptimizationProblem3D::OptimizationProblem3D( - const pose_graph::proto::OptimizationProblemOptions& options) + const optimization::proto::OptimizationProblemOptions& options) : options_(options) {} OptimizationProblem3D::~OptimizationProblem3D() {} @@ -425,7 +425,7 @@ void OptimizationProblem3D::Solve( trajectory_data.imu_calibration.data()); } problem.AddResidualBlock( - RotationCostFunction::CreateAutoDiffCostFunction( + RotationCostFunction3D::CreateAutoDiffCostFunction( options_.rotation_weight(), result.delta_rotation), nullptr /* loss function */, C_nodes.at(first_node_id).rotation(), C_nodes.at(second_node_id).rotation(), @@ -605,6 +605,6 @@ OptimizationProblem3D::CalculateOdometryBetweenNodes( return nullptr; } -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h b/cartographer/mapping/internal/optimization/optimization_problem_3d.h similarity index 90% rename from cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h rename to cartographer/mapping/internal/optimization/optimization_problem_3d.h index 6c545fa..db1a475 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_ #include #include @@ -28,7 +28,7 @@ #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/mapping/id.h" -#include "cartographer/mapping/internal/pose_graph/optimization_problem_interface.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_interface.h" #include "cartographer/mapping/pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/sensor/fixed_frame_pose_data.h" @@ -39,7 +39,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { struct NodeSpec3D { common::Time time; @@ -56,7 +56,7 @@ class OptimizationProblem3D transform::Rigid3d> { public: explicit OptimizationProblem3D( - const pose_graph::proto::OptimizationProblemOptions& options); + const optimization::proto::OptimizationProblemOptions& options); ~OptimizationProblem3D(); OptimizationProblem3D(const OptimizationProblem3D&) = delete; @@ -121,7 +121,7 @@ class OptimizationProblem3D int trajectory_id, const NodeSpec3D& first_node_data, const NodeSpec3D& second_node_data) const; - pose_graph::proto::OptimizationProblemOptions options_; + optimization::proto::OptimizationProblemOptions options_; MapById node_data_; MapById submap_data_; std::map landmark_data_; @@ -131,8 +131,8 @@ class OptimizationProblem3D std::map trajectory_data_; }; -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_POSE_GRAPH_OPTIMIZATION_PROBLEM_3D_H_ +#endif // CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_3D_H_ diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc similarity index 95% rename from cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc rename to cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc index 963fd55..facfb90 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_3d_test.cc @@ -14,21 +14,21 @@ * limitations under the License. */ -#include "cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" #include #include "Eigen/Core" #include "cartographer/common/lua_parameter_dictionary_test_helpers.h" #include "cartographer/common/time.h" -#include "cartographer/mapping/internal/pose_graph/optimization_problem_options.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_options.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" #include "gmock/gmock.h" namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { namespace { class OptimizationProblem3DTest : public ::testing::Test { @@ -36,7 +36,7 @@ class OptimizationProblem3DTest : public ::testing::Test { OptimizationProblem3DTest() : optimization_problem_(CreateOptions()), rng_(45387) {} - pose_graph::proto::OptimizationProblemOptions CreateOptions() { + optimization::proto::OptimizationProblemOptions CreateOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( return { acceleration_weight = 1e-4, @@ -55,7 +55,7 @@ class OptimizationProblem3DTest : public ::testing::Test { num_threads = 4, }, })text"); - return pose_graph::CreateOptimizationProblemOptions( + return optimization::CreateOptimizationProblemOptions( parameter_dictionary.get()); } @@ -191,6 +191,6 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) { } } // namespace -} // namespace pose_graph +} // namespace optimization } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/internal/pose_graph/optimization_problem_interface.h b/cartographer/mapping/internal/optimization/optimization_problem_interface.h similarity index 91% rename from cartographer/mapping/internal/pose_graph/optimization_problem_interface.h rename to cartographer/mapping/internal/optimization/optimization_problem_interface.h index 24f41af..2725ec2 100644 --- a/cartographer/mapping/internal/pose_graph/optimization_problem_interface.h +++ b/cartographer/mapping/internal/optimization/optimization_problem_interface.h @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_ -#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_ +#ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_OPTIMIZATION_PROBLEM_INTERFACE_H_ #include #include @@ -33,7 +33,7 @@ namespace cartographer { namespace mapping { -namespace pose_graph { +namespace optimization { // Implements the SPA loop closure method. template ( options_.pose_graph_options(), - common::make_unique( + common::make_unique( options_.pose_graph_options().optimization_problem_options()), &thread_pool_); } if (options.use_trajectory_builder_3d()) { pose_graph_ = common::make_unique( options_.pose_graph_options(), - common::make_unique( + common::make_unique( options_.pose_graph_options().optimization_problem_options()), &thread_pool_); } diff --git a/cartographer/mapping/pose_graph.cc b/cartographer/mapping/pose_graph.cc index e5b843b..8d8601b 100644 --- a/cartographer/mapping/pose_graph.cc +++ b/cartographer/mapping/pose_graph.cc @@ -16,8 +16,8 @@ #include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/internal/optimization/optimization_problem_options.h" #include "cartographer/mapping/internal/pose_graph/constraint_builder.h" -#include "cartographer/mapping/internal/pose_graph/optimization_problem_options.h" #include "cartographer/transform/transform.h" #include "glog/logging.h" @@ -82,7 +82,7 @@ proto::PoseGraphOptions CreatePoseGraphOptions( options.set_matcher_rotation_weight( parameter_dictionary->GetDouble("matcher_rotation_weight")); *options.mutable_optimization_problem_options() = - pose_graph::CreateOptimizationProblemOptions( + optimization::CreateOptimizationProblemOptions( parameter_dictionary->GetDictionary("optimization_problem").get()); options.set_max_num_final_iterations( parameter_dictionary->GetNonNegativeInt("max_num_final_iterations")); diff --git a/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto b/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto index f399735..9994adb 100644 --- a/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto +++ b/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto @@ -14,7 +14,7 @@ syntax = "proto3"; -package cartographer.mapping.pose_graph.proto; +package cartographer.mapping.optimization.proto; import "cartographer/common/proto/ceres_solver_options.proto"; diff --git a/cartographer/mapping/proto/pose_graph_options.proto b/cartographer/mapping/proto/pose_graph_options.proto index 31cdfc8..5de3a2a 100644 --- a/cartographer/mapping/proto/pose_graph_options.proto +++ b/cartographer/mapping/proto/pose_graph_options.proto @@ -37,7 +37,7 @@ message PoseGraphOptions { double matcher_rotation_weight = 8; // Options for the optimization problem. - mapping.pose_graph.proto.OptimizationProblemOptions + mapping.optimization.proto.OptimizationProblemOptions optimization_problem_options = 4; // Number of iterations to use in 'optimization_problem_options' for the final