From 505570349026d0ece5b90f65be46f44f59380fc5 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Fri, 8 Jun 2018 14:40:24 +0200 Subject: [PATCH] Use PoseGraphData in PoseGraph3d. (#1188) [RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0022-pose-graph-data.md) --- .../mapping/internal/3d/pose_graph_3d.cc | 294 ++++++++++-------- .../mapping/internal/3d/pose_graph_3d.h | 46 +-- 2 files changed, 160 insertions(+), 180 deletions(-) diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 99fb6b5..dfaa331 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -61,26 +61,28 @@ std::vector PoseGraph3D::InitializeGlobalSubmapPoses( if (insertion_submaps.size() == 1) { // If we don't already have an entry for the first submap, add one. if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { - if (initial_trajectory_poses_.count(trajectory_id) > 0) { - trajectory_connectivity_state_.Connect( + if (data_.initial_trajectory_poses.count(trajectory_id) > 0) { + data_.trajectory_connectivity_state.Connect( trajectory_id, - initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time); + data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id, + time); } optimization_problem_->AddSubmap( - trajectory_id, - ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id) * - insertion_submaps[0]->local_pose()); + trajectory_id, ComputeLocalToGlobalTransform( + data_.global_submap_poses_3d, trajectory_id) * + insertion_submaps[0]->local_pose()); } CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); const SubmapId submap_id{trajectory_id, 0}; - CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front()); + CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front()); return {submap_id}; } CHECK_EQ(2, insertion_submaps.size()); const auto end_it = submap_data.EndOfTrajectory(trajectory_id); CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it); const SubmapId last_submap_id = std::prev(end_it)->id; - if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) { + if (data_.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(last_submap_id).global_pose; @@ -91,10 +93,12 @@ std::vector PoseGraph3D::InitializeGlobalSubmapPoses( return {last_submap_id, SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; } - CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back()); + CHECK(data_.submap_data.at(last_submap_id).submap == + insertion_submaps.back()); const SubmapId front_submap_id{trajectory_id, last_submap_id.submap_index - 1}; - CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front()); + CHECK(data_.submap_data.at(front_submap_id).submap == + insertion_submaps.front()); return {front_submap_id, last_submap_id}; } @@ -107,19 +111,19 @@ NodeId PoseGraph3D::AddNode( common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(trajectory_id); - const NodeId node_id = trajectory_nodes_.Append( + const NodeId node_id = data_.trajectory_nodes.Append( trajectory_id, TrajectoryNode{constant_data, optimized_pose}); - ++num_trajectory_nodes_; + ++data_.num_trajectory_nodes; // Test if the 'insertion_submap.back()' is one we never saw before. - if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 || - std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap != - insertion_submaps.back()) { - // We grow 'submap_data_' as needed. This code assumes that the first + if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || + std::prev(data_.submap_data.EndOfTrajectory(trajectory_id)) + ->data.submap != insertion_submaps.back()) { + // We grow 'data_.submap_data' as needed. This code assumes that the first // time we see a new submap is as 'insertion_submaps.back()'. const SubmapId submap_id = - submap_data_.Append(trajectory_id, InternalSubmapData()); - submap_data_.at(submap_id).submap = insertion_submaps.back(); + data_.submap_data.Append(trajectory_id, InternalSubmapData()); + data_.submap_data.at(submap_id).submap = insertion_submaps.back(); } // We have to check this here, because it might have changed by the time we @@ -141,7 +145,7 @@ void PoseGraph3D::AddWorkItem(const std::function& work_item) { } void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) { - trajectory_connectivity_state_.Add(trajectory_id); + data_.trajectory_connectivity_state.Add(trajectory_id); // Make sure we have a sampler for this trajectory. if (!global_localization_samplers_[trajectory_id]) { global_localization_samplers_[trajectory_id] = @@ -182,7 +186,7 @@ void PoseGraph3D::AddLandmarkData(int trajectory_id, common::MutexLocker locker(&mutex_); AddWorkItem([=]() REQUIRES(mutex_) { for (const auto& observation : landmark_data.landmark_observations) { - landmark_nodes_[observation.id].landmark_observations.emplace_back( + data_.landmark_nodes[observation.id].landmark_observations.emplace_back( PoseGraphInterface::LandmarkNode::LandmarkObservation{ trajectory_id, landmark_data.time, observation.landmark_to_tracking_transform, @@ -193,7 +197,7 @@ void PoseGraph3D::AddLandmarkData(int trajectory_id, void PoseGraph3D::ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) { - CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); + CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); const transform::Rigid3d global_node_pose = optimization_problem_->node_data().at(node_id).global_pose; @@ -205,16 +209,17 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id, global_submap_pose.inverse(); std::vector submap_nodes; - for (const NodeId& submap_node_id : submap_data_.at(submap_id).node_ids) { - submap_nodes.push_back( - TrajectoryNode{trajectory_nodes_.at(submap_node_id).constant_data, - global_submap_pose_inverse * - trajectory_nodes_.at(submap_node_id).global_pose}); + for (const NodeId& submap_node_id : + data_.submap_data.at(submap_id).node_ids) { + submap_nodes.push_back(TrajectoryNode{ + data_.trajectory_nodes.at(submap_node_id).constant_data, + global_submap_pose_inverse * + data_.trajectory_nodes.at(submap_node_id).global_pose}); } const common::Time node_time = GetLatestNodeTime(node_id, submap_id); const common::Time last_connection_time = - trajectory_connectivity_state_.LastConnectionTime( + data_.trajectory_connectivity_state.LastConnectionTime( node_id.trajectory_id, submap_id.trajectory_id); if (node_id.trajectory_id == submap_id.trajectory_id || node_time < @@ -226,9 +231,11 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id, // submap's trajectory, it suffices to do a match constrained to a local // search window. constraint_builder_.MaybeAddConstraint( - submap_id, submap_data_.at(submap_id).submap.get(), node_id, - trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, - global_node_pose, global_submap_pose); + submap_id, + static_cast( + data_.submap_data.at(submap_id).submap.get()), + node_id, data_.trajectory_nodes.at(node_id).constant_data.get(), + submap_nodes, global_node_pose, global_submap_pose); } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { // In this situation, 'global_node_pose' and 'global_submap_pose' have // orientations agreeing on gravity. Their relationship regarding yaw is @@ -236,14 +243,17 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id, // matching procedure in the FastCorrelativeScanMatcher, and the given yaw // is essentially ignored. constraint_builder_.MaybeAddGlobalConstraint( - submap_id, submap_data_.at(submap_id).submap.get(), node_id, - trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, - global_node_pose.rotation(), global_submap_pose.rotation()); + submap_id, + static_cast( + data_.submap_data.at(submap_id).submap.get()), + node_id, data_.trajectory_nodes.at(node_id).constant_data.get(), + submap_nodes, global_node_pose.rotation(), + global_submap_pose.rotation()); } } void PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) { - const auto& submap_data = submap_data_.at(submap_id); + const auto& submap_data = data_.submap_data.at(submap_id); for (const auto& node_id_data : optimization_problem_->node_data()) { const NodeId& node_id = node_id_data.id; if (submap_data.node_ids.count(node_id) == 0) { @@ -256,7 +266,7 @@ void PoseGraph3D::ComputeConstraintsForNode( const NodeId& node_id, std::vector> insertion_submaps, const bool newly_finished_submap) { - const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; + const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data; const std::vector submap_ids = InitializeGlobalSubmapPoses( node_id.trajectory_id, constant_data->time, insertion_submaps); CHECK_EQ(submap_ids.size(), insertion_submaps.size()); @@ -271,12 +281,12 @@ void PoseGraph3D::ComputeConstraintsForNode( 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 - // be marked as finished in 'submap_data_' further below. - CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); - submap_data_.at(submap_id).node_ids.emplace(node_id); + // be marked as finished in 'data_.submap_data' further below. + CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive); + data_.submap_data.at(submap_id).node_ids.emplace(node_id); const transform::Rigid3d constraint_transform = insertion_submaps[i]->local_pose().inverse() * local_pose; - constraints_.push_back( + data_.constraints.push_back( Constraint{submap_id, node_id, {constraint_transform, options_.matcher_translation_weight(), @@ -284,7 +294,7 @@ void PoseGraph3D::ComputeConstraintsForNode( Constraint::INTRA_SUBMAP}); } - for (const auto& submap_id_data : submap_data_) { + for (const auto& submap_id_data : data_.submap_data) { if (submap_id_data.data.state == SubmapState::kFinished) { CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0); ComputeConstraint(node_id, submap_id_data.id); @@ -294,7 +304,7 @@ void PoseGraph3D::ComputeConstraintsForNode( if (newly_finished_submap) { const SubmapId finished_submap_id = submap_ids.front(); InternalSubmapData& finished_submap_data = - submap_data_.at(finished_submap_id); + data_.submap_data.at(finished_submap_id); CHECK(finished_submap_data.state == SubmapState::kActive); finished_submap_data.state = SubmapState::kFinished; // We have a new completed submap, so we look into adding constraints for @@ -322,13 +332,14 @@ void PoseGraph3D::DispatchOptimization() { common::Time PoseGraph3D::GetLatestNodeTime(const NodeId& node_id, const SubmapId& submap_id) const { - common::Time time = trajectory_nodes_.at(node_id).constant_data->time; - const InternalSubmapData& submap_data = submap_data_.at(submap_id); + common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time; + const InternalSubmapData& submap_data = data_.submap_data.at(submap_id); if (!submap_data.node_ids.empty()) { const NodeId last_submap_node_id = - *submap_data_.at(submap_id).node_ids.rbegin(); + *data_.submap_data.at(submap_id).node_ids.rbegin(); time = std::max( - time, trajectory_nodes_.at(last_submap_node_id).constant_data->time); + time, + data_.trajectory_nodes.at(last_submap_node_id).constant_data->time); } return time; } @@ -337,16 +348,17 @@ void PoseGraph3D::UpdateTrajectoryConnectivity(const Constraint& constraint) { CHECK_EQ(constraint.tag, PoseGraphInterface::Constraint::INTER_SUBMAP); const common::Time time = GetLatestNodeTime(constraint.node_id, constraint.submap_id); - trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, - constraint.submap_id.trajectory_id, - time); + data_.trajectory_connectivity_state.Connect( + constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id, + time); } void PoseGraph3D::HandleWorkQueue( const constraints::ConstraintBuilder3D::Result& result) { { common::MutexLocker locker(&mutex_); - constraints_.insert(constraints_.end(), result.begin(), result.end()); + data_.constraints.insert(data_.constraints.end(), result.begin(), + result.end()); } RunOptimization(); @@ -408,18 +420,19 @@ void PoseGraph3D::WaitForAllComputations() { while (!locker.AwaitWithTimeout( [this]() REQUIRES(mutex_) { return ((constraint_builder_.GetNumFinishedNodes() == - num_trajectory_nodes_) && + data_.num_trajectory_nodes) && !work_queue_); }, common::FromSeconds(1.))) { // Log progress on nodes only when we are actually processing nodes. - if (num_trajectory_nodes_ != num_finished_nodes_at_start) { + if (data_.num_trajectory_nodes != num_finished_nodes_at_start) { std::ostringstream progress_info; progress_info << "Optimizing: " << std::fixed << std::setprecision(1) << 100. * (constraint_builder_.GetNumFinishedNodes() - num_finished_nodes_at_start) / - (num_trajectory_nodes_ - num_finished_nodes_at_start) + (data_.num_trajectory_nodes - + num_finished_nodes_at_start) << "%..."; std::cout << "\r\x1b[K" << progress_info.str() << std::flush; } @@ -429,7 +442,8 @@ void PoseGraph3D::WaitForAllComputations() { [this, ¬ification](const constraints::ConstraintBuilder3D::Result& result) { common::MutexLocker locker(&mutex_); - constraints_.insert(constraints_.end(), result.begin(), result.end()); + data_.constraints.insert(data_.constraints.end(), result.begin(), + result.end()); notification = true; }); locker.Await([¬ification]() { return notification; }); @@ -438,11 +452,11 @@ void PoseGraph3D::WaitForAllComputations() { void PoseGraph3D::FinishTrajectory(const int trajectory_id) { common::MutexLocker locker(&mutex_); AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { - CHECK_EQ(finished_trajectories_.count(trajectory_id), 0); - finished_trajectories_.insert(trajectory_id); + CHECK_EQ(data_.finished_trajectories.count(trajectory_id), 0); + data_.finished_trajectories.insert(trajectory_id); - for (const auto& submap : submap_data_.trajectory(trajectory_id)) { - submap_data_.at(submap.id).state = SubmapState::kFinished; + for (const auto& submap : data_.submap_data.trajectory(trajectory_id)) { + data_.submap_data.at(submap.id).state = SubmapState::kFinished; } CHECK(!run_loop_closure_); DispatchOptimization(); @@ -450,20 +464,20 @@ void PoseGraph3D::FinishTrajectory(const int trajectory_id) { } bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const { - return finished_trajectories_.count(trajectory_id) > 0; + return data_.finished_trajectories.count(trajectory_id) > 0; } void PoseGraph3D::FreezeTrajectory(const int trajectory_id) { common::MutexLocker locker(&mutex_); - trajectory_connectivity_state_.Add(trajectory_id); + data_.trajectory_connectivity_state.Add(trajectory_id); AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { - CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0); - frozen_trajectories_.insert(trajectory_id); + CHECK_EQ(data_.frozen_trajectories.count(trajectory_id), 0); + data_.frozen_trajectories.insert(trajectory_id); }); } bool PoseGraph3D::IsTrajectoryFrozen(const int trajectory_id) const { - return frozen_trajectories_.count(trajectory_id) > 0; + return data_.frozen_trajectories.count(trajectory_id) > 0; } void PoseGraph3D::AddSubmapFromProto( @@ -479,13 +493,13 @@ void PoseGraph3D::AddSubmapFromProto( common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(submap_id.trajectory_id); - submap_data_.Insert(submap_id, InternalSubmapData()); - submap_data_.at(submap_id).submap = submap_ptr; + data_.submap_data.Insert(submap_id, InternalSubmapData()); + data_.submap_data.at(submap_id).submap = submap_ptr; // Immediately show the submap at the 'global_submap_pose'. - global_submap_poses_.Insert(submap_id, - optimization::SubmapSpec3D{global_submap_pose}); + data_.global_submap_poses_3d.Insert( + submap_id, optimization::SubmapSpec3D{global_submap_pose}); AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) { - submap_data_.at(submap_id).state = SubmapState::kFinished; + data_.submap_data.at(submap_id).state = SubmapState::kFinished; optimization_problem_->InsertSubmap(submap_id, global_submap_pose); }); } @@ -499,10 +513,12 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose, common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(node_id.trajectory_id); - trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose}); + data_.trajectory_nodes.Insert(node_id, + TrajectoryNode{constant_data, global_pose}); AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { - const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; + const auto& constant_data = + data_.trajectory_nodes.at(node_id).constant_data; optimization_problem_->InsertTrajectoryNode( node_id, optimization::NodeSpec3D{constant_data->time, constant_data->local_pose, @@ -533,7 +549,7 @@ void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id, const SubmapId& submap_id) { common::MutexLocker locker(&mutex_); AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) { - submap_data_.at(submap_id).node_ids.insert(node_id); + data_.submap_data.at(submap_id).node_ids.insert(node_id); }); } @@ -542,13 +558,14 @@ void PoseGraph3D::AddSerializedConstraints( common::MutexLocker locker(&mutex_); AddWorkItem([this, constraints]() REQUIRES(mutex_) { for (const auto& constraint : constraints) { - CHECK(trajectory_nodes_.Contains(constraint.node_id)); - CHECK(submap_data_.Contains(constraint.submap_id)); - CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr); - CHECK(submap_data_.at(constraint.submap_id).submap != nullptr); + CHECK(data_.trajectory_nodes.Contains(constraint.node_id)); + CHECK(data_.submap_data.Contains(constraint.submap_id)); + CHECK(data_.trajectory_nodes.at(constraint.node_id).constant_data != + nullptr); + CHECK(data_.submap_data.at(constraint.submap_id).submap != nullptr); switch (constraint.tag) { case Constraint::Tag::INTRA_SUBMAP: - CHECK(submap_data_.at(constraint.submap_id) + CHECK(data_.submap_data.at(constraint.submap_id) .node_ids.emplace(constraint.node_id) .second); break; @@ -556,7 +573,7 @@ void PoseGraph3D::AddSerializedConstraints( UpdateTrajectoryConnectivity(constraint); break; } - constraints_.push_back(constraint); + data_.constraints.push_back(constraint); } LOG(INFO) << "Loaded " << constraints.size() << " constraints."; }); @@ -591,14 +608,14 @@ void PoseGraph3D::RunFinalOptimization() { void PoseGraph3D::LogResidualHistograms() const { common::Histogram rotational_residual; common::Histogram translational_residual; - for (const Constraint& constraint : constraints_) { + for (const Constraint& constraint : data_.constraints) { if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { const cartographer::transform::Rigid3d optimized_node_to_map = - trajectory_nodes_.at(constraint.node_id).global_pose; + data_.trajectory_nodes.at(constraint.node_id).global_pose; const cartographer::transform::Rigid3d node_to_submap_constraint = constraint.pose.zbar_ij; const cartographer::transform::Rigid3d optimized_submap_to_map = - global_submap_poses_.at(constraint.submap_id).global_pose; + data_.global_submap_poses_3d.at(constraint.submap_id).global_pose; const cartographer::transform::Rigid3d optimized_node_to_submap = optimized_submap_to_map.inverse() * optimized_node_to_map; const cartographer::transform::Rigid3d residual = @@ -619,44 +636,45 @@ void PoseGraph3D::RunOptimization() { return; } - // No other thread is accessing the optimization_problem_, constraints_, - // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is - // time consuming, so not taking the mutex before Solve to avoid blocking - // foreground processing. - optimization_problem_->Solve(constraints_, frozen_trajectories_, - landmark_nodes_); + // No other thread is accessing the optimization_problem_, data_.constraints, + // data_.frozen_trajectories and data_.landmark_nodes when executing the + // Solve. Solve is time consuming, so not taking the mutex before Solve to + // avoid blocking foreground processing. + optimization_problem_->Solve(data_.constraints, data_.frozen_trajectories, + data_.landmark_nodes); common::MutexLocker locker(&mutex_); const auto& submap_data = optimization_problem_->submap_data(); const auto& node_data = optimization_problem_->node_data(); for (const int trajectory_id : node_data.trajectory_ids()) { for (const auto& node : node_data.trajectory(trajectory_id)) { - trajectory_nodes_.at(node.id).global_pose = node.data.global_pose; + data_.trajectory_nodes.at(node.id).global_pose = node.data.global_pose; } // Extrapolate all point cloud poses that were not included in the // 'optimization_problem_' yet. const auto local_to_new_global = ComputeLocalToGlobalTransform(submap_data, trajectory_id); - const auto local_to_old_global = - ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); + const auto local_to_old_global = ComputeLocalToGlobalTransform( + data_.global_submap_poses_3d, trajectory_id); const transform::Rigid3d old_global_to_new_global = local_to_new_global * local_to_old_global.inverse(); const NodeId last_optimized_node_id = std::prev(node_data.EndOfTrajectory(trajectory_id))->id; - auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id)); - for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); + auto node_it = + std::next(data_.trajectory_nodes.find(last_optimized_node_id)); + for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id); ++node_it) { - auto& mutable_trajectory_node = trajectory_nodes_.at(node_it->id); + auto& mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id); mutable_trajectory_node.global_pose = old_global_to_new_global * mutable_trajectory_node.global_pose; } } for (const auto& landmark : optimization_problem_->landmark_data()) { - landmark_nodes_[landmark.first].global_landmark_pose = landmark.second; + data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second; } - global_submap_poses_ = submap_data; + data_.global_submap_poses_3d = submap_data; // Log the histograms for the pose residuals. if (options_.log_residual_histograms()) { @@ -666,14 +684,14 @@ void PoseGraph3D::RunOptimization() { MapById PoseGraph3D::GetTrajectoryNodes() const { common::MutexLocker locker(&mutex_); - return trajectory_nodes_; + return data_.trajectory_nodes; } MapById PoseGraph3D::GetTrajectoryNodePoses() const { MapById node_poses; common::MutexLocker locker(&mutex_); - for (const auto& node_id_data : trajectory_nodes_) { + for (const auto& node_id_data : data_.trajectory_nodes) { common::optional constant_pose_data; if (node_id_data.data.constant_data != nullptr) { constant_pose_data = TrajectoryNodePose::ConstantPoseData{ @@ -691,7 +709,7 @@ std::map PoseGraph3D::GetLandmarkPoses() const { std::map landmark_poses; common::MutexLocker locker(&mutex_); - for (const auto& landmark : landmark_nodes_) { + for (const auto& landmark : data_.landmark_nodes) { // Landmark without value has not been optimized yet. if (!landmark.second.global_landmark_pose.has_value()) continue; landmark_poses[landmark.first] = @@ -704,7 +722,7 @@ void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id, const transform::Rigid3d& global_pose) { common::MutexLocker locker(&mutex_); AddWorkItem([=]() REQUIRES(mutex_) { - landmark_nodes_[landmark_id].global_landmark_pose = global_pose; + data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; }); } @@ -727,7 +745,7 @@ PoseGraph3D::GetFixedFramePoseData() const { std::map PoseGraph3D::GetLandmarkNodes() const { common::MutexLocker locker(&mutex_); - return landmark_nodes_; + return data_.landmark_nodes; } std::map @@ -738,7 +756,7 @@ PoseGraph3D::GetTrajectoryData() const { std::vector PoseGraph3D::constraints() const { common::MutexLocker locker(&mutex_); - return constraints_; + return data_.constraints; } void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id, @@ -746,19 +764,20 @@ void PoseGraph3D::SetInitialTrajectoryPose(const int from_trajectory_id, const transform::Rigid3d& pose, const common::Time time) { common::MutexLocker locker(&mutex_); - initial_trajectory_poses_[from_trajectory_id] = + data_.initial_trajectory_poses[from_trajectory_id] = InitialTrajectoryPose{to_trajectory_id, pose, time}; } transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose( const int trajectory_id, const common::Time time) const { - CHECK_GT(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id), 0); - const auto it = trajectory_nodes_.lower_bound(trajectory_id, time); - if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) { - return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose; + CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0); + const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time); + if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)) { + return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id) + ->data.global_pose; } - if (it == trajectory_nodes_.EndOfTrajectory(trajectory_id)) { - return std::prev(trajectory_nodes_.EndOfTrajectory(trajectory_id)) + if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) { + return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) ->data.global_pose; } return transform::Interpolate( @@ -773,11 +792,12 @@ transform::Rigid3d PoseGraph3D::GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d PoseGraph3D::GetLocalToGlobalTransform( const int trajectory_id) const { common::MutexLocker locker(&mutex_); - return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); + return ComputeLocalToGlobalTransform(data_.global_submap_poses_3d, + trajectory_id); } std::vector> PoseGraph3D::GetConnectedTrajectories() const { - return trajectory_connectivity_state_.Components(); + return data_.trajectory_connectivity_state.Components(); } PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapData( @@ -796,7 +816,7 @@ MapById PoseGraph3D::GetAllSubmapPoses() const { common::MutexLocker locker(&mutex_); MapById submap_poses; - for (const auto& submap_id_data : submap_data_) { + for (const auto& submap_id_data : data_.submap_data) { auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); submap_poses.Insert( submap_id_data.id, @@ -812,8 +832,8 @@ transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform( auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id); if (begin_it == end_it) { - const auto it = initial_trajectory_poses_.find(trajectory_id); - if (it != initial_trajectory_poses_.end()) { + const auto it = data_.initial_trajectory_poses.find(trajectory_id); + if (it != data_.initial_trajectory_poses.end()) { return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id, it->second.time) * it->second.relative_pose; @@ -824,24 +844,24 @@ transform::Rigid3d PoseGraph3D::ComputeLocalToGlobalTransform( const SubmapId last_optimized_submap_id = std::prev(end_it)->id; // Accessing 'local_pose' in Submap is okay, since the member is const. return global_submap_poses.at(last_optimized_submap_id).global_pose * - submap_data_.at(last_optimized_submap_id) + data_.submap_data.at(last_optimized_submap_id) .submap->local_pose() .inverse(); } PoseGraphInterface::SubmapData PoseGraph3D::GetSubmapDataUnderLock( const SubmapId& submap_id) const { - const auto it = submap_data_.find(submap_id); - if (it == submap_data_.end()) { + const auto it = data_.submap_data.find(submap_id); + if (it == data_.submap_data.end()) { return {}; } auto submap = it->data.submap; - if (global_submap_poses_.Contains(submap_id)) { + if (data_.global_submap_poses_3d.Contains(submap_id)) { // We already have an optimized pose. - return {submap, global_submap_poses_.at(submap_id).global_pose}; + return {submap, data_.global_submap_poses_3d.at(submap_id).global_pose}; } // We have to extrapolate. - return {submap, ComputeLocalToGlobalTransform(global_submap_poses_, + return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_3d, submap_id.trajectory_id) * submap->local_pose()}; } @@ -866,28 +886,28 @@ std::vector PoseGraph3D::TrimmingHandle::GetSubmapIds( MapById PoseGraph3D::TrimmingHandle::GetOptimizedSubmapData() const { MapById submaps; - for (const auto& submap_id_data : parent_->submap_data_) { + for (const auto& submap_id_data : parent_->data_.submap_data) { if (submap_id_data.data.state != SubmapState::kFinished || - !parent_->global_submap_poses_.Contains(submap_id_data.id)) { + !parent_->data_.global_submap_poses_3d.Contains(submap_id_data.id)) { continue; } submaps.Insert( submap_id_data.id, - SubmapData{ - submap_id_data.data.submap, - parent_->global_submap_poses_.at(submap_id_data.id).global_pose}); + SubmapData{submap_id_data.data.submap, + parent_->data_.global_submap_poses_3d.at(submap_id_data.id) + .global_pose}); } return submaps; } const MapById& PoseGraph3D::TrimmingHandle::GetTrajectoryNodes() const { - return parent_->trajectory_nodes_; + return parent_->data_.trajectory_nodes; } const std::vector& PoseGraph3D::TrimmingHandle::GetConstraints() const { - return parent_->constraints_; + return parent_->data_.constraints; } bool PoseGraph3D::TrimmingHandle::IsFinished(const int trajectory_id) const { @@ -898,22 +918,23 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed( const SubmapId& submap_id) { // TODO(hrapp): We have to make sure that the trajectory has been finished // if we want to delete the last submaps. - CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); + CHECK(parent_->data_.submap_data.at(submap_id).state == + SubmapState::kFinished); // Compile all nodes that are still INTRA_SUBMAP constrained once the submap // with 'submap_id' is gone. std::set nodes_to_retain; - for (const Constraint& constraint : parent_->constraints_) { + for (const Constraint& constraint : parent_->data_.constraints) { if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && constraint.submap_id != submap_id) { nodes_to_retain.insert(constraint.node_id); } } - // Remove all 'constraints_' related to 'submap_id'. + // Remove all 'data_.constraints' related to 'submap_id'. std::set nodes_to_remove; { std::vector constraints; - for (const Constraint& constraint : parent_->constraints_) { + for (const Constraint& constraint : parent_->data_.constraints) { if (constraint.submap_id == submap_id) { if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && nodes_to_retain.count(constraint.node_id) == 0) { @@ -925,29 +946,30 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed( constraints.push_back(constraint); } } - parent_->constraints_ = std::move(constraints); + parent_->data_.constraints = std::move(constraints); } - // Remove all 'constraints_' related to 'nodes_to_remove'. + // Remove all 'data_.constraints' related to 'nodes_to_remove'. { std::vector constraints; - for (const Constraint& constraint : parent_->constraints_) { + for (const Constraint& constraint : parent_->data_.constraints) { if (nodes_to_remove.count(constraint.node_id) == 0) { constraints.push_back(constraint); } } - parent_->constraints_ = std::move(constraints); + parent_->data_.constraints = std::move(constraints); } // Mark the submap with 'submap_id' as trimmed and remove its data. - CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); - parent_->submap_data_.Trim(submap_id); + CHECK(parent_->data_.submap_data.at(submap_id).state == + SubmapState::kFinished); + parent_->data_.submap_data.Trim(submap_id); parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->optimization_problem_->TrimSubmap(submap_id); // Remove the 'nodes_to_remove' from the pose graph and the optimization // problem. for (const NodeId& node_id : nodes_to_remove) { - parent_->trajectory_nodes_.Trim(node_id); + parent_->data_.trajectory_nodes.Trim(node_id); parent_->optimization_problem_->TrimTrajectoryNode(node_id); } } @@ -955,7 +977,7 @@ void PoseGraph3D::TrimmingHandle::MarkSubmapAsTrimmed( MapById PoseGraph3D::GetSubmapDataUnderLock() const { MapById submaps; - for (const auto& submap_id_data : submap_data_) { + for (const auto& submap_id_data : data_.submap_data) { submaps.Insert(submap_id_data.id, GetSubmapDataUnderLock(submap_id_data.id)); } diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.h b/cartographer/mapping/internal/3d/pose_graph_3d.h index 24ce57e..9e0bfef 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.h +++ b/cartographer/mapping/internal/3d/pose_graph_3d.h @@ -37,6 +37,7 @@ #include "cartographer/mapping/internal/optimization/optimization_problem_3d.h" #include "cartographer/mapping/internal/trajectory_connectivity_state.h" #include "cartographer/mapping/pose_graph.h" +#include "cartographer/mapping/pose_graph_data.h" #include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/landmark_data.h" @@ -150,21 +151,6 @@ class PoseGraph3D : public PoseGraph { void WaitForAllComputations() EXCLUDES(mutex_); private: - // The current state of the submap in the background threads. When this - // transitions to kFinished, all nodes are tried to match against this submap. - // Likewise, all new nodes are matched against submaps which are finished. - enum class SubmapState { kActive, kFinished }; - struct InternalSubmapData { - std::shared_ptr submap; - - // IDs of the nodes that were inserted into this map together with - // constraints for them. They are not to be matched again when this submap - // becomes 'finished'. - std::set node_ids; - - SubmapState state = SubmapState::kActive; - }; - MapById GetSubmapDataUnderLock() const REQUIRES(mutex_); // Handles a new work item. @@ -235,9 +221,6 @@ class PoseGraph3D : public PoseGraph { std::unique_ptr>> work_queue_ GUARDED_BY(mutex_); - // How our various trajectories are related. - TrajectoryConnectivityState trajectory_connectivity_state_; - // We globally localize a fraction of the nodes from each trajectory. std::unordered_map> global_localization_samplers_ GUARDED_BY(mutex_); @@ -251,36 +234,11 @@ class PoseGraph3D : public PoseGraph { // Current optimization problem. std::unique_ptr optimization_problem_; constraints::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_); - std::vector constraints_ GUARDED_BY(mutex_); - - // Submaps get assigned an ID and state as soon as they are seen, even - // before they take part in the background computations. - MapById submap_data_ GUARDED_BY(mutex_); - - // Data that are currently being shown. - MapById trajectory_nodes_ GUARDED_BY(mutex_); - int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; - - // Global submap poses currently used for displaying data. - MapById global_submap_poses_ - GUARDED_BY(mutex_); - - // Global landmark poses with all observations. - std::map - landmark_nodes_ GUARDED_BY(mutex_); // List of all trimmers to consult when optimizations finish. std::vector> trimmers_ GUARDED_BY(mutex_); - // Set of all frozen trajectories not being optimized. - std::set frozen_trajectories_ GUARDED_BY(mutex_); - - // Set of all finished trajectories. - std::set finished_trajectories_ GUARDED_BY(mutex_); - - // Set of all initial trajectory poses. - std::map initial_trajectory_poses_ - GUARDED_BY(mutex_); + PoseGraphData data_ GUARDED_BY(mutex_); // Allows querying and manipulating the pose graph by the 'trimmers_'. The // 'mutex_' of the pose graph is held while this class is used.