diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index d728666..f621c59 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -106,13 +106,11 @@ std::vector PoseGraph2D::InitializeGlobalSubmapPoses( return {front_submap_id, last_submap_id}; } -NodeId PoseGraph2D::AddNode( +NodeId PoseGraph2D::AppendNode( std::shared_ptr constant_data, const int trajectory_id, - const std::vector>& insertion_submaps) { - const transform::Rigid3d optimized_pose( - GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); - + const std::vector>& insertion_submaps, + const transform::Rigid3d& optimized_pose) { common::MutexLocker locker(&mutex_); AddTrajectoryIfNeeded(trajectory_id); if (!CanAddWorkItemModifying(trajectory_id)) { @@ -121,7 +119,6 @@ NodeId PoseGraph2D::AddNode( const NodeId node_id = data_.trajectory_nodes.Append( trajectory_id, TrajectoryNode{constant_data, optimized_pose}); ++data_.num_trajectory_nodes; - // Test if the 'insertion_submap.back()' is one we never saw before. if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || std::prev(data_.submap_data.EndOfTrajectory(trajectory_id)) @@ -133,11 +130,22 @@ NodeId PoseGraph2D::AddNode( data_.submap_data.at(submap_id).submap = insertion_submaps.back(); LOG(INFO) << "Inserted submap " << submap_id << "."; } + return node_id; +} +NodeId PoseGraph2D::AddNode( + std::shared_ptr constant_data, + const int trajectory_id, + const std::vector>& insertion_submaps) { + const transform::Rigid3d optimized_pose( + GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); + + const NodeId node_id = AppendNode(constant_data, trajectory_id, + insertion_submaps, optimized_pose); // We have to check this here, because it might have changed by the time we // execute the lambda. const bool newly_finished_submap = insertion_submaps.front()->finished(); - AddWorkItem([=]() REQUIRES(mutex_) { + AddWorkItem([=]() EXCLUDES(mutex_) { return ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); }); @@ -146,19 +154,25 @@ NodeId PoseGraph2D::AddNode( void PoseGraph2D::AddWorkItem( const std::function& work_item) { - if (work_queue_ == nullptr) { - if (work_item() == WorkItem::Result::kRunOptimization) { - work_queue_ = common::make_unique(); - constraint_builder_.WhenDone( - [this](const constraints::ConstraintBuilder2D::Result& result) { - HandleWorkQueue(result); - }); + { + common::MutexLocker locker(&mutex_); + if (work_queue_ != nullptr) { + const auto now = std::chrono::steady_clock::now(); + work_queue_->push_back({now, work_item}); + kWorkQueueDelayMetric->Set( + common::ToSeconds(now - work_queue_->front().time)); + return; } - } else { - const auto now = std::chrono::steady_clock::now(); - work_queue_->push_back({now, work_item}); - kWorkQueueDelayMetric->Set( - common::ToSeconds(now - work_queue_->front().time)); + } + if (work_item() == WorkItem::Result::kRunOptimization) { + { + common::MutexLocker locker(&mutex_); + work_queue_ = common::make_unique(); + } + constraint_builder_.WhenDone( + [this](const constraints::ConstraintBuilder2D::Result& result) { + HandleWorkQueue(result); + }); } } @@ -181,20 +195,22 @@ void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph2D::AddImuData(const int trajectory_id, const sensor::ImuData& imu_data) { - common::MutexLocker locker(&mutex_); - if (!CanAddWorkItemModifying(trajectory_id)) return; - AddWorkItem([=]() REQUIRES(mutex_) { - optimization_problem_->AddImuData(trajectory_id, imu_data); + AddWorkItem([=]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddImuData(trajectory_id, imu_data); + } return WorkItem::Result::kDoNotRunOptimization; }); } void PoseGraph2D::AddOdometryData(const int trajectory_id, const sensor::OdometryData& odometry_data) { - common::MutexLocker locker(&mutex_); - if (!CanAddWorkItemModifying(trajectory_id)) return; - AddWorkItem([=]() REQUIRES(mutex_) { - optimization_problem_->AddOdometryData(trajectory_id, odometry_data); + AddWorkItem([=]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + optimization_problem_->AddOdometryData(trajectory_id, odometry_data); + } return WorkItem::Result::kDoNotRunOptimization; }); } @@ -207,15 +223,16 @@ void PoseGraph2D::AddFixedFramePoseData( void PoseGraph2D::AddLandmarkData(int trajectory_id, const sensor::LandmarkData& landmark_data) { - common::MutexLocker locker(&mutex_); - if (!CanAddWorkItemModifying(trajectory_id)) return; - AddWorkItem([=]() REQUIRES(mutex_) { - for (const auto& observation : landmark_data.landmark_observations) { - data_.landmark_nodes[observation.id].landmark_observations.emplace_back( - LandmarkNode::LandmarkObservation{ - trajectory_id, landmark_data.time, - observation.landmark_to_tracking_transform, - observation.translation_weight, observation.rotation_weight}); + AddWorkItem([=]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); + if (CanAddWorkItemModifying(trajectory_id)) { + for (const auto& observation : landmark_data.landmark_observations) { + data_.landmark_nodes[observation.id].landmark_observations.emplace_back( + PoseGraphInterface::LandmarkNode::LandmarkObservation{ + trajectory_id, landmark_data.time, + observation.landmark_to_tracking_transform, + observation.translation_weight, observation.rotation_weight}); + } } return WorkItem::Result::kDoNotRunOptimization; }); @@ -272,6 +289,7 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode( const NodeId& node_id, std::vector> insertion_submaps, const bool newly_finished_submap) { + common::MutexLocker locker(&mutex_); 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); @@ -409,34 +427,43 @@ void PoseGraph2D::HandleWorkQueue( trajectory_id_to_last_optimized_node_id); } - common::MutexLocker locker(&mutex_); - for (const Constraint& constraint : result) { - UpdateTrajectoryConnectivity(constraint); - } - DeleteTrajectoriesIfNeeded(); - TrimmingHandle trimming_handle(this); - for (auto& trimmer : trimmers_) { - trimmer->Trim(&trimming_handle); - } - trimmers_.erase( - std::remove_if(trimmers_.begin(), trimmers_.end(), - [](std::unique_ptr& trimmer) { - return trimmer->IsFinished(); - }), - trimmers_.end()); + { + common::MutexLocker locker(&mutex_); + for (const Constraint& constraint : result) { + UpdateTrajectoryConnectivity(constraint); + } + DeleteTrajectoriesIfNeeded(); + TrimmingHandle trimming_handle(this); + for (auto& trimmer : trimmers_) { + trimmer->Trim(&trimming_handle); + } + trimmers_.erase( + std::remove_if(trimmers_.begin(), trimmers_.end(), + [](std::unique_ptr& trimmer) { + return trimmer->IsFinished(); + }), + trimmers_.end()); - num_nodes_since_last_loop_closure_ = 0; + num_nodes_since_last_loop_closure_ = 0; + } + + size_t work_queue_size; bool process_work_queue = true; while (process_work_queue) { - if (work_queue_->empty()) { - work_queue_.reset(); - return; + std::function work_item; + { + common::MutexLocker locker(&mutex_); + if (work_queue_->empty()) { + work_queue_.reset(); + return; + } + work_item = work_queue_->front().task; + work_queue_->pop_front(); + work_queue_size = work_queue_->size(); } - process_work_queue = - work_queue_->front().task() == WorkItem::Result::kDoNotRunOptimization; - work_queue_->pop_front(); + process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization; } - LOG(INFO) << "Remaining work items in queue: " << work_queue_->size(); + LOG(INFO) << "Remaining work items in queue: " << work_queue_size; // We have to optimize again. constraint_builder_.WhenDone( [this](const constraints::ConstraintBuilder2D::Result& result) { @@ -482,16 +509,19 @@ void PoseGraph2D::WaitForAllComputations() { } void PoseGraph2D::DeleteTrajectory(const int trajectory_id) { - common::MutexLocker locker(&mutex_); - auto it = data_.trajectories_state.find(trajectory_id); - if (it == data_.trajectories_state.end()) { - LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " - << trajectory_id; - return; + { + common::MutexLocker locker(&mutex_); + auto it = data_.trajectories_state.find(trajectory_id); + if (it == data_.trajectories_state.end()) { + LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " + << trajectory_id; + return; + } + it->second.deletion_state = + InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION; } - it->second.deletion_state = - InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION; - AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { + AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); CHECK(data_.trajectories_state.at(trajectory_id).state != TrajectoryState::ACTIVE); CHECK(data_.trajectories_state.at(trajectory_id).state != @@ -505,8 +535,8 @@ void PoseGraph2D::DeleteTrajectory(const int trajectory_id) { } void PoseGraph2D::FinishTrajectory(const int trajectory_id) { - common::MutexLocker locker(&mutex_); - AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { + AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); CHECK(!IsTrajectoryFinished(trajectory_id)); data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED; @@ -524,9 +554,12 @@ bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const { } void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { - common::MutexLocker locker(&mutex_); - data_.trajectory_connectivity_state.Add(trajectory_id); - AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { + { + common::MutexLocker locker(&mutex_); + data_.trajectory_connectivity_state.Add(trajectory_id); + } + AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); CHECK(!IsTrajectoryFrozen(trajectory_id)); data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; return WorkItem::Result::kDoNotRunOptimization; @@ -548,21 +581,25 @@ void PoseGraph2D::AddSubmapFromProto( const SubmapId submap_id = {submap.submap_id().trajectory_id(), submap.submap_id().submap_index()}; - common::MutexLocker locker(&mutex_); - std::shared_ptr submap_ptr = - std::make_shared(submap.submap_2d(), &conversion_tables_); const transform::Rigid2d global_submap_pose_2d = transform::Project2D(global_submap_pose); - AddTrajectoryIfNeeded(submap_id.trajectory_id); - if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; - 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'. - data_.global_submap_poses_2d.Insert( - submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); - bool finished = submap.submap_2d().finished(); + { + common::MutexLocker locker(&mutex_); + const std::shared_ptr submap_ptr = + std::make_shared(submap.submap_2d(), + &conversion_tables_); + AddTrajectoryIfNeeded(submap_id.trajectory_id); + if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; + 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'. + data_.global_submap_poses_2d.Insert( + submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); + } + const bool finished = submap.submap_2d().finished(); AddWorkItem( - [this, submap_id, global_submap_pose_2d, finished]() REQUIRES(mutex_) { + [this, submap_id, global_submap_pose_2d, finished]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); data_.submap_data.at(submap_id).state = finished ? SubmapState::kFinished : SubmapState::kActive; optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); @@ -577,13 +614,16 @@ void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose, std::shared_ptr constant_data = std::make_shared(FromProto(node.node_data())); - common::MutexLocker locker(&mutex_); - AddTrajectoryIfNeeded(node_id.trajectory_id); - if (!CanAddWorkItemModifying(node_id.trajectory_id)) return; - data_.trajectory_nodes.Insert(node_id, - TrajectoryNode{constant_data, global_pose}); + { + common::MutexLocker locker(&mutex_); + AddTrajectoryIfNeeded(node_id.trajectory_id); + if (!CanAddWorkItemModifying(node_id.trajectory_id)) return; + data_.trajectory_nodes.Insert(node_id, + TrajectoryNode{constant_data, global_pose}); + } - AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { + AddWorkItem([this, node_id, global_pose]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data; const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( @@ -607,18 +647,19 @@ void PoseGraph2D::SetTrajectoryDataFromProto( void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, const SubmapId& submap_id) { - common::MutexLocker locker(&mutex_); - if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; - AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) { - data_.submap_data.at(submap_id).node_ids.insert(node_id); + AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); + if (CanAddWorkItemModifying(submap_id.trajectory_id)) { + data_.submap_data.at(submap_id).node_ids.insert(node_id); + } return WorkItem::Result::kDoNotRunOptimization; }); } void PoseGraph2D::AddSerializedConstraints( const std::vector& constraints) { - common::MutexLocker locker(&mutex_); - AddWorkItem([this, constraints]() REQUIRES(mutex_) { + AddWorkItem([this, constraints]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); for (const auto& constraint : constraints) { CHECK(data_.trajectory_nodes.Contains(constraint.node_id)); CHECK(data_.submap_data.Contains(constraint.submap_id)); @@ -650,10 +691,10 @@ void PoseGraph2D::AddSerializedConstraints( } void PoseGraph2D::AddTrimmer(std::unique_ptr trimmer) { - common::MutexLocker locker(&mutex_); // C++11 does not allow us to move a unique_ptr into a lambda. PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); - AddWorkItem([this, trimmer_ptr]() REQUIRES(mutex_) { + AddWorkItem([this, trimmer_ptr]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); trimmers_.emplace_back(trimmer_ptr); return WorkItem::Result::kDoNotRunOptimization; }); @@ -661,13 +702,14 @@ void PoseGraph2D::AddTrimmer(std::unique_ptr trimmer) { void PoseGraph2D::RunFinalOptimization() { { - common::MutexLocker locker(&mutex_); - AddWorkItem([this]() REQUIRES(mutex_) { + AddWorkItem([this]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); optimization_problem_->SetMaxNumIterations( options_.max_num_final_iterations()); return WorkItem::Result::kRunOptimization; }); - AddWorkItem([this]() REQUIRES(mutex_) { + AddWorkItem([this]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); optimization_problem_->SetMaxNumIterations( options_.optimization_problem_options() .ceres_solver_options() @@ -807,8 +849,8 @@ std::map PoseGraph2D::GetLandmarkPoses() void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id, const transform::Rigid3d& global_pose) { - common::MutexLocker locker(&mutex_); - AddWorkItem([=]() REQUIRES(mutex_) { + AddWorkItem([=]() EXCLUDES(mutex_) { + common::MutexLocker locker(&mutex_); data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; return WorkItem::Result::kDoNotRunOptimization; }); diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index 07ffee9..d579585 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -40,6 +40,7 @@ #include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph_data.h" #include "cartographer/mapping/pose_graph_trimmer.h" +#include "cartographer/mapping/value_conversion_tables.h" #include "cartographer/metrics/family_factory.h" #include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/landmark_data.h" @@ -159,11 +160,19 @@ class PoseGraph2D : public PoseGraph { // Handles a new work item. void AddWorkItem(const std::function& work_item) - REQUIRES(mutex_); + EXCLUDES(mutex_); // Adds connectivity and sampler for a trajectory if it does not exist. void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_); + // Appends the new node and submap (if needed) to the internal data + // structures. + NodeId AppendNode( + std::shared_ptr constant_data, + int trajectory_id, + const std::vector>& insertion_submaps, + const transform::Rigid3d& optimized_pose) EXCLUDES(mutex_); + // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. std::vector InitializeGlobalSubmapPoses( @@ -175,7 +184,7 @@ class PoseGraph2D : public PoseGraph { WorkItem::Result ComputeConstraintsForNode( const NodeId& node_id, std::vector> insertion_submaps, - bool newly_finished_submap) REQUIRES(mutex_); + bool newly_finished_submap) EXCLUDES(mutex_); // Computes constraints for a node and submap pair. void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) @@ -191,7 +200,7 @@ class PoseGraph2D : public PoseGraph { // Runs the optimization, executes the trimmers and processes the work queue. void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result) - REQUIRES(mutex_); + EXCLUDES(mutex_); // Waits until we caught up (i.e. nothing is waiting to be scheduled), and // all computations have finished. @@ -237,7 +246,7 @@ class PoseGraph2D : public PoseGraph { // Current optimization problem. std::unique_ptr optimization_problem_; - constraints::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_); + constraints::ConstraintBuilder2D constraint_builder_; // List of all trimmers to consult when optimizations finish. std::vector> trimmers_ GUARDED_BY(mutex_);