diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 23d16a0..0143753 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -61,27 +61,29 @@ std::vector PoseGraph2D::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, - transform::Project2D(ComputeLocalToGlobalTransform( - global_submap_poses_, trajectory_id) * - insertion_submaps[0]->local_pose())); + trajectory_id, transform::Project2D( + ComputeLocalToGlobalTransform( + data_.global_submap_poses_2d, 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; @@ -93,10 +95,12 @@ std::vector PoseGraph2D::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}; } @@ -109,19 +113,19 @@ NodeId PoseGraph2D::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 @@ -143,7 +147,7 @@ void PoseGraph2D::AddWorkItem(const std::function& work_item) { } void PoseGraph2D::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] = @@ -180,7 +184,7 @@ void PoseGraph2D::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( LandmarkNode::LandmarkObservation{ trajectory_id, landmark_data.time, observation.landmark_to_tracking_transform, @@ -191,11 +195,11 @@ void PoseGraph2D::AddLandmarkData(int trajectory_id, void PoseGraph2D::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 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 < @@ -212,18 +216,22 @@ void PoseGraph2D::ComputeConstraint(const NodeId& node_id, .global_pose.inverse() * optimization_problem_->node_data().at(node_id).global_pose_2d; constraint_builder_.MaybeAddConstraint( - submap_id, submap_data_.at(submap_id).submap.get(), node_id, - trajectory_nodes_.at(node_id).constant_data.get(), + submap_id, + static_cast( + data_.submap_data.at(submap_id).submap.get()), + node_id, data_.trajectory_nodes.at(node_id).constant_data.get(), initial_relative_pose); } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( - submap_id, submap_data_.at(submap_id).submap.get(), node_id, - trajectory_nodes_.at(node_id).constant_data.get()); + submap_id, + static_cast( + data_.submap_data.at(submap_id).submap.get()), + node_id, data_.trajectory_nodes.at(node_id).constant_data.get()); } } void PoseGraph2D::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) { @@ -236,7 +244,7 @@ void PoseGraph2D::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()); @@ -256,21 +264,22 @@ void PoseGraph2D::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); + // only 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::Rigid2d constraint_transform = constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() * local_pose_2d; - constraints_.push_back(Constraint{submap_id, - node_id, - {transform::Embed3D(constraint_transform), - options_.matcher_translation_weight(), - options_.matcher_rotation_weight()}, - Constraint::INTRA_SUBMAP}); + data_.constraints.push_back( + Constraint{submap_id, + node_id, + {transform::Embed3D(constraint_transform), + options_.matcher_translation_weight(), + options_.matcher_rotation_weight()}, + 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); @@ -280,7 +289,7 @@ void PoseGraph2D::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 @@ -307,13 +316,14 @@ void PoseGraph2D::DispatchOptimization() { } common::Time PoseGraph2D::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; } @@ -322,16 +332,17 @@ void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) { CHECK_EQ(constraint.tag, 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 PoseGraph2D::HandleWorkQueue( const constraints::ConstraintBuilder2D::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(); @@ -393,18 +404,19 @@ void PoseGraph2D::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; } @@ -414,7 +426,8 @@ void PoseGraph2D::WaitForAllComputations() { [this, ¬ification](const constraints::ConstraintBuilder2D::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; }); @@ -423,11 +436,11 @@ void PoseGraph2D::WaitForAllComputations() { void PoseGraph2D::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(); @@ -435,20 +448,20 @@ void PoseGraph2D::FinishTrajectory(const int trajectory_id) { } bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const { - return finished_trajectories_.count(trajectory_id) > 0; + return data_.finished_trajectories.count(trajectory_id) > 0; } void PoseGraph2D::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 PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const { - return frozen_trajectories_.count(trajectory_id) > 0; + return data_.frozen_trajectories.count(trajectory_id) > 0; } void PoseGraph2D::AddSubmapFromProto( @@ -466,13 +479,13 @@ void PoseGraph2D::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( + data_.global_submap_poses_2d.Insert( submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); AddWorkItem([this, submap_id, global_submap_pose_2d]() 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_2d); }); } @@ -486,10 +499,12 @@ void PoseGraph2D::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; const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( constant_data->gravity_alignment.inverse()); optimization_problem_->InsertTrajectoryNode( @@ -512,7 +527,7 @@ void PoseGraph2D::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); }); } @@ -521,13 +536,14 @@ void PoseGraph2D::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; @@ -538,10 +554,10 @@ void PoseGraph2D::AddSerializedConstraints( const Constraint::Pose pose = { constraint.pose.zbar_ij * transform::Rigid3d::Rotation( - trajectory_nodes_.at(constraint.node_id) + data_.trajectory_nodes.at(constraint.node_id) .constant_data->gravity_alignment.inverse()), constraint.pose.translation_weight, constraint.pose.rotation_weight}; - constraints_.push_back(Constraint{ + data_.constraints.push_back(Constraint{ constraint.submap_id, constraint.node_id, pose, constraint.tag}); } LOG(INFO) << "Loaded " << constraints.size() << " constraints."; @@ -579,19 +595,19 @@ void PoseGraph2D::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)) { - auto& mutable_trajectory_node = trajectory_nodes_.at(node.id); + auto& mutable_trajectory_node = data_.trajectory_nodes.at(node.id); mutable_trajectory_node.global_pose = transform::Embed3D(node.data.global_pose_2d) * transform::Rigid3d::Rotation( @@ -602,37 +618,38 @@ void PoseGraph2D::RunOptimization() { // '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_2d, 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_2d = submap_data; } MapById PoseGraph2D::GetTrajectoryNodes() const { common::MutexLocker locker(&mutex_); - return trajectory_nodes_; + return data_.trajectory_nodes; } MapById PoseGraph2D::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{ @@ -650,7 +667,7 @@ std::map PoseGraph2D::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] = @@ -663,7 +680,7 @@ void PoseGraph2D::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; }); } @@ -680,7 +697,7 @@ sensor::MapByTime PoseGraph2D::GetOdometryData() const { std::map PoseGraph2D::GetLandmarkNodes() const { common::MutexLocker locker(&mutex_); - return landmark_nodes_; + return data_.landmark_nodes; } std::map @@ -696,12 +713,12 @@ PoseGraph2D::GetFixedFramePoseData() const { std::vector PoseGraph2D::constraints() const { std::vector result; common::MutexLocker locker(&mutex_); - for (const Constraint& constraint : constraints_) { + for (const Constraint& constraint : data_.constraints) { result.push_back(Constraint{ constraint.submap_id, constraint.node_id, Constraint::Pose{constraint.pose.zbar_ij * transform::Rigid3d::Rotation( - trajectory_nodes_.at(constraint.node_id) + data_.trajectory_nodes.at(constraint.node_id) .constant_data->gravity_alignment), constraint.pose.translation_weight, constraint.pose.rotation_weight}, @@ -715,19 +732,20 @@ void PoseGraph2D::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 PoseGraph2D::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( @@ -742,11 +760,12 @@ transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform( const int trajectory_id) const { common::MutexLocker locker(&mutex_); - return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); + return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, + trajectory_id); } std::vector> PoseGraph2D::GetConnectedTrajectories() const { - return trajectory_connectivity_state_.Components(); + return data_.trajectory_connectivity_state.Components(); } PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData( @@ -765,7 +784,7 @@ MapById PoseGraph2D::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, @@ -781,8 +800,8 @@ transform::Rigid3d PoseGraph2D::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; @@ -794,25 +813,26 @@ transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform( // Accessing 'local_pose' in Submap is okay, since the member is const. return transform::Embed3D( 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 PoseGraph2D::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_2d.Contains(submap_id)) { // We already have an optimized pose. return {submap, - transform::Embed3D(global_submap_poses_.at(submap_id).global_pose)}; + transform::Embed3D( + data_.global_submap_poses_2d.at(submap_id).global_pose)}; } // We have to extrapolate. - return {submap, ComputeLocalToGlobalTransform(global_submap_poses_, + return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, submap_id.trajectory_id) * submap->local_pose()}; } @@ -828,16 +848,17 @@ int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const { MapById PoseGraph2D::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_2d.Contains(submap_id_data.id)) { continue; } - submaps.Insert(submap_id_data.id, - SubmapData{submap_id_data.data.submap, - transform::Embed3D(parent_->global_submap_poses_ - .at(submap_id_data.id) - .global_pose)}); + submaps.Insert( + submap_id_data.id, + SubmapData{submap_id_data.data.submap, + transform::Embed3D(parent_->data_.global_submap_poses_2d + .at(submap_id_data.id) + .global_pose)}); } return submaps; } @@ -854,12 +875,12 @@ std::vector PoseGraph2D::TrimmingHandle::GetSubmapIds( const MapById& PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const { - return parent_->trajectory_nodes_; + return parent_->data_.trajectory_nodes; } const std::vector& PoseGraph2D::TrimmingHandle::GetConstraints() const { - return parent_->constraints_; + return parent_->data_.constraints; } bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const { @@ -870,22 +891,23 @@ void PoseGraph2D::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) { @@ -897,29 +919,30 @@ void PoseGraph2D::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); } } @@ -927,7 +950,7 @@ void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed( MapById PoseGraph2D::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/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 83c8a71..c211cf6 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -37,6 +37,7 @@ #include "cartographer/mapping/internal/optimization/optimization_problem_2d.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" @@ -146,21 +147,6 @@ class PoseGraph2D : public PoseGraph { int trajectory_id, const common::Time time) const REQUIRES(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_); @@ -229,9 +215,6 @@ class PoseGraph2D : 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_); @@ -248,36 +231,11 @@ class PoseGraph2D : public PoseGraph { // Current optimization problem. std::unique_ptr optimization_problem_; constraints::ConstraintBuilder2D 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. diff --git a/cartographer/mapping/pose_graph_data.h b/cartographer/mapping/pose_graph_data.h new file mode 100644 index 0000000..b88333e --- /dev/null +++ b/cartographer/mapping/pose_graph_data.h @@ -0,0 +1,79 @@ +/* + * Copyright 2018 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_ +#define CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_ + +#include +#include +#include + +#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h" +#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_interface.h" +#include "cartographer/mapping/submaps.h" + +namespace cartographer { +namespace mapping { + +// 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; + SubmapState state = SubmapState::kActive; + + // 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; +}; + +struct PoseGraphData { + // 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; + + // Global submap poses currently used for displaying data. + MapById global_submap_poses_2d; + MapById global_submap_poses_3d; + + // Data that are currently being shown. + MapById trajectory_nodes; + + // Global landmark poses with all observations. + std::map + landmark_nodes; + + // How our various trajectories are related. + TrajectoryConnectivityState trajectory_connectivity_state; + int num_trajectory_nodes = 0; + std::set finished_trajectories; + std::set frozen_trajectories; + + // Set of all initial trajectory poses. + std::map initial_trajectory_poses; + + std::vector constraints; +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_DATA_H_