From 2dd2d6f448f1f9858b6876222ff38563b15675ec Mon Sep 17 00:00:00 2001 From: Jihoon Lee Date: Wed, 23 Aug 2017 12:16:42 +0200 Subject: [PATCH] Use vector> instead of vector for submap_data. (#422) --- cartographer/mapping/pose_graph_trimmer.cc | 3 +- cartographer/mapping_2d/sparse_pose_graph.cc | 95 +++++++------------ cartographer/mapping_2d/sparse_pose_graph.h | 8 +- .../sparse_pose_graph/optimization_problem.cc | 50 ++++------ .../sparse_pose_graph/optimization_problem.h | 7 +- 5 files changed, 61 insertions(+), 102 deletions(-) diff --git a/cartographer/mapping/pose_graph_trimmer.cc b/cartographer/mapping/pose_graph_trimmer.cc index 60ac470..9103e5b 100644 --- a/cartographer/mapping/pose_graph_trimmer.cc +++ b/cartographer/mapping/pose_graph_trimmer.cc @@ -28,8 +28,7 @@ PureLocalizationTrimmer::PureLocalizationTrimmer(const int trajectory_id, } void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) { - const int total_num_submaps = pose_graph->num_submaps(trajectory_id_); - while (total_num_submaps > num_submaps_trimmed_ + num_submaps_to_keep_) { + while (pose_graph->num_submaps(trajectory_id_) > num_submaps_to_keep_) { const int submap_index_to_trim_next = num_submaps_trimmed_; pose_graph->MarkSubmapAsTrimmed( SubmapId{trajectory_id_, submap_index_to_trim_next}); diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index ead7f2c..7eae8b2 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -64,25 +64,20 @@ std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( trajectory_id, sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])); } - CHECK_EQ(optimization_problem_.num_trimmed_submaps(trajectory_id), 0); 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()); - const int num_trimmed_submaps = - optimization_problem_.num_trimmed_submaps(trajectory_id); const mapping::SubmapId last_submap_id{ - trajectory_id, static_cast(submap_data.at(trajectory_id).size() + - num_trimmed_submaps - 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. const auto& first_submap_pose = - submap_data.at(trajectory_id) - .at(last_submap_id.submap_index - num_trimmed_submaps) - .pose; + submap_data.at(trajectory_id).at(last_submap_id.submap_index).pose; optimization_problem_.AddSubmap( trajectory_id, first_submap_pose * @@ -194,14 +189,13 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, const transform::Rigid2d initial_relative_pose = optimization_problem_.submap_data() .at(submap_id.trajectory_id) - .at(submap_id.submap_index - - optimization_problem_.num_trimmed_submaps( - submap_id.trajectory_id)) + .at(submap_id.submap_index) .pose.inverse() * optimization_problem_.node_data() .at(node_id.trajectory_id) - .at(node_id.node_index - optimization_problem_.num_trimmed_nodes( - node_id.trajectory_id)) + .at(node_id.node_index - + optimization_problem_.num_trimmed_nodes( + node_id.trajectory_id)) .point_cloud_pose; constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, @@ -241,12 +235,10 @@ void SparsePoseGraph::ComputeConstraintsForScan( GrowSubmapTransformsAsNeeded(trajectory_id, insertion_submaps); CHECK_EQ(submap_ids.size(), insertion_submaps.size()); const mapping::SubmapId matching_id = submap_ids.front(); - const int num_trimmed_submaps = - optimization_problem_.num_trimmed_submaps(trajectory_id); const transform::Rigid2d optimized_pose = optimization_problem_.submap_data() .at(matching_id.trajectory_id) - .at(matching_id.submap_index - num_trimmed_submaps) + .at(matching_id.submap_index) .pose * sparse_pose_graph::ComputeSubmapPose(*insertion_submaps.front()) .inverse() * @@ -356,21 +348,19 @@ void SparsePoseGraph::WaitForAllComputations() { common::FromSeconds(1.))) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) - << 100. * - (constraint_builder_.GetNumFinishedScans() - - num_finished_scans_at_start) / + << 100. * (constraint_builder_.GetNumFinishedScans() - + num_finished_scans_at_start) / (num_trajectory_nodes_ - num_finished_scans_at_start) << "%..."; std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; - constraint_builder_.WhenDone( - [this, ¬ification]( - const sparse_pose_graph::ConstraintBuilder::Result& result) { - common::MutexLocker locker(&mutex_); - constraints_.insert(constraints_.end(), result.begin(), result.end()); - notification = true; - }); + constraint_builder_.WhenDone([this, ¬ification]( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + common::MutexLocker locker(&mutex_); + constraints_.insert(constraints_.end(), result.begin(), result.end()); + notification = true; + }); locker.Await([¬ification]() { return notification; }); } @@ -401,14 +391,11 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id, CHECK_GE(static_cast(submap_data_.num_trajectories()), optimized_submap_transforms_.size()); optimized_submap_transforms_.resize(submap_data_.num_trajectories()); - CHECK_GE(static_cast(submap_data_.num_trajectories()), - num_trimmed_submaps_at_last_optimization_.size()); - num_trimmed_submaps_at_last_optimization_.resize( - submap_data_.num_trajectories()); 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_2d}); + .emplace(submap_id.submap_index, + sparse_pose_graph::SubmapData{initial_pose_2d}); AddWorkItem([this, submap_id, initial_pose_2d]() REQUIRES(mutex_) { CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); submap_data_.at(submap_id).state = SubmapState::kFinished; @@ -447,14 +434,7 @@ void SparsePoseGraph::RunOptimization() { optimization_problem_.Solve(constraints_, frozen_trajectories_); common::MutexLocker locker(&mutex_); - std::vector num_trimmed_submaps; const auto& submap_data = optimization_problem_.submap_data(); - for (int trajectory_id = 0; - trajectory_id != static_cast(submap_data.size()); ++trajectory_id) { - num_trimmed_submaps.push_back( - optimization_problem_.num_trimmed_submaps(trajectory_id)); - } - const auto& node_data = optimization_problem_.node_data(); for (int trajectory_id = 0; trajectory_id != static_cast(node_data.size()); ++trajectory_id) { @@ -468,11 +448,10 @@ void SparsePoseGraph::RunOptimization() { node_data[trajectory_id][node_data_index].point_cloud_pose); } // Extrapolate all point cloud poses that were added later. - const auto local_to_new_global = ComputeLocalToGlobalTransform( - submap_data, num_trimmed_submaps, trajectory_id); + const auto local_to_new_global = + ComputeLocalToGlobalTransform(submap_data, trajectory_id); const auto local_to_old_global = ComputeLocalToGlobalTransform( - optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_, - trajectory_id); + optimized_submap_transforms_, trajectory_id); const transform::Rigid3d old_global_to_new_global = local_to_new_global * local_to_old_global.inverse(); for (; node_index < num_nodes; ++node_index) { @@ -482,7 +461,6 @@ void SparsePoseGraph::RunOptimization() { } } optimized_submap_transforms_ = submap_data; - num_trimmed_submaps_at_last_optimization_ = num_trimmed_submaps; connected_components_ = trajectory_connectivity_.ConnectedComponents(); reverse_connected_components_.clear(); for (size_t i = 0; i != connected_components_.size(); ++i) { @@ -511,9 +489,8 @@ std::vector SparsePoseGraph::constraints() { transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); - return ComputeLocalToGlobalTransform( - optimized_submap_transforms_, num_trimmed_submaps_at_last_optimization_, - trajectory_id); + return ComputeLocalToGlobalTransform(optimized_submap_transforms_, + trajectory_id); } std::vector> SparsePoseGraph::GetConnectedTrajectories() { @@ -555,20 +532,19 @@ SparsePoseGraph::GetAllSubmapData() { } transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( - const std::vector>& + const std::vector>& submap_transforms, - const std::vector& num_trimmed_submaps, 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() + - num_trimmed_submaps.at(trajectory_id) - 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 transform::Embed3D(submap_transforms.at(trajectory_id).back().pose) * + return transform::Embed3D( + submap_transforms.at(trajectory_id).at(submap_index).pose) * submap_data_.at(last_optimized_submap_id) .submap->local_pose() .inverse(); @@ -582,9 +558,7 @@ 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 - - num_trimmed_submaps_at_last_optimization_.at(submap_id.trajectory_id); + 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. @@ -595,10 +569,8 @@ mapping::SparsePoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( } } // We have to extrapolate. - return {submap, ComputeLocalToGlobalTransform( - optimized_submap_transforms_, - num_trimmed_submaps_at_last_optimization_, - submap_id.trajectory_id) * + return {submap, ComputeLocalToGlobalTransform(optimized_submap_transforms_, + submap_id.trajectory_id) * submap->local_pose()}; } @@ -607,8 +579,7 @@ SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent) int SparsePoseGraph::TrimmingHandle::num_submaps( const int trajectory_id) const { - return parent_->optimization_problem_.submap_data().at(trajectory_id).size() + - parent_->optimization_problem_.num_trimmed_submaps(trajectory_id); + return parent_->optimization_problem_.submap_data().at(trajectory_id).size(); } void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 43d67d2..c30eee2 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -155,10 +155,9 @@ 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, - const std::vector& num_trimmed_submaps, int trajectory_id) const - REQUIRES(mutex_); + int trajectory_id) const REQUIRES(mutex_); mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) REQUIRES(mutex_); @@ -205,8 +204,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; // Current submap transforms used for displaying data. - std::vector num_trimmed_submaps_at_last_optimization_ GUARDED_BY(mutex_); - std::vector> + std::vector> optimized_submap_transforms_ GUARDED_BY(mutex_); // List of all trimmers to consult when optimizations finish. diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index d193a69..128e010 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -112,19 +112,18 @@ 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_.at(trajectory_id); + submap_data_[trajectory_id].emplace(trajectory_data.next_submap_index, + SubmapData{submap_pose}); + ++trajectory_data.next_submap_index; } void OptimizationProblem::TrimSubmap(const mapping::SubmapId& submap_id) { - auto& trajectory_data = trajectory_data_.at(submap_id.trajectory_id); - // We only allow trimming from the start. - CHECK_EQ(trajectory_data.num_trimmed_submaps, submap_id.submap_index); auto& submap_data = submap_data_.at(submap_id.trajectory_id); - CHECK(!submap_data.empty()); - submap_data.pop_front(); - ++trajectory_data.num_trimmed_submaps; + CHECK(submap_data.erase(submap_id.submap_index)); } void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) { @@ -144,25 +143,27 @@ void OptimizationProblem::Solve(const std::vector& constraints, // Set the starting point. // TODO(hrapp): Move ceres data into SubmapData. - std::vector>> C_submaps( + 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); - // Reserve guarantees that data does not move, so the pointers for Ceres - // stay valid. - C_submaps[trajectory_id].reserve(submap_data_[trajectory_id].size()); - for (const SubmapData& submap_data : submap_data_[trajectory_id]) { - C_submaps[trajectory_id].push_back(FromPose(submap_data.pose)); - problem.AddParameterBlock(C_submaps[trajectory_id].back().data(), 3); + for (const auto& index_submap_data : submap_data_[trajectory_id]) { + const int submap_index = index_submap_data.first; + const SubmapData& submap_data = index_submap_data.second; + + C_submaps[trajectory_id].emplace( + submap_index, FromPose(submap_data.pose)); + problem.AddParameterBlock( + C_submaps[trajectory_id].at(submap_index).data(), 3); if (first_submap || frozen) { first_submap = false; // Fix the pose of the first submap or all submaps of a frozen // trajectory. problem.SetParameterBlockConstant( - C_submaps[trajectory_id].back().data()); + C_submaps[trajectory_id].at(submap_index).data()); } } } @@ -190,9 +191,7 @@ void OptimizationProblem::Solve(const std::vector& constraints, ? new ceres::HuberLoss(options_.huber_scale()) : nullptr, C_submaps.at(constraint.submap_id.trajectory_id) - .at(constraint.submap_id.submap_index - - trajectory_data_.at(constraint.submap_id.trajectory_id) - .num_trimmed_submaps) + .at(constraint.submap_id.submap_index) .data(), C_nodes.at(constraint.node_id.trajectory_id) .at(constraint.node_id.node_index - @@ -244,7 +243,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(); } @@ -252,11 +250,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_data_index = 0; - submap_data_index != submap_data_[trajectory_id].size(); - ++submap_data_index) { - submap_data_[trajectory_id][submap_data_index].pose = - ToPose(C_submaps[trajectory_id][submap_data_index]); + for (auto& index_submap_data : submap_data_[trajectory_id]) { + index_submap_data.second.pose = + ToPose(C_submaps[trajectory_id].at(index_submap_data.first)); } } for (size_t trajectory_id = 0; trajectory_id != node_data_.size(); @@ -275,7 +271,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_; } @@ -284,10 +280,6 @@ int OptimizationProblem::num_trimmed_nodes(int trajectory_id) const { return trajectory_data_.at(trajectory_id).num_trimmed_nodes; } -int OptimizationProblem::num_trimmed_submaps(int trajectory_id) const { - return trajectory_data_.at(trajectory_id).num_trimmed_submaps; -} - } // namespace sparse_pose_graph } // namespace mapping_2d } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index b8f16f9..e856871 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -77,22 +77,21 @@ 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; int num_trimmed_nodes(int trajectory_id) const; - int num_trimmed_submaps(int trajectory_id) const; private: struct TrajectoryData { // TODO(hrapp): Remove, once we can relabel constraints. + int next_submap_index = 0; int num_trimmed_nodes = 0; - int num_trimmed_submaps = 0; }; mapping::sparse_pose_graph::proto::OptimizationProblemOptions options_; std::vector> imu_data_; std::vector> node_data_; std::vector odometry_data_; - std::vector> submap_data_; + std::vector> submap_data_; std::vector trajectory_data_; };