From ed47f9d8f8a4336e8c70708a544d51a33252280b Mon Sep 17 00:00:00 2001 From: gaschler Date: Thu, 15 Mar 2018 11:12:26 +0100 Subject: [PATCH] OptimizationProblemInterface (#991) Derive optimization problems from a new OptimizationProblemInterface. Move fix_z into OptimizationProblemOptions. Move trivial getters to header files. This will allow mocking the optimization problems. --- .../2d/pose_graph/landmark_cost_function_2d.h | 9 +- .../landmark_cost_function_2d_test.cc | 4 +- .../2d/pose_graph/optimization_problem_2d.cc | 46 ++----- .../2d/pose_graph/optimization_problem_2d.h | 96 ++++++++------- .../mapping/internal/2d/pose_graph_2d.cc | 25 ++-- .../mapping/internal/2d/pose_graph_2d.h | 7 +- .../3d/pose_graph/landmark_cost_function_3d.h | 9 +- .../landmark_cost_function_3d_test.cc | 4 +- .../3d/pose_graph/optimization_problem_3d.cc | 64 ++-------- .../3d/pose_graph/optimization_problem_3d.h | 116 ++++++++++-------- .../optimization_problem_3d_test.cc | 9 +- .../mapping/internal/3d/pose_graph_3d.cc | 18 +-- .../mapping/internal/3d/pose_graph_3d.h | 7 +- .../optimization_problem_interface.h | 88 +++++++++++++ .../proto/optimization_problem_options.proto | 5 +- 15 files changed, 271 insertions(+), 236 deletions(-) create mode 100644 cartographer/mapping/internal/pose_graph/optimization_problem_interface.h diff --git a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h index 75f9345..8bcf0c6 100644 --- a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d.h @@ -40,9 +40,8 @@ class LandmarkCostFunction2D { PoseGraphInterface::LandmarkNode::LandmarkObservation; static ceres::CostFunction* CreateAutoDiffCostFunction( - const LandmarkObservation& observation, - const OptimizationProblem2D::NodeData& prev_node, - const OptimizationProblem2D::NodeData& next_node) { + const LandmarkObservation& observation, const NodeData2D& prev_node, + const NodeData2D& next_node) { return new ceres::AutoDiffCostFunction< LandmarkCostFunction2D, 6 /* residuals */, 3 /* previous node pose variables */, 3 /* next node pose variables */, @@ -72,8 +71,8 @@ class LandmarkCostFunction2D { private: LandmarkCostFunction2D(const LandmarkObservation& observation, - const OptimizationProblem2D::NodeData& prev_node, - const OptimizationProblem2D::NodeData& next_node) + const NodeData2D& prev_node, + const NodeData2D& next_node) : landmark_to_tracking_transform_( observation.landmark_to_tracking_transform), prev_node_gravity_alignment_(prev_node.gravity_alignment), diff --git a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc index c8de26c..b9b3706 100644 --- a/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc +++ b/cartographer/mapping/internal/2d/pose_graph/landmark_cost_function_2d_test.cc @@ -34,10 +34,10 @@ using LandmarkObservation = PoseGraphInterface::LandmarkNode::LandmarkObservation; TEST(LandmarkCostFunctionTest, SmokeTest) { - OptimizationProblem2D::NodeData prev_node; + NodeData2D prev_node; prev_node.time = common::FromUniversal(0); prev_node.gravity_alignment = Eigen::Quaterniond::Identity(); - OptimizationProblem2D::NodeData next_node; + NodeData2D next_node; next_node.time = common::FromUniversal(10); next_node.gravity_alignment = Eigen::Quaterniond::Identity(); diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc index e3b3402..575383a 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.cc @@ -42,8 +42,8 @@ namespace { using ::cartographer::mapping::pose_graph::CeresPose; using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; -using NodeData = OptimizationProblem2D::NodeData; -using SubmapData = OptimizationProblem2D::SubmapData; +using NodeData = NodeData2D; +using SubmapData = SubmapData2D; // Converts a pose into the 3 optimization variable format used for Ceres: // translation in x and y, followed by the rotation angle representing the @@ -149,20 +149,14 @@ void OptimizationProblem2D::AddOdometryData( odometry_data_.Append(trajectory_id, odometry_data); } -void OptimizationProblem2D::AddTrajectoryNode( - const int trajectory_id, const common::Time time, - const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose, - const Eigen::Quaterniond& gravity_alignment) { - node_data_.Append(trajectory_id, - NodeData{time, initial_pose, pose, gravity_alignment}); +void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id, + const NodeData& node_data) { + node_data_.Append(trajectory_id, node_data); } -void OptimizationProblem2D::InsertTrajectoryNode( - const NodeId& node_id, const common::Time time, - const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose, - const Eigen::Quaterniond& gravity_alignment) { - node_data_.Insert(node_id, - NodeData{time, initial_pose, pose, gravity_alignment}); +void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id, + const NodeData& node_data) { + node_data_.Insert(node_id, node_data); } void OptimizationProblem2D::TrimTrajectoryNode(const NodeId& node_id) { @@ -301,30 +295,6 @@ void OptimizationProblem2D::Solve( } } -const MapById& OptimizationProblem2D::node_data() const { - return node_data_; -} - -const MapById& OptimizationProblem2D::submap_data() - const { - return submap_data_; -} - -const std::map& -OptimizationProblem2D::landmark_data() const { - return landmark_data_; -} - -const sensor::MapByTime& OptimizationProblem2D::imu_data() - const { - return imu_data_; -} - -const sensor::MapByTime& -OptimizationProblem2D::odometry_data() const { - return odometry_data_; -} - std::unique_ptr OptimizationProblem2D::InterpolateOdometry( const int trajectory_id, const common::Time time) const { const auto it = odometry_data_.lower_bound(trajectory_id, time); diff --git a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h index ef3e484..48d8f85 100644 --- a/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph/optimization_problem_2d.h @@ -28,6 +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/pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/sensor/imu_data.h" @@ -39,23 +40,21 @@ namespace cartographer { namespace mapping { namespace pose_graph { -// Implements the SPA loop closure method. -class OptimizationProblem2D { +struct NodeData2D { + common::Time time; + transform::Rigid2d initial_pose; + transform::Rigid2d pose; + Eigen::Quaterniond gravity_alignment; +}; + +struct SubmapData2D { + transform::Rigid2d global_pose; +}; + +class OptimizationProblem2D + : public OptimizationProblemInterface { public: - using Constraint = PoseGraphInterface::Constraint; - using LandmarkNode = PoseGraphInterface::LandmarkNode; - - struct NodeData { - common::Time time; - transform::Rigid2d initial_pose; - transform::Rigid2d pose; - Eigen::Quaterniond gravity_alignment; - }; - - struct SubmapData { - transform::Rigid2d global_pose; - }; - explicit OptimizationProblem2D( const pose_graph::proto::OptimizationProblemOptions& options); ~OptimizationProblem2D(); @@ -63,48 +62,55 @@ class OptimizationProblem2D { OptimizationProblem2D(const OptimizationProblem2D&) = delete; OptimizationProblem2D& operator=(const OptimizationProblem2D&) = delete; - void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); + void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override; void AddOdometryData(int trajectory_id, - const sensor::OdometryData& odometry_data); - void AddTrajectoryNode(int trajectory_id, common::Time time, - const transform::Rigid2d& initial_pose, - const transform::Rigid2d& pose, - const Eigen::Quaterniond& gravity_alignment); - void InsertTrajectoryNode(const NodeId& node_id, common::Time time, - const transform::Rigid2d& initial_pose, - const transform::Rigid2d& pose, - const Eigen::Quaterniond& gravity_alignment); - void TrimTrajectoryNode(const NodeId& node_id); + const sensor::OdometryData& odometry_data) override; + void AddTrajectoryNode(int trajectory_id, + const NodeData2D& node_data) override; + void InsertTrajectoryNode(const NodeId& node_id, + const NodeData2D& node_data) override; + void TrimTrajectoryNode(const NodeId& node_id) override; void AddSubmap(int trajectory_id, - const transform::Rigid2d& global_submap_pose); + const transform::Rigid2d& global_submap_pose) override; void InsertSubmap(const SubmapId& submap_id, - const transform::Rigid2d& global_submap_pose); - void TrimSubmap(const SubmapId& submap_id); + const transform::Rigid2d& global_submap_pose) override; + void TrimSubmap(const SubmapId& submap_id) override; + void SetMaxNumIterations(int32 max_num_iterations) override; - void SetMaxNumIterations(int32 max_num_iterations); + void Solve( + const std::vector& constraints, + const std::set& frozen_trajectories, + const std::map& landmark_nodes) override; - // Optimizes the global poses. - void Solve(const std::vector& constraints, - const std::set& frozen_trajectories, - const std::map& landmark_nodes); - - const MapById& node_data() const; - const MapById& submap_data() const; - const std::map& landmark_data() const; - const sensor::MapByTime& imu_data() const; - const sensor::MapByTime& odometry_data() const; + const MapById& node_data() const override { + return node_data_; + } + const MapById& submap_data() const override { + return submap_data_; + } + const std::map& landmark_data() + const override { + return landmark_data_; + } + const sensor::MapByTime& imu_data() const override { + return imu_data_; + } + const sensor::MapByTime& odometry_data() + const override { + return odometry_data_; + } private: std::unique_ptr InterpolateOdometry( int trajectory_id, common::Time time) const; // Uses odometry if available, otherwise the local SLAM results. transform::Rigid3d ComputeRelativePose( - int trajectory_id, const NodeData& first_node_data, - const NodeData& second_node_data) const; + int trajectory_id, const NodeData2D& first_node_data, + const NodeData2D& second_node_data) const; pose_graph::proto::OptimizationProblemOptions options_; - MapById node_data_; - MapById submap_data_; + MapById node_data_; + MapById submap_data_; std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 61175b7..e413436 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -246,8 +246,9 @@ void PoseGraph2D::ComputeConstraintsForNode( pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() * pose; optimization_problem_.AddTrajectoryNode( - matching_id.trajectory_id, constant_data->time, pose, optimized_pose, - constant_data->gravity_alignment); + matching_id.trajectory_id, + pose_graph::NodeData2D{constant_data->time, pose, optimized_pose, + 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 @@ -440,9 +441,8 @@ void PoseGraph2D::AddSubmapFromProto( submap_data_.Insert(submap_id, SubmapData()); 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::OptimizationProblem2D::SubmapData{global_submap_pose_2d}); + global_submap_poses_.Insert(submap_id, + pose_graph::SubmapData2D{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); @@ -465,11 +465,13 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( constant_data->gravity_alignment.inverse()); optimization_problem_.InsertTrajectoryNode( - node_id, constant_data->time, - transform::Project2D(constant_data->local_pose * - gravity_alignment_inverse), - transform::Project2D(global_pose * gravity_alignment_inverse), - constant_data->gravity_alignment); + node_id, + pose_graph::NodeData2D{ + constant_data->time, + transform::Project2D(constant_data->local_pose * + gravity_alignment_inverse), + transform::Project2D(global_pose * gravity_alignment_inverse), + constant_data->gravity_alignment}); }); } @@ -728,8 +730,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 ec33e57..942a13f 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -191,8 +191,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_); PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id) @@ -244,8 +243,8 @@ class PoseGraph2D : public PoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Global submap poses currently used for displaying data. - MapById - global_submap_poses_ GUARDED_BY(mutex_); + MapById global_submap_poses_ + GUARDED_BY(mutex_); // Global landmark poses with all observations. std::map diff --git a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h index d2cca35..4072844 100644 --- a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d.h @@ -39,9 +39,8 @@ class LandmarkCostFunction3D { PoseGraphInterface::LandmarkNode::LandmarkObservation; static ceres::CostFunction* CreateAutoDiffCostFunction( - const LandmarkObservation& observation, - const OptimizationProblem3D::NodeData& prev_node, - const OptimizationProblem3D::NodeData& next_node) { + const LandmarkObservation& observation, const NodeData3D& prev_node, + const NodeData3D& next_node) { return new ceres::AutoDiffCostFunction< LandmarkCostFunction3D, 6 /* residuals */, 4 /* previous node rotation variables */, @@ -77,8 +76,8 @@ class LandmarkCostFunction3D { private: LandmarkCostFunction3D(const LandmarkObservation& observation, - const OptimizationProblem3D::NodeData& prev_node, - const OptimizationProblem3D::NodeData& next_node) + const NodeData3D& prev_node, + const NodeData3D& next_node) : landmark_to_tracking_transform_( observation.landmark_to_tracking_transform), translation_weight_(observation.translation_weight), diff --git a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc index 7ec671e..0f4048a 100644 --- a/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph/landmark_cost_function_3d_test.cc @@ -34,9 +34,9 @@ using LandmarkObservation = PoseGraphInterface::LandmarkNode::LandmarkObservation; TEST(LandmarkCostFunction3DTest, SmokeTest) { - OptimizationProblem3D::NodeData prev_node; + NodeData3D prev_node; prev_node.time = common::FromUniversal(0); - OptimizationProblem3D::NodeData next_node; + NodeData3D next_node; next_node.time = common::FromUniversal(10); std::unique_ptr cost_function( diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc index c33123e..a071fc7 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.cc @@ -52,8 +52,8 @@ namespace { using LandmarkNode = ::cartographer::mapping::PoseGraphInterface::LandmarkNode; using TrajectoryData = ::cartographer::mapping::PoseGraphInterface::TrajectoryData; -using NodeData = OptimizationProblem3D::NodeData; -using SubmapData = OptimizationProblem3D::SubmapData; +using NodeData = NodeData3D; +using SubmapData = SubmapData3D; // For odometry. std::unique_ptr Interpolate( @@ -179,8 +179,8 @@ void AddLandmarkCostFunctions( } // namespace OptimizationProblem3D::OptimizationProblem3D( - const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z) - : options_(options), fix_z_(fix_z) {} + const pose_graph::proto::OptimizationProblemOptions& options) + : options_(options) {} OptimizationProblem3D::~OptimizationProblem3D() {} @@ -200,11 +200,9 @@ void OptimizationProblem3D::AddFixedFramePoseData( fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data); } -void OptimizationProblem3D::AddTrajectoryNode( - const int trajectory_id, const common::Time time, - const transform::Rigid3d& local_pose, - const transform::Rigid3d& global_pose) { - node_data_.Append(trajectory_id, NodeData{time, local_pose, global_pose}); +void OptimizationProblem3D::AddTrajectoryNode(const int trajectory_id, + const NodeData& node_data) { + node_data_.Append(trajectory_id, node_data); trajectory_data_[trajectory_id]; } @@ -213,11 +211,9 @@ void OptimizationProblem3D::SetTrajectoryData( trajectory_data_[trajectory_id] = trajectory_data; } -void OptimizationProblem3D::InsertTrajectoryNode( - const NodeId& node_id, const common::Time time, - const transform::Rigid3d& local_pose, - const transform::Rigid3d& global_pose) { - node_data_.Insert(node_id, NodeData{time, local_pose, global_pose}); +void OptimizationProblem3D::InsertTrajectoryNode(const NodeId& node_id, + const NodeData& node_data) { + node_data_.Insert(node_id, node_data); trajectory_data_[node_id.trajectory_id]; } @@ -265,7 +261,7 @@ void OptimizationProblem3D::Solve( const auto translation_parameterization = [this]() -> std::unique_ptr { - return fix_z_ == FixZ::kYes + return options_.fix_z_in_3d() ? common::make_unique( 3, std::vector{2}) : nullptr; @@ -342,7 +338,7 @@ void OptimizationProblem3D::Solve( &C_nodes, &C_landmarks, &problem); // Add constraints based on IMU observations of angular velocities and // linear acceleration. - if (fix_z_ == FixZ::kNo) { + if (!options_.fix_z_in_3d()) { for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { const int trajectory_id = node_it->id.trajectory_id; const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); @@ -429,7 +425,7 @@ void OptimizationProblem3D::Solve( } } - if (fix_z_ == FixZ::kYes) { + if (options_.fix_z_in_3d()) { // 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();) { @@ -565,40 +561,6 @@ void OptimizationProblem3D::Solve( } } -const MapById& OptimizationProblem3D::node_data() const { - return node_data_; -} - -const MapById& OptimizationProblem3D::submap_data() - const { - return submap_data_; -} - -const std::map& -OptimizationProblem3D::landmark_data() const { - return landmark_data_; -} - -const sensor::MapByTime& OptimizationProblem3D::imu_data() - const { - return imu_data_; -} - -const sensor::MapByTime& -OptimizationProblem3D::odometry_data() const { - return odometry_data_; -} - -const sensor::MapByTime& -OptimizationProblem3D::fixed_frame_pose_data() const { - return fixed_frame_pose_data_; -} - -const std::map& OptimizationProblem3D::trajectory_data() - const { - return trajectory_data_; -} - transform::Rigid3d OptimizationProblem3D::ComputeRelativePose( const int trajectory_id, const NodeData& first_node_data, const NodeData& second_node_data) const { diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h index e3c487a..e36f666 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d.h @@ -28,6 +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/pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping/pose_graph_interface.h" #include "cartographer/sensor/fixed_frame_pose_data.h" @@ -40,80 +41,89 @@ namespace cartographer { namespace mapping { namespace pose_graph { -// Implements the SPA loop closure method. -class OptimizationProblem3D { +struct NodeData3D { + common::Time time; + transform::Rigid3d local_pose; + transform::Rigid3d global_pose; +}; + +struct SubmapData3D { + transform::Rigid3d global_pose; +}; + +class OptimizationProblem3D + : public OptimizationProblemInterface { public: - using Constraint = PoseGraphInterface::Constraint; - using LandmarkNode = PoseGraphInterface::LandmarkNode; - - struct NodeData { - common::Time time; - transform::Rigid3d local_pose; - transform::Rigid3d global_pose; - }; - - struct SubmapData { - transform::Rigid3d global_pose; - }; - - enum class FixZ { kYes, kNo }; - - OptimizationProblem3D( - const pose_graph::proto::OptimizationProblemOptions& options, FixZ fix_z); + explicit OptimizationProblem3D( + const pose_graph::proto::OptimizationProblemOptions& options); ~OptimizationProblem3D(); OptimizationProblem3D(const OptimizationProblem3D&) = delete; OptimizationProblem3D& operator=(const OptimizationProblem3D&) = delete; - void AddImuData(int trajectory_id, const sensor::ImuData& imu_data); + void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override; void AddOdometryData(int trajectory_id, - const sensor::OdometryData& odometry_data); + const sensor::OdometryData& odometry_data) override; + void AddTrajectoryNode(int trajectory_id, + const NodeData3D& node_data) override; + void InsertTrajectoryNode(const NodeId& node_id, + const NodeData3D& node_data) override; + void TrimTrajectoryNode(const NodeId& node_id) override; + void AddSubmap(int trajectory_id, + const transform::Rigid3d& global_submap_pose) override; + void InsertSubmap(const SubmapId& submap_id, + const transform::Rigid3d& global_submap_pose) override; + void TrimSubmap(const SubmapId& submap_id) override; + void SetMaxNumIterations(int32 max_num_iterations) override; + + void Solve( + const std::vector& constraints, + const std::set& frozen_trajectories, + const std::map& landmark_nodes) override; + + const MapById& node_data() const override { + return node_data_; + } + const MapById& submap_data() const override { + return submap_data_; + } + const std::map& landmark_data() + const override { + return landmark_data_; + } + const sensor::MapByTime& imu_data() const override { + return imu_data_; + } + const sensor::MapByTime& odometry_data() + const override { + return odometry_data_; + } + void AddFixedFramePoseData( int trajectory_id, const sensor::FixedFramePoseData& fixed_frame_pose_data); - void AddTrajectoryNode(int trajectory_id, common::Time time, - const transform::Rigid3d& local_pose, - const transform::Rigid3d& global_pose); void SetTrajectoryData( int trajectory_id, const PoseGraphInterface::TrajectoryData& trajectory_data); - void InsertTrajectoryNode(const NodeId& node_id, common::Time time, - const transform::Rigid3d& local_pose, - const transform::Rigid3d& global_pose); - void TrimTrajectoryNode(const NodeId& node_id); - void AddSubmap(int trajectory_id, - const transform::Rigid3d& global_submap_pose); - void InsertSubmap(const SubmapId& submap_id, - const transform::Rigid3d& global_submap_pose); - void TrimSubmap(const SubmapId& submap_id); - - void SetMaxNumIterations(int32 max_num_iterations); - - // Optimizes the global poses. - void Solve(const std::vector& constraints, - const std::set& frozen_trajectories, - const std::map& landmark_nodes); - - const MapById& node_data() const; - const MapById& submap_data() const; - const std::map& landmark_data() const; - const sensor::MapByTime& imu_data() const; - const sensor::MapByTime& odometry_data() const; const sensor::MapByTime& fixed_frame_pose_data() - const; + const { + return fixed_frame_pose_data_; + } const std::map& trajectory_data() - const; + const { + return trajectory_data_; + } private: // Uses odometry if available, otherwise the local SLAM results. transform::Rigid3d ComputeRelativePose( - int trajectory_id, const NodeData& first_node_data, - const NodeData& second_node_data) const; + int trajectory_id, const NodeData3D& first_node_data, + const NodeData3D& second_node_data) const; pose_graph::proto::OptimizationProblemOptions options_; - FixZ fix_z_; - MapById node_data_; - MapById submap_data_; + MapById node_data_; + MapById submap_data_; std::map landmark_data_; sensor::MapByTime imu_data_; sensor::MapByTime odometry_data_; diff --git a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc index deeb9d1..7c96f2b 100644 --- a/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph/optimization_problem_3d_test.cc @@ -34,9 +34,7 @@ namespace { class OptimizationProblem3DTest : public ::testing::Test { protected: OptimizationProblem3DTest() - : optimization_problem_(CreateOptions(), - OptimizationProblem3D::FixZ::kNo), - rng_(45387) {} + : optimization_problem_(CreateOptions()), rng_(45387) {} pose_graph::proto::OptimizationProblemOptions CreateOptions() { auto parameter_dictionary = common::MakeDictionary(R"text( @@ -127,7 +125,8 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) { optimization_problem_.AddImuData( kTrajectoryId, sensor::ImuData{now, Eigen::Vector3d::UnitZ() * 9.81, Eigen::Vector3d::Zero()}); - optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose, pose); + optimization_problem_.AddTrajectoryNode(kTrajectoryId, + NodeData3D{now, pose, pose}); now += common::FromSeconds(0.01); } @@ -170,7 +169,7 @@ TEST_F(OptimizationProblem3DTest, ReducesNoise) { optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap0Transform); optimization_problem_.AddSubmap(kTrajectoryId, kSubmap2Transform); - const std::set kFrozen; + const std::set kFrozen = {}; optimization_problem_.Solve(constraints, kFrozen, {}); double translation_error_after = 0.; diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 78abc2a..4c51b94 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -42,8 +42,7 @@ namespace mapping { PoseGraph3D::PoseGraph3D(const proto::PoseGraphOptions& options, common::ThreadPool* thread_pool) : options_(options), - optimization_problem_(options_.optimization_problem_options(), - pose_graph::OptimizationProblem3D::FixZ::kNo), + optimization_problem_(options_.optimization_problem_options()), constraint_builder_(options_.constraint_builder_options(), thread_pool) {} PoseGraph3D::~PoseGraph3D() { @@ -264,7 +263,8 @@ void PoseGraph3D::ComputeConstraintsForNode( optimization_problem_.submap_data().at(matching_id).global_pose * insertion_submaps.front()->local_pose().inverse() * local_pose; optimization_problem_.AddTrajectoryNode( - matching_id.trajectory_id, constant_data->time, local_pose, global_pose); + matching_id.trajectory_id, + pose_graph::NodeData3D{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 @@ -456,9 +456,8 @@ void PoseGraph3D::AddSubmapFromProto( submap_data_.Insert(submap_id, SubmapData()); 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::OptimizationProblem3D::SubmapData{global_submap_pose}); + global_submap_poses_.Insert(submap_id, + pose_graph::SubmapData3D{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); @@ -479,7 +478,9 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose, AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; optimization_problem_.InsertTrajectoryNode( - node_id, constant_data->time, constant_data->local_pose, global_pose); + node_id, + pose_graph::NodeData3D{constant_data->time, constant_data->local_pose, + global_pose}); }); } @@ -762,8 +763,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 f3e2dcb..a3dac98 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -191,8 +191,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) @@ -248,8 +247,8 @@ class PoseGraph3D : public PoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Global submap poses currently used for displaying data. - MapById - global_submap_poses_ GUARDED_BY(mutex_); + MapById global_submap_poses_ + GUARDED_BY(mutex_); // Global landmark poses with all observations. std::map diff --git a/cartographer/mapping/internal/pose_graph/optimization_problem_interface.h b/cartographer/mapping/internal/pose_graph/optimization_problem_interface.h new file mode 100644 index 0000000..24f41af --- /dev/null +++ b/cartographer/mapping/internal/pose_graph/optimization_problem_interface.h @@ -0,0 +1,88 @@ +/* + * 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_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_ +#define CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_ + +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/time.h" +#include "cartographer/mapping/id.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" +#include "cartographer/sensor/odometry_data.h" + +namespace cartographer { +namespace mapping { +namespace pose_graph { + +// Implements the SPA loop closure method. +template +class OptimizationProblemInterface { + public: + using Constraint = PoseGraphInterface::Constraint; + using LandmarkNode = PoseGraphInterface::LandmarkNode; + + OptimizationProblemInterface() {} + virtual ~OptimizationProblemInterface() {} + + OptimizationProblemInterface(const OptimizationProblemInterface&) = delete; + OptimizationProblemInterface& operator=(const OptimizationProblemInterface&) = + delete; + + virtual void AddImuData(int trajectory_id, + const sensor::ImuData& imu_data) = 0; + virtual void AddOdometryData(int trajectory_id, + const sensor::OdometryData& odometry_data) = 0; + virtual void AddTrajectoryNode(int trajectory_id, + const NodeDataType& node_data) = 0; + virtual void InsertTrajectoryNode(const NodeId& node_id, + const NodeDataType& node_data) = 0; + virtual void TrimTrajectoryNode(const NodeId& node_id) = 0; + virtual void AddSubmap(int trajectory_id, + const RigidTransformType& global_submap_pose) = 0; + virtual void InsertSubmap(const SubmapId& submap_id, + const RigidTransformType& global_submap_pose) = 0; + virtual void TrimSubmap(const SubmapId& submap_id) = 0; + virtual void SetMaxNumIterations(int32 max_num_iterations) = 0; + + // Optimizes the global poses. + virtual void Solve( + const std::vector& constraints, + const std::set& frozen_trajectories, + const std::map& landmark_nodes) = 0; + + virtual const MapById& node_data() const = 0; + virtual const MapById& submap_data() const = 0; + virtual const std::map& landmark_data() + const = 0; + virtual const sensor::MapByTime& imu_data() const = 0; + virtual const sensor::MapByTime& odometry_data() + const = 0; +}; + +} // namespace pose_graph +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_INTERNAL_POSE_GRAPH_OPTIMIZATION_PROBLEM_INTERFACE_H_ diff --git a/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto b/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto index dce9050..9827465 100644 --- a/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto +++ b/cartographer/mapping/pose_graph/proto/optimization_problem_options.proto @@ -18,7 +18,7 @@ package cartographer.mapping.pose_graph.proto; import "cartographer/common/proto/ceres_solver_options.proto"; -// NEXT ID: 13 +// NEXT ID: 14 message OptimizationProblemOptions { // Scaling parameter for Huber loss function. double huber_scale = 1; @@ -41,6 +41,9 @@ message OptimizationProblemOptions { // Scaling parameter for the FixedFramePose rotation. double fixed_frame_pose_rotation_weight = 12; + // 3D only: fix Z. + bool fix_z_in_3d = 13; + // If true, the Ceres solver summary will be logged for every optimization. bool log_solver_summary = 5;