From 926b0320cb0ad328bf5f25b1fb17f0a9d5dc8290 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 10 May 2017 16:28:58 +0200 Subject: [PATCH] Use per-trajectory SubmapData in OptimizationProblem. (#272) --- cartographer/mapping_2d/sparse_pose_graph.cc | 68 ++++++++++-------- cartographer/mapping_2d/sparse_pose_graph.h | 12 +++- .../sparse_pose_graph/optimization_problem.cc | 49 ++++++++----- .../sparse_pose_graph/optimization_problem.h | 6 +- cartographer/mapping_3d/sparse_pose_graph.cc | 72 ++++++++++--------- cartographer/mapping_3d/sparse_pose_graph.h | 12 +++- .../sparse_pose_graph/optimization_problem.cc | 61 ++++++++++------ .../sparse_pose_graph/optimization_problem.h | 6 +- 8 files changed, 167 insertions(+), 119 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index d11fb2a..47dd5ea 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -54,31 +54,32 @@ SparsePoseGraph::~SparsePoseGraph() { void SparsePoseGraph::GrowSubmapTransformsAsNeeded( const std::vector& insertion_submaps) { CHECK(!insertion_submaps.empty()); - CHECK_LT(optimization_problem_.submap_data().size(), - std::numeric_limits::max()); - const int next_submap_index = optimization_problem_.submap_data().size(); - // Verify that we have an index for the first submap. - const int first_submap_index = GetSubmapIndex(insertion_submaps[0]); - CHECK_LE(first_submap_index, next_submap_index); + const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]); + const int trajectory_id = first_submap_id.trajectory_id; + CHECK_GE(trajectory_id, 0); + const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { - // If we don't already have an entry for this submap, add one. - if (first_submap_index == next_submap_index) { - optimization_problem_.AddSubmap( - submap_states_[first_submap_index].id.trajectory_id, - transform::Rigid2d::Identity()); + // If we don't already have an entry for the first submap, add one. + CHECK_EQ(first_submap_id.submap_index, 0); + if (static_cast(trajectory_id) >= submap_data.size() || + submap_data[trajectory_id].empty()) { + optimization_problem_.AddSubmap(trajectory_id, + transform::Rigid2d::Identity()); } return; } CHECK_EQ(2, insertion_submaps.size()); + const int next_submap_index = submap_data.at(trajectory_id).size(); // CHECK that we have a index for the second submap. - const int second_submap_index = GetSubmapIndex(insertion_submaps[1]); - CHECK_LE(second_submap_index, next_submap_index); + const mapping::SubmapId second_submap_id = GetSubmapId(insertion_submaps[1]); + CHECK_EQ(second_submap_id.trajectory_id, trajectory_id); + CHECK_LE(second_submap_id.submap_index, next_submap_index); // Extrapolate if necessary. - if (second_submap_index == next_submap_index) { + if (second_submap_id.submap_index == next_submap_index) { const auto& first_submap_pose = - optimization_problem_.submap_data().at(first_submap_index).pose; + submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose; optimization_problem_.AddSubmap( - submap_states_[second_submap_index].id.trajectory_id, + trajectory_id, first_submap_pose * sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]) .inverse() * @@ -160,8 +161,12 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, void SparsePoseGraph::ComputeConstraint(const int scan_index, const int submap_index) { + const mapping::SubmapId submap_id = submap_states_[submap_index].id; const transform::Rigid2d relative_pose = - optimization_problem_.submap_data().at(submap_index).pose.inverse() * + optimization_problem_.submap_data() + .at(submap_id.trajectory_id) + .at(submap_id.submap_index) + .pose.inverse() * optimization_problem_.node_data().at(scan_index).point_cloud_pose; const mapping::Submaps* const scan_trajectory = @@ -169,8 +174,6 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, const mapping::Submaps* const submap_trajectory = submap_states_[submap_index].trajectory; - const mapping::SubmapId submap_id = submap_states_[submap_index].id; - // Only globally match against submaps not in this trajectory. if (scan_trajectory != submap_trajectory && global_localization_samplers_[scan_trajectory]->Pulse()) { @@ -214,9 +217,12 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& covariance) { GrowSubmapTransformsAsNeeded(insertion_submaps); - const int matching_index = GetSubmapIndex(matching_submap); + const mapping::SubmapId matching_id = GetSubmapId(matching_submap); const transform::Rigid2d optimized_pose = - optimization_problem_.submap_data().at(matching_index).pose * + optimization_problem_.submap_data() + .at(matching_id.trajectory_id) + .at(matching_id.submap_index) + .pose * sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; CHECK_EQ(scan_index, optimization_problem_.node_data().size()); const mapping::TrajectoryNode::ConstantData* const scan_data = @@ -358,16 +364,12 @@ void SparsePoseGraph::RunOptimization() { // Extrapolate all point cloud poses that were added later. std::unordered_map extrapolation_transforms; - std::vector submap_transforms; - for (const auto& submap_data : optimization_problem_.submap_data()) { - submap_transforms.push_back(submap_data.pose); - } for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { const mapping::Submaps* trajectory = trajectory_nodes_[i].constant_data->trajectory; if (extrapolation_transforms.count(trajectory) == 0) { - const auto new_submap_transforms = - ExtrapolateSubmapTransforms(submap_transforms, trajectory); + const auto new_submap_transforms = ExtrapolateSubmapTransforms( + optimization_problem_.submap_data(), trajectory); const auto old_submap_transforms = ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); @@ -378,7 +380,7 @@ void SparsePoseGraph::RunOptimization() { trajectory_nodes_[i].pose = extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; } - optimized_submap_transforms_ = submap_transforms; + optimized_submap_transforms_ = optimization_problem_.submap_data(); connected_components_ = trajectory_connectivity_.ConnectedComponents(); reverse_connected_components_.clear(); for (size_t i = 0; i != connected_components_.size(); ++i) { @@ -426,7 +428,8 @@ std::vector SparsePoseGraph::GetSubmapTransforms( } std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( - const std::vector& submap_transforms, + const std::vector>& + submap_transforms, const mapping::Submaps* const trajectory) const { std::vector result; size_t flat_index = 0; @@ -438,10 +441,13 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( if (state.trajectory != trajectory) { continue; } - if (flat_index >= submap_transforms.size()) { + const int trajectory_id = trajectory_ids_.at(trajectory); + if (static_cast(trajectory_id) >= submap_transforms.size() || + result.size() >= submap_transforms.at(trajectory_id).size()) { break; } - result.push_back(transform::Embed3D(submap_transforms[flat_index])); + result.push_back(transform::Embed3D( + submap_transforms.at(trajectory_id).at(result.size()).pose)); flat_index_of_result_back = flat_index; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 4472473..342dffb 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -127,6 +127,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { return iterator->second; } + mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const + REQUIRES(mutex_) { + return submap_states_.at(GetSubmapIndex(submap)).id; + } + // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. void GrowSubmapTransformsAsNeeded( @@ -162,7 +167,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds extrapolated transforms, so that there are transforms for all submaps. std::vector ExtrapolateSubmapTransforms( - const std::vector& submap_transforms, + const std::vector>& + submap_transforms, const mapping::Submaps* trajectory) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; @@ -212,8 +218,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector trajectory_nodes_ GUARDED_BY(mutex_); // Current submap transforms used for displaying data. - std::vector optimized_submap_transforms_ - GUARDED_BY(mutex_); + std::vector> + optimized_submap_transforms_ GUARDED_BY(mutex_); // Map from submap pointers to trajectory IDs. std::unordered_map trajectory_ids_ diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 54dc792..7bb8cbc 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -16,6 +16,7 @@ #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" +#include #include #include #include @@ -77,7 +78,10 @@ void OptimizationProblem::AddTrajectoryNode( void OptimizationProblem::AddSubmap(const int trajectory_id, const transform::Rigid2d& submap_pose) { - submap_data_.push_back(SubmapData{trajectory_id, submap_pose}); + CHECK_GE(trajectory_id, 0); + submap_data_.resize( + std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); + submap_data_[trajectory_id].push_back(SubmapData{submap_pose}); } void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { @@ -96,27 +100,31 @@ void OptimizationProblem::Solve(const std::vector& constraints) { // Set the starting point. // TODO(hrapp): Move ceres data into SubmapData. - std::deque>> C_submaps; + std::vector>> C_submaps(submap_data_.size()); std::vector> C_point_clouds(node_data_.size()); - for (size_t i = 0; i != submap_data_.size(); ++i) { - while (static_cast(C_submaps.size()) <= - submap_data_[i].trajectory_id) { - C_submaps.emplace_back(); + for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); + ++trajectory_id) { + for (size_t submap_index = 0; + submap_index != submap_data_[trajectory_id].size(); ++submap_index) { + if (trajectory_id == 0 && submap_index == 0) { + // Fix the pose of the first submap of the first trajectory. + C_submaps[trajectory_id].push_back( + FromPose(transform::Rigid2d::Identity())); + problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3); + problem.SetParameterBlockConstant( + C_submaps[trajectory_id].back().data()); + } else { + C_submaps[trajectory_id].push_back( + FromPose(submap_data_[trajectory_id][submap_index].pose)); + problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3); + } } - C_submaps[submap_data_[i].trajectory_id].push_back( - FromPose(submap_data_[i].pose)); - problem.AddParameterBlock( - C_submaps[submap_data_[i].trajectory_id].back().data(), 3); } for (size_t j = 0; j != node_data_.size(); ++j) { C_point_clouds[j] = FromPose(node_data_[j].point_cloud_pose); problem.AddParameterBlock(C_point_clouds[j].data(), 3); } - // Fix the pose of the first submap. - problem.SetParameterBlockConstant( - C_submaps[submap_data_[0].trajectory_id].front().data()); - // Add cost functions for intra- and inter-submap constraints. for (const Constraint& constraint : constraints) { CHECK_GE(constraint.j, 0); @@ -179,9 +187,13 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } // Store the result. - for (auto& submap_data : submap_data_) { - submap_data.pose = ToPose(C_submaps[submap_data.trajectory_id].front()); - C_submaps[submap_data.trajectory_id].pop_front(); + for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); + ++trajectory_id) { + for (size_t submap_index = 0; + submap_index != submap_data_[trajectory_id].size(); ++submap_index) { + submap_data_[trajectory_id][submap_index].pose = + ToPose(C_submaps[trajectory_id][submap_index]); + } } for (size_t j = 0; j != node_data_.size(); ++j) { @@ -193,7 +205,8 @@ const std::vector& OptimizationProblem::node_data() const { return node_data_; } -const std::vector& OptimizationProblem::submap_data() const { +const std::vector>& OptimizationProblem::submap_data() + const { return submap_data_; } diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index fea8f27..0ca0766 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -43,8 +43,6 @@ struct NodeData { }; struct SubmapData { - // TODO(whess): Keep nodes per trajectory instead. - const int trajectory_id; transform::Rigid2d pose; }; @@ -75,13 +73,13 @@ class OptimizationProblem { void Solve(const std::vector& constraints); const std::vector& node_data() const; - const std::vector& submap_data() const; + const std::vector>& submap_data() const; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; std::map> imu_data_; std::vector node_data_; - std::vector submap_data_; + std::vector> submap_data_; }; } // namespace sparse_pose_graph diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 9630d3d..2fadc45 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -55,33 +55,34 @@ SparsePoseGraph::~SparsePoseGraph() { void SparsePoseGraph::GrowSubmapTransformsAsNeeded( const std::vector& insertion_submaps) { CHECK(!insertion_submaps.empty()); - CHECK_LT(optimization_problem_.submap_data().size(), - std::numeric_limits::max()); - const int next_submap_index = optimization_problem_.submap_data().size(); - // Verify that we have an index for the first submap. - const int first_submap_index = GetSubmapIndex(insertion_submaps[0]); - CHECK_LE(first_submap_index, next_submap_index); + const mapping::SubmapId first_submap_id = GetSubmapId(insertion_submaps[0]); + const int trajectory_id = first_submap_id.trajectory_id; + CHECK_GE(trajectory_id, 0); + const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { - // If we don't already have an entry for this submap, add one. - if (first_submap_index == next_submap_index) { - optimization_problem_.AddSubmap( - submap_states_[first_submap_index].id.trajectory_id, - transform::Rigid3d::Identity()); + // If we don't already have an entry for the first submap, add one. + CHECK_EQ(first_submap_id.submap_index, 0); + if (static_cast(trajectory_id) >= submap_data.size() || + submap_data[trajectory_id].empty()) { + optimization_problem_.AddSubmap(trajectory_id, + transform::Rigid3d::Identity()); } return; } CHECK_EQ(2, insertion_submaps.size()); + const int next_submap_index = submap_data.at(trajectory_id).size(); // CHECK that we have a index for the second submap. - const int second_submap_index = GetSubmapIndex(insertion_submaps[1]); - CHECK_LE(second_submap_index, next_submap_index); + const mapping::SubmapId second_submap_id = GetSubmapId(insertion_submaps[1]); + CHECK_EQ(second_submap_id.trajectory_id, trajectory_id); + CHECK_LE(second_submap_id.submap_index, next_submap_index); // Extrapolate if necessary. - if (second_submap_index == next_submap_index) { + if (second_submap_id.submap_index == next_submap_index) { const auto& first_submap_pose = - optimization_problem_.submap_data().at(first_submap_index).pose; + submap_data.at(trajectory_id).at(first_submap_id.submap_index).pose; optimization_problem_.AddSubmap( - submap_states_[second_submap_index].id.trajectory_id, - first_submap_pose * insertion_submaps[0]->local_pose().inverse() * - insertion_submaps[1]->local_pose()); + trajectory_id, first_submap_pose * + insertion_submaps[0]->local_pose().inverse() * + insertion_submaps[1]->local_pose()); } } @@ -160,8 +161,12 @@ void SparsePoseGraph::AddImuData(const mapping::Submaps* trajectory, void SparsePoseGraph::ComputeConstraint(const int scan_index, const int submap_index) { + const mapping::SubmapId submap_id = submap_states_[submap_index].id; const transform::Rigid3d relative_pose = - optimization_problem_.submap_data().at(submap_index).pose.inverse() * + optimization_problem_.submap_data() + .at(submap_id.trajectory_id) + .at(submap_id.submap_index) + .pose.inverse() * optimization_problem_.node_data().at(scan_index).point_cloud_pose; const mapping::Submaps* const scan_trajectory = @@ -169,8 +174,6 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, const mapping::Submaps* const submap_trajectory = submap_states_[submap_index].trajectory; - const mapping::SubmapId submap_id = submap_states_[submap_index].id; - // Only globally match against submaps not in this trajectory. if (scan_trajectory != submap_trajectory && global_localization_samplers_[scan_trajectory]->Pulse()) { @@ -212,9 +215,12 @@ void SparsePoseGraph::ComputeConstraintsForScan( const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) { GrowSubmapTransformsAsNeeded(insertion_submaps); - const int matching_index = GetSubmapIndex(matching_submap); + const mapping::SubmapId matching_id = GetSubmapId(matching_submap); const transform::Rigid3d optimized_pose = - optimization_problem_.submap_data().at(matching_index).pose * + optimization_problem_.submap_data() + .at(matching_id.trajectory_id) + .at(matching_id.submap_index) + .pose * matching_submap->local_pose().inverse() * pose; CHECK_EQ(scan_index, optimization_problem_.node_data().size()); const mapping::TrajectoryNode::ConstantData* const scan_data = @@ -351,16 +357,12 @@ void SparsePoseGraph::RunOptimization() { // Extrapolate all point cloud poses that were added later. std::unordered_map extrapolation_transforms; - std::vector submap_transforms; - for (const auto& submap_data : optimization_problem_.submap_data()) { - submap_transforms.push_back(submap_data.pose); - } for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { const mapping::Submaps* trajectory = trajectory_nodes_[i].constant_data->trajectory; if (extrapolation_transforms.count(trajectory) == 0) { - const auto new_submap_transforms = - ExtrapolateSubmapTransforms(submap_transforms, trajectory); + const auto new_submap_transforms = ExtrapolateSubmapTransforms( + optimization_problem_.submap_data(), trajectory); const auto old_submap_transforms = ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); @@ -371,7 +373,7 @@ void SparsePoseGraph::RunOptimization() { trajectory_nodes_[i].pose = extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; } - optimized_submap_transforms_ = submap_transforms; + optimized_submap_transforms_ = optimization_problem_.submap_data(); connected_components_ = trajectory_connectivity_.ConnectedComponents(); reverse_connected_components_.clear(); for (size_t i = 0; i != connected_components_.size(); ++i) { @@ -419,7 +421,8 @@ std::vector SparsePoseGraph::GetSubmapTransforms( } std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( - const std::vector& submap_transforms, + const std::vector>& + submap_transforms, const mapping::Submaps* const trajectory) const { std::vector result; size_t flat_index = 0; @@ -431,10 +434,13 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( if (state.trajectory != trajectory) { continue; } - if (flat_index >= submap_transforms.size()) { + const int trajectory_id = trajectory_ids_.at(trajectory); + if (static_cast(trajectory_id) >= submap_transforms.size() || + result.size() >= submap_transforms.at(trajectory_id).size()) { break; } - result.push_back(submap_transforms[flat_index]); + result.push_back( + submap_transforms.at(trajectory_id).at(result.size()).pose); flat_index_of_result_back = flat_index; } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index aa9dfe5..918e2cb 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -129,6 +129,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { return iterator->second; } + mapping::SubmapId GetSubmapId(const mapping::Submap* submap) const + REQUIRES(mutex_) { + return submap_states_.at(GetSubmapIndex(submap)).id; + } + // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. void GrowSubmapTransformsAsNeeded( @@ -162,7 +167,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds extrapolated transforms, so that there are transforms for all submaps. std::vector ExtrapolateSubmapTransforms( - const std::vector& submap_transforms, + const std::vector>& + submap_transforms, const mapping::Submaps* trajectory) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; @@ -212,8 +218,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector trajectory_nodes_ GUARDED_BY(mutex_); // Current submap transforms used for displaying data. - std::vector optimized_submap_transforms_ - GUARDED_BY(mutex_); + std::vector> + optimized_submap_transforms_ GUARDED_BY(mutex_); // Map from submap pointers to trajectory IDs. std::unordered_map trajectory_ids_ diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index e8a33f8..c2ee175 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -16,6 +16,7 @@ #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" +#include #include #include #include @@ -93,7 +94,10 @@ void OptimizationProblem::AddTrajectoryNode( void OptimizationProblem::AddSubmap(const int trajectory_id, const transform::Rigid3d& submap_pose) { - submap_data_.push_back(SubmapData{trajectory_id, submap_pose}); + CHECK_GE(trajectory_id, 0); + submap_data_.resize( + std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); + submap_data_[trajectory_id].push_back(SubmapData{submap_pose}); } void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { @@ -125,25 +129,31 @@ void OptimizationProblem::Solve(const std::vector& constraints) { }; // Set the starting point. - // TODO(hrapp): Move ceres data into SubmapData. - std::vector> C_submaps(nodes_per_trajectory.size()); - - std::deque C_point_clouds; - - // Tie the first submap to the origin. CHECK(!submap_data_.empty()); - C_submaps[submap_data_[0].trajectory_id].emplace_back( - transform::Rigid3d::Identity(), translation_parameterization(), - common::make_unique>(), - &problem); - problem.SetParameterBlockConstant( - C_submaps[submap_data_[0].trajectory_id].back().translation()); - - for (size_t i = 1; i != submap_data_.size(); ++i) { - C_submaps[submap_data_[i].trajectory_id].emplace_back( - submap_data_[i].pose, translation_parameterization(), - common::make_unique(), &problem); + CHECK(!submap_data_[0].empty()); + // TODO(hrapp): Move ceres data into SubmapData. + std::vector> C_submaps(submap_data_.size()); + std::deque C_point_clouds; + for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); + ++trajectory_id) { + for (size_t submap_index = 0; + submap_index != submap_data_[trajectory_id].size(); ++submap_index) { + if (trajectory_id == 0 && submap_index == 0) { + // Tie the first submap of the first trajectory to the origin. + C_submaps[trajectory_id].emplace_back( + transform::Rigid3d::Identity(), translation_parameterization(), + common::make_unique>(), + &problem); + problem.SetParameterBlockConstant( + C_submaps[trajectory_id].back().translation()); + } else { + C_submaps[trajectory_id].emplace_back( + submap_data_[trajectory_id][submap_index].pose, + translation_parameterization(), + common::make_unique(), &problem); + } + } } for (size_t j = 0; j != node_data_.size(); ++j) { C_point_clouds.emplace_back( @@ -248,9 +258,13 @@ void OptimizationProblem::Solve(const std::vector& constraints) { } // Store the result. - for (auto& submap_data : submap_data_) { - submap_data.pose = C_submaps[submap_data.trajectory_id].front().ToRigid(); - C_submaps[submap_data.trajectory_id].pop_front(); + for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); + ++trajectory_id) { + for (size_t submap_index = 0; + submap_index != submap_data_[trajectory_id].size(); ++submap_index) { + submap_data_[trajectory_id][submap_index].pose = + C_submaps[trajectory_id][submap_index].ToRigid(); + } } for (size_t j = 0; j != node_data_.size(); ++j) { @@ -262,7 +276,8 @@ const std::vector& OptimizationProblem::node_data() const { return node_data_; } -const std::vector& OptimizationProblem::submap_data() const { +const std::vector>& OptimizationProblem::submap_data() + const { return submap_data_; } diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h index 30d0637..52d578c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -43,8 +43,6 @@ struct NodeData { }; struct SubmapData { - // TODO(whess): Keep nodes per trajectory instead. - const int trajectory_id; transform::Rigid3d pose; }; @@ -77,14 +75,14 @@ class OptimizationProblem { void Solve(const std::vector& constraints); const std::vector& node_data() const; - const std::vector& submap_data() const; + const std::vector>& submap_data() const; private: mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; FixZ fix_z_; std::map> imu_data_; std::vector node_data_; - std::vector submap_data_; + std::vector> submap_data_; double gravity_constant_ = 9.8; };