From 660eb341ff00c5905513fecff5358e7f19fdda96 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Wed, 10 May 2017 17:12:38 +0200 Subject: [PATCH] Optimization problem only deals with trajectory IDs now. (#273) Related to #256. PAIR=wohe --- cartographer/mapping_2d/sparse_pose_graph.cc | 8 +++--- .../sparse_pose_graph/optimization_problem.cc | 27 ++++++++++--------- .../sparse_pose_graph/optimization_problem.h | 8 +++--- cartographer/mapping_3d/sparse_pose_graph.cc | 10 ++++--- .../sparse_pose_graph/optimization_problem.cc | 24 ++++++++++------- .../sparse_pose_graph/optimization_problem.h | 9 +++---- .../optimization_problem_test.cc | 5 ++-- 7 files changed, 50 insertions(+), 41 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 47dd5ea..c731cb1 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -153,9 +153,10 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { common::MutexLocker locker(&mutex_); + trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); AddWorkItem([=]() REQUIRES(mutex_) { - optimization_problem_.AddImuData(trajectory, time, linear_acceleration, - angular_velocity); + optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time, + linear_acceleration, angular_velocity); }); } @@ -228,7 +229,8 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_[scan_index].constant_data; optimization_problem_.AddTrajectoryNode( - scan_data->trajectory, scan_data->time, pose, optimized_pose); + trajectory_ids_.at(scan_data->trajectory), scan_data->time, pose, + optimized_pose); for (const mapping::Submap* submap : insertion_submaps) { const int submap_index = GetSubmapIndex(submap); CHECK(!submap_states_[submap_index].finished); diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 7bb8cbc..b4578b7 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -60,20 +60,23 @@ OptimizationProblem::OptimizationProblem( OptimizationProblem::~OptimizationProblem() {} -void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory, +void OptimizationProblem::AddImuData(const int trajectory_id, const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { - imu_data_[trajectory].push_back( + CHECK_GE(trajectory_id, 0); + imu_data_.resize( + std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); + imu_data_[trajectory_id].push_back( mapping_3d::ImuData{time, linear_acceleration, angular_velocity}); } void OptimizationProblem::AddTrajectoryNode( - const mapping::Submaps* const trajectory, const common::Time time, + const int trajectory_id, const common::Time time, const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& point_cloud_pose) { - node_data_.push_back( - NodeData{trajectory, time, initial_point_cloud_pose, point_cloud_pose}); + node_data_.push_back(NodeData{trajectory_id, time, initial_point_cloud_pose, + point_cloud_pose}); } void OptimizationProblem::AddSubmap(const int trajectory_id, @@ -150,15 +153,15 @@ void OptimizationProblem::Solve(const std::vector& constraints) { // The poses in 'node_data_' are interleaved from multiple trajectories // (although the points from a given trajectory are in time order). - // 'last_pose_indices[trajectory]' is the index of the most-recent pose on - // 'trajectory'. - std::map last_pose_indices; + // 'last_pose_indices[trajectory_id]' is the index of the most-recent pose on + // 'trajectory_id'. + std::map last_pose_indices; for (size_t j = 0; j != node_data_.size(); ++j) { - const mapping::Submaps* trajectory = node_data_[j].trajectory; + const int trajectory_id = node_data_[j].trajectory_id; // This pose has a predecessor. - if (last_pose_indices.count(trajectory) != 0) { - const int last_pose_index = last_pose_indices[trajectory]; + if (last_pose_indices.count(trajectory_id) != 0) { + const int last_pose_index = last_pose_indices[trajectory_id]; constexpr double kUnusedPositionPenalty = 1.; constexpr double kUnusedOrientationPenalty = 1.; problem.AddResidualBlock( @@ -173,7 +176,7 @@ void OptimizationProblem::Solve(const std::vector& constraints) { nullptr /* loss function */, C_point_clouds[last_pose_index].data(), C_point_clouds[j].data()); } - last_pose_indices[trajectory] = j; + last_pose_indices[trajectory_id] = j; } // Solve. diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index 0ca0766..cca2e2f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -36,7 +36,7 @@ namespace sparse_pose_graph { struct NodeData { // TODO(whess): Keep nodes per trajectory instead. - const mapping::Submaps* trajectory; + int trajectory_id; common::Time time; transform::Rigid2d initial_point_cloud_pose; transform::Rigid2d point_cloud_pose; @@ -59,10 +59,10 @@ class OptimizationProblem { OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem& operator=(const OptimizationProblem&) = delete; - void AddImuData(const mapping::Submaps* trajectory, common::Time time, + void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); - void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time, + void AddTrajectoryNode(int trajectory_id, common::Time time, const transform::Rigid2d& initial_point_cloud_pose, const transform::Rigid2d& point_cloud_pose); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); @@ -77,7 +77,7 @@ class OptimizationProblem { private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; - std::map> imu_data_; + std::vector> imu_data_; std::vector node_data_; std::vector> submap_data_; }; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 2fadc45..b8eebfb 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -153,9 +153,10 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { common::MutexLocker locker(&mutex_); + trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); AddWorkItem([=]() REQUIRES(mutex_) { - optimization_problem_.AddImuData(trajectory, time, linear_acceleration, - angular_velocity); + optimization_problem_.AddImuData(trajectory_ids_.at(trajectory), time, + linear_acceleration, angular_velocity); }); } @@ -225,8 +226,9 @@ void SparsePoseGraph::ComputeConstraintsForScan( CHECK_EQ(scan_index, optimization_problem_.node_data().size()); const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_[scan_index].constant_data; - optimization_problem_.AddTrajectoryNode(scan_data->trajectory, - scan_data->time, optimized_pose); + optimization_problem_.AddTrajectoryNode( + trajectory_ids_.at(scan_data->trajectory), scan_data->time, + optimized_pose); for (const Submap* submap : insertion_submaps) { const int submap_index = GetSubmapIndex(submap); CHECK(!submap_states_[submap_index].finished); diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index c2ee175..6e6263a 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -78,18 +78,21 @@ OptimizationProblem::OptimizationProblem( OptimizationProblem::~OptimizationProblem() {} -void OptimizationProblem::AddImuData(const mapping::Submaps* const trajectory, +void OptimizationProblem::AddImuData(const int trajectory_id, const common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity) { - imu_data_[trajectory].push_back( + CHECK_GE(trajectory_id, 0); + imu_data_.resize( + std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); + imu_data_[trajectory_id].push_back( ImuData{time, linear_acceleration, angular_velocity}); } void OptimizationProblem::AddTrajectoryNode( - const mapping::Submaps* const trajectory, const common::Time time, + const int trajectory_id, const common::Time time, const transform::Rigid3d& point_cloud_pose) { - node_data_.push_back(NodeData{trajectory, time, point_cloud_pose}); + node_data_.push_back(NodeData{trajectory_id, time, point_cloud_pose}); } void OptimizationProblem::AddSubmap(const int trajectory_id, @@ -112,9 +115,9 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } // Compute the indices of consecutive nodes for each trajectory. - std::map> nodes_per_trajectory; + std::vector> nodes_per_trajectory(submap_data_.size()); for (size_t j = 0; j != node_data_.size(); ++j) { - nodes_per_trajectory[node_data_[j].trajectory].push_back(j); + nodes_per_trajectory.at(node_data_[j].trajectory_id).push_back(j); } ceres::Problem::Options problem_options; @@ -184,10 +187,11 @@ void OptimizationProblem::Solve(const std::vector& constraints) { // Add constraints based on IMU observations of angular velocities and // linear acceleration. - for (const auto& trajectory_nodes_pair : nodes_per_trajectory) { - const mapping::Submaps* const trajectory = trajectory_nodes_pair.first; - const std::vector& node_indices = trajectory_nodes_pair.second; - const std::deque& imu_data = imu_data_.at(trajectory); + for (size_t trajectory_id = 0; trajectory_id != nodes_per_trajectory.size(); + ++trajectory_id) { + const std::vector& node_indices = + nodes_per_trajectory[trajectory_id]; + const std::deque& imu_data = imu_data_.at(trajectory_id); CHECK(!node_indices.empty()); CHECK(!imu_data.empty()); diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 52d578c..8f316ea 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -29,7 +29,6 @@ #include "cartographer/mapping/sparse_pose_graph.h" #include "cartographer/mapping/sparse_pose_graph/proto/optimization_problem_options.pb.h" #include "cartographer/mapping_3d/imu_integration.h" -#include "cartographer/mapping_3d/submaps.h" namespace cartographer { namespace mapping_3d { @@ -37,7 +36,7 @@ namespace sparse_pose_graph { struct NodeData { // TODO(whess): Keep nodes per trajectory instead. - const mapping::Submaps* trajectory; + int trajectory_id; common::Time time; transform::Rigid3d point_cloud_pose; }; @@ -62,10 +61,10 @@ class OptimizationProblem { OptimizationProblem(const OptimizationProblem&) = delete; OptimizationProblem& operator=(const OptimizationProblem&) = delete; - void AddImuData(const mapping::Submaps* trajectory, common::Time time, + void AddImuData(int trajectory_id, common::Time time, const Eigen::Vector3d& linear_acceleration, const Eigen::Vector3d& angular_velocity); - void AddTrajectoryNode(const mapping::Submaps* trajectory, common::Time time, + void AddTrajectoryNode(int trajectory_id, common::Time time, const transform::Rigid3d& point_cloud_pose); void AddSubmap(int trajectory_id, const transform::Rigid3d& submap_pose); @@ -80,7 +79,7 @@ class OptimizationProblem { private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; - std::map> imu_data_; + std::vector> imu_data_; std::vector node_data_; std::vector> submap_data_; double gravity_constant_ = 9.8; diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc index 42f998a..593e135 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem_test.cc @@ -106,7 +106,6 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { const transform::Rigid3d kSubmap0Transform = transform::Rigid3d::Identity(); const transform::Rigid3d kSubmap2Transform = transform::Rigid3d::Rotation( Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())); - const mapping::Submaps* const kTrajectory = nullptr; const int kTrajectoryId = 0; struct NoisyNode { @@ -123,10 +122,10 @@ TEST_F(OptimizationProblemTest, ReducesNoise) { for (const NoisyNode& node : test_data) { const transform::Rigid3d pose = AddNoise(node.ground_truth_pose, node.noise); - optimization_problem_.AddImuData(kTrajectory, now, + optimization_problem_.AddImuData(kTrajectoryId, now, Eigen::Vector3d::UnitZ() * 9.81, Eigen::Vector3d::Zero()); - optimization_problem_.AddTrajectoryNode(kTrajectory, now, pose); + optimization_problem_.AddTrajectoryNode(kTrajectoryId, now, pose); now += common::FromSeconds(0.01); }