diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index c57c8ac..06c9dde 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -565,16 +565,15 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( } auto submap = submap_data_.at(submap_id).submap; if (submap_id.trajectory_id < - static_cast(optimized_submap_transforms_.size())) { - const size_t submap_data_index = submap_id.submap_index; - if (submap_data_index < - optimized_submap_transforms_.at(submap_id.trajectory_id).size()) { - // We already have an optimized pose. - return {submap, transform::Embed3D(optimized_submap_transforms_ - .at(submap_id.trajectory_id) - .at(submap_data_index) - .pose)}; - } + static_cast(optimized_submap_transforms_.size()) && + submap_id.submap_index < static_cast(optimized_submap_transforms_ + .at(submap_id.trajectory_id) + .size())) { + // We already have an optimized pose. + return {submap, transform::Embed3D( + optimized_submap_transforms_.at(submap_id.trajectory_id) + .at(submap_id.submap_index) + .pose)}; } // We have to extrapolate. return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 7c9865f..49c47ac 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -85,8 +85,7 @@ void OptimizationProblem::AddTrajectoryNode( node_data_.resize( std::max(node_data_.size(), static_cast(trajectory_id) + 1)); trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); - - auto& trajectory_data = trajectory_data_.at(trajectory_id); + auto& trajectory_data = trajectory_data_[trajectory_id]; node_data_[trajectory_id].emplace(trajectory_data.next_node_index, NodeData{time, initial_pose, pose}); ++trajectory_data.next_node_index; @@ -98,9 +97,7 @@ void OptimizationProblem::TrimTrajectoryNode(const mapping::NodeId& node_id) { if (!node_data.empty() && node_id.trajectory_id < static_cast(imu_data_.size())) { - auto node_it = node_data.begin(); - const common::Time node_time = node_it->second.time; - + const common::Time node_time = node_data.begin()->second.time; auto& imu_data = imu_data_.at(node_id.trajectory_id); while (imu_data.size() > 1 && imu_data[1].time <= node_time) { imu_data.pop_front(); @@ -115,8 +112,7 @@ void OptimizationProblem::AddSubmap(const int trajectory_id, std::max(submap_data_.size(), static_cast(trajectory_id) + 1)); trajectory_data_.resize( std::max(trajectory_data_.size(), submap_data_.size())); - - auto& trajectory_data = trajectory_data_.at(trajectory_id); + auto& trajectory_data = trajectory_data_[trajectory_id]; submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index, SubmapData{submap_pose}); ++trajectory_data.next_submap_index; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 3f182fe..892ac1c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -64,15 +64,15 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( optimization_problem_.AddSubmap(trajectory_id, insertion_submaps[0]->local_pose()); } - const mapping::SubmapId submap_id{ - trajectory_id, static_cast(submap_data[trajectory_id].size()) - 1}; + CHECK_EQ(submap_data[trajectory_id].size(), 1); + const mapping::SubmapId submap_id{trajectory_id, 0}; CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front()); return {submap_id}; } CHECK_EQ(2, insertion_submaps.size()); + CHECK(!submap_data.at(trajectory_id).empty()); const mapping::SubmapId last_submap_id{ - trajectory_id, - static_cast(submap_data.at(trajectory_id).size() - 1)}; + trajectory_id, submap_data.at(trajectory_id).rbegin()->first}; if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) { // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()' // and 'insertions_submaps.back()' is new. @@ -98,6 +98,7 @@ void SparsePoseGraph::AddScan( const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( GetLocalToGlobalTransform(trajectory_id) * pose); + common::MutexLocker locker(&mutex_); trajectory_nodes_.Append( trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); @@ -411,7 +412,8 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, CHECK_EQ(optimized_submap_transforms_.at(trajectory_id).size(), submap_id.submap_index); optimized_submap_transforms_.at(trajectory_id) - .push_back(sparse_pose_graph::SubmapData{initial_pose}); + .emplace(submap_id.submap_index, + sparse_pose_graph::SubmapData{initial_pose}); AddWorkItem([this, submap_id, initial_pose]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; @@ -478,6 +480,7 @@ void SparsePoseGraph::RunOptimization() { optimization_problem_.Solve(constraints_, frozen_trajectories_); common::MutexLocker locker(&mutex_); + const auto& submap_data = optimization_problem_.submap_data(); const auto& node_data = optimization_problem_.node_data(); for (int trajectory_id = 0; trajectory_id != static_cast(node_data.size()); ++trajectory_id) { @@ -490,8 +493,8 @@ void SparsePoseGraph::RunOptimization() { node_data[trajectory_id][node_index].pose; } // Extrapolate all point cloud poses that were added later. - const auto local_to_new_global = ComputeLocalToGlobalTransform( - optimization_problem_.submap_data(), trajectory_id); + const auto local_to_new_global = + ComputeLocalToGlobalTransform(submap_data, trajectory_id); const auto local_to_old_global = ComputeLocalToGlobalTransform( optimized_submap_transforms_, trajectory_id); const transform::Rigid3d old_global_to_new_global = @@ -502,7 +505,7 @@ void SparsePoseGraph::RunOptimization() { old_global_to_new_global * trajectory_nodes_.at(node_id).pose; } } - optimized_submap_transforms_ = optimization_problem_.submap_data(); + optimized_submap_transforms_ = submap_data; TrimmingHandle trimming_handle(this); for (auto& trimmer : trimmers_) { @@ -571,18 +574,18 @@ SparsePoseGraph::GetAllSubmapData() { } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( - const std::vector>& + const std::vector>& submap_transforms, const int trajectory_id) const { if (trajectory_id >= static_cast(submap_transforms.size()) || submap_transforms.at(trajectory_id).empty()) { return transform::Rigid3d::Identity(); } - const mapping::SubmapId last_optimized_submap_id{ - trajectory_id, - static_cast(submap_transforms.at(trajectory_id).size() - 1)}; + + const int submap_index = submap_transforms.at(trajectory_id).rbegin()->first; + const mapping::SubmapId last_optimized_submap_id{trajectory_id, submap_index}; // Accessing 'local_pose' in Submap is okay, since the member is const. - return submap_transforms.at(trajectory_id).back().pose * + return submap_transforms.at(trajectory_id).at(submap_index).pose * submap_data_.at(last_optimized_submap_id) .submap->local_pose() .inverse(); @@ -591,12 +594,12 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) { auto submap = submap_data_.at(submap_id).submap; - // We already have an optimized pose. if (submap_id.trajectory_id < static_cast(optimized_submap_transforms_.size()) && submap_id.submap_index < static_cast(optimized_submap_transforms_ .at(submap_id.trajectory_id) .size())) { + // We already have an optimized pose. return {submap, optimized_submap_transforms_.at(submap_id.trajectory_id) .at(submap_id.submap_index) .pose}; diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index eae42b1..3009e4e 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -157,7 +157,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Computes the local to global frame transform based on the given optimized // 'submap_transforms'. transform::Rigid3d ComputeLocalToGlobalTransform( - const std::vector>& + const std::vector>& submap_transforms, int trajectory_id) const REQUIRES(mutex_); @@ -205,7 +205,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. - std::vector> + std::vector> optimized_submap_transforms_ GUARDED_BY(mutex_); // List of all trimmers to consult when optimizations finish. diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 732247e..11c0b2b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -93,7 +94,12 @@ void OptimizationProblem::AddSubmap(const int trajectory_id, 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}); + trajectory_data_.resize( + std::max(trajectory_data_.size(), submap_data_.size())); + auto& trajectory_data = trajectory_data_[trajectory_id]; + submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index, + SubmapData{submap_pose}); + ++trajectory_data.next_submap_index; } void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { @@ -123,33 +129,40 @@ void OptimizationProblem::Solve(const std::vector& constraints, CHECK(!submap_data_.empty()); CHECK(!submap_data_[0].empty()); // TODO(hrapp): Move ceres data into SubmapData. - std::vector> C_submaps(submap_data_.size()); + std::vector> C_submaps(submap_data_.size()); std::vector> C_nodes(node_data_.size()); + bool first_submap = true; for (size_t trajectory_id = 0; trajectory_id != submap_data_.size(); ++trajectory_id) { const bool frozen = frozen_trajectories.count(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); + for (const auto& index_submap_data : submap_data_[trajectory_id]) { + const int submap_index = index_submap_data.first; + if (first_submap) { + first_submap = false; + // Fix the first submap of the first trajectory except for allowing + // gravity alignment. + C_submaps[trajectory_id].emplace( + std::piecewise_construct, std::forward_as_tuple(submap_index), + std::forward_as_tuple( + index_submap_data.second.pose, translation_parameterization(), + common::make_unique>(), + &problem)); problem.SetParameterBlockConstant( - C_submaps[trajectory_id].back().translation()); + C_submaps[trajectory_id].at(submap_index).translation()); } else { - C_submaps[trajectory_id].emplace_back( - submap_data_[trajectory_id][submap_index].pose, - translation_parameterization(), - common::make_unique(), &problem); + C_submaps[trajectory_id].emplace( + std::piecewise_construct, std::forward_as_tuple(submap_index), + std::forward_as_tuple( + index_submap_data.second.pose, translation_parameterization(), + common::make_unique(), + &problem)); } if (frozen) { problem.SetParameterBlockConstant( - C_submaps[trajectory_id].back().rotation()); + C_submaps[trajectory_id].at(submap_index).rotation()); problem.SetParameterBlockConstant( - C_submaps[trajectory_id].back().translation()); + C_submaps[trajectory_id].at(submap_index).translation()); } } } @@ -170,7 +183,6 @@ void OptimizationProblem::Solve(const std::vector& constraints, } } } - // Add cost functions for intra- and inter-submap constraints. for (const Constraint& constraint : constraints) { problem.AddResidualBlock( @@ -321,7 +333,6 @@ void OptimizationProblem::Solve(const std::vector& constraints, ceres::Solve( common::CreateCeresSolverOptions(options_.ceres_solver_options()), &problem, &summary); - if (options_.log_solver_summary()) { LOG(INFO) << summary.FullReport(); for (size_t trajectory_id = 0; trajectory_id != trajectory_data_.size(); @@ -344,10 +355,9 @@ void OptimizationProblem::Solve(const std::vector& constraints, // Store the result. 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 (auto& index_submap_data : submap_data_[trajectory_id]) { + index_submap_data.second.pose = + C_submaps[trajectory_id].at(index_submap_data.first).ToRigid(); } } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); @@ -365,7 +375,7 @@ const std::vector>& OptimizationProblem::node_data() return node_data_; } -const std::vector>& OptimizationProblem::submap_data() +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 3745f93..43565d1 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h @@ -82,12 +82,13 @@ class OptimizationProblem { const std::set& frozen_trajectories); const std::vector>& node_data() const; - const std::vector>& submap_data() const; + const std::vector>& submap_data() const; private: struct TrajectoryData { double gravity_constant = 9.8; std::array imu_calibration{{1., 0., 0., 0.}}; + int next_submap_index = 0; }; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; @@ -95,7 +96,7 @@ class OptimizationProblem { std::vector> imu_data_; std::vector> node_data_; std::vector odometry_data_; - std::vector> submap_data_; + std::vector> submap_data_; std::vector trajectory_data_; std::vector fixed_frame_pose_data_; };