From eed51b8bb6d8618c7b66fb6bff3ad97c57c973c8 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Thu, 1 Feb 2018 20:59:22 +0100 Subject: [PATCH] Cleaning up dependencies for constraint builder & optimization problem. (#877) --- cartographer/mapping_2d/pose_graph/constraint_builder.h | 4 ++-- .../mapping_2d/pose_graph/optimization_problem.cc | 3 +-- .../mapping_2d/pose_graph/optimization_problem.h | 4 ++-- cartographer/mapping_3d/pose_graph/constraint_builder.cc | 9 ++++----- cartographer/mapping_3d/pose_graph/constraint_builder.h | 5 +++-- .../mapping_3d/pose_graph/optimization_problem.cc | 6 ++---- .../mapping_3d/pose_graph/optimization_problem.h | 4 ++-- 7 files changed, 16 insertions(+), 19 deletions(-) diff --git a/cartographer/mapping_2d/pose_graph/constraint_builder.h b/cartographer/mapping_2d/pose_graph/constraint_builder.h index ab9096f..8a65816 100644 --- a/cartographer/mapping_2d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/pose_graph/constraint_builder.h @@ -30,8 +30,8 @@ #include "cartographer/common/math.h" #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" -#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h" +#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_2d/submaps.h" @@ -58,7 +58,7 @@ transform::Rigid2d ComputeSubmapPose(const Submap& submap); // This class is thread-safe. class ConstraintBuilder { public: - using Constraint = mapping::PoseGraph::Constraint; + using Constraint = mapping::PoseGraphInterface::Constraint; using Result = std::vector; ConstraintBuilder( diff --git a/cartographer/mapping_2d/pose_graph/optimization_problem.cc b/cartographer/mapping_2d/pose_graph/optimization_problem.cc index af2b636..5833ae2 100644 --- a/cartographer/mapping_2d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/pose_graph/optimization_problem.cc @@ -160,7 +160,6 @@ void OptimizationProblem::Solve(const std::vector& constraints, C_submaps.at(constraint.submap_id).data(), C_nodes.at(constraint.node_id).data()); } - // Add penalties for violating odometry or changes between consecutive nodes // if odometry is not available. for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { @@ -211,7 +210,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, for (const auto& C_node_id_data : C_nodes) { node_data_.at(C_node_id_data.id).pose = ToPose(C_node_id_data.data); } -} +} // namespace pose_graph const mapping::MapById& OptimizationProblem::node_data() const { diff --git a/cartographer/mapping_2d/pose_graph/optimization_problem.h b/cartographer/mapping_2d/pose_graph/optimization_problem.h index d478a91..3fdbb66 100644 --- a/cartographer/mapping_2d/pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/pose_graph/optimization_problem.h @@ -28,8 +28,8 @@ #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/mapping/id.h" -#include "cartographer/mapping/pose_graph.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" #include "cartographer/sensor/map_by_time.h" #include "cartographer/sensor/odometry_data.h" @@ -53,7 +53,7 @@ struct SubmapData { // Implements the SPA loop closure method. class OptimizationProblem { public: - using Constraint = mapping::PoseGraph::Constraint; + using Constraint = mapping::PoseGraphInterface::Constraint; explicit OptimizationProblem( const mapping::pose_graph::proto::OptimizationProblemOptions& options); diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.cc b/cartographer/mapping_3d/pose_graph/constraint_builder.cc index c3ac1a4..49f5099 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/pose_graph/constraint_builder.cc @@ -172,7 +172,7 @@ void ConstraintBuilder::ComputeConstraint( const mapping::TrajectoryNode::Data* const constant_data, const transform::Rigid3d& global_node_pose, const transform::Rigid3d& global_submap_pose, - std::unique_ptr* constraint) { + std::unique_ptr* constraint) { const SubmapScanMatcher* const submap_scan_matcher = GetSubmapScanMatcher(submap_id); @@ -229,12 +229,12 @@ void ConstraintBuilder::ComputeConstraint( submap_scan_matcher->low_resolution_hybrid_grid}}, &constraint_transform, &unused_summary); - constraint->reset(new OptimizationProblem::Constraint{ + constraint->reset(new Constraint{ submap_id, node_id, {constraint_transform, options_.loop_closure_translation_weight(), options_.loop_closure_rotation_weight()}, - OptimizationProblem::Constraint::INTER_SUBMAP}); + Constraint::INTER_SUBMAP}); if (options_.log_matches()) { std::ostringstream info; @@ -270,8 +270,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) { if (pending_computations_.empty()) { CHECK_EQ(submap_queued_work_items_.size(), 0); if (when_done_ != nullptr) { - for (const std::unique_ptr& - constraint : constraints_) { + for (const std::unique_ptr& constraint : constraints_) { if (constraint != nullptr) { result.push_back(*constraint); } diff --git a/cartographer/mapping_3d/pose_graph/constraint_builder.h b/cartographer/mapping_3d/pose_graph/constraint_builder.h index e0d2e7d..43bf519 100644 --- a/cartographer/mapping_3d/pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/pose_graph/constraint_builder.h @@ -31,8 +31,9 @@ #include "cartographer/common/math.h" #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" +#include "cartographer/mapping/pose_graph/proto/constraint_builder_options.pb.h" +#include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/mapping/trajectory_node.h" -#include "cartographer/mapping_3d/pose_graph/optimization_problem.h" #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/mapping_3d/submaps.h" @@ -54,7 +55,7 @@ namespace pose_graph { // This class is thread-safe. class ConstraintBuilder { public: - using Constraint = mapping::PoseGraph::Constraint; + using Constraint = mapping::PoseGraphInterface::Constraint; using Result = std::vector; ConstraintBuilder( diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.cc b/cartographer/mapping_3d/pose_graph/optimization_problem.cc index 6316c5b..21378f0 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.cc @@ -251,7 +251,6 @@ void OptimizationProblem::Solve(const std::vector& constraints, C_nodes.at(constraint.node_id).rotation(), C_nodes.at(constraint.node_id).translation()); } - // Add constraints based on IMU observations of angular velocities and // linear acceleration. if (fix_z_ == FixZ::kNo) { @@ -400,7 +399,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, continue; } - const mapping::PoseGraph::Constraint::Pose constraint_pose{ + const Constraint::Pose constraint_pose{ *fixed_frame_pose, options_.fixed_frame_pose_translation_weight(), options_.fixed_frame_pose_rotation_weight()}; @@ -435,7 +434,6 @@ void OptimizationProblem::Solve(const std::vector& constraints, C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation()); } } - // Solve. ceres::Solver::Summary summary; ceres::Solve( @@ -472,7 +470,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, trajectory_data_.at(C_fixed_frame.first).fixed_frame = C_fixed_frame.second.ToRigid(); } -} +} // namespace pose_graph const mapping::MapById& OptimizationProblem::node_data() const { diff --git a/cartographer/mapping_3d/pose_graph/optimization_problem.h b/cartographer/mapping_3d/pose_graph/optimization_problem.h index 863274f..7a02621 100644 --- a/cartographer/mapping_3d/pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/pose_graph/optimization_problem.h @@ -28,8 +28,8 @@ #include "cartographer/common/port.h" #include "cartographer/common/time.h" #include "cartographer/mapping/id.h" -#include "cartographer/mapping/pose_graph.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" #include "cartographer/sensor/imu_data.h" #include "cartographer/sensor/map_by_time.h" @@ -53,7 +53,7 @@ struct SubmapData { // Implements the SPA loop closure method. class OptimizationProblem { public: - using Constraint = mapping::PoseGraph::Constraint; + using Constraint = mapping::PoseGraphInterface::Constraint; enum class FixZ { kYes, kNo };