12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Take PoseGraph3D mutex inside instead of outside work item (#1264) 

Take PoseGraph3D mutex inside instead of outside work item
    
This refactors the code but does not alter behavior except
that the shared mutex is separately taken for work_queue_ access,
released, and then re-acquired inside each work item.
    
As a consequence AddWorkItem() also needs to EXCLUDE() the lock and
acquire it internally, because for the case where we run the task
in the foreground we cannot hold the lock and recursively acquire it again
inside the task being run.
    
This prepares for making locking more fine-grained, see issue #1250.
master
danielsievers 2018-07-13 11:53:25 +02:00 committed by Wally B. Feed
parent 61a89d8ab8
commit 8c9104568a
2 changed files with 161 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

255
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -104,13 +104,11 @@ std::vector<SubmapId> PoseGraph3D::InitializeGlobalSubmapPoses(
return {front_submap_id, last_submap_id}; return {front_submap_id, last_submap_id};
} }
NodeId PoseGraph3D::AddNode( NodeId PoseGraph3D::AppendNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data, std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps,
const transform::Rigid3d optimized_pose( const transform::Rigid3d& optimized_pose) {
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id); AddTrajectoryIfNeeded(trajectory_id);
if (!CanAddWorkItemModifying(trajectory_id)) { if (!CanAddWorkItemModifying(trajectory_id)) {
@ -119,7 +117,6 @@ NodeId PoseGraph3D::AddNode(
const NodeId node_id = data_.trajectory_nodes.Append( const NodeId node_id = data_.trajectory_nodes.Append(
trajectory_id, TrajectoryNode{constant_data, optimized_pose}); trajectory_id, TrajectoryNode{constant_data, optimized_pose});
++data_.num_trajectory_nodes; ++data_.num_trajectory_nodes;
// Test if the 'insertion_submap.back()' is one we never saw before. // Test if the 'insertion_submap.back()' is one we never saw before.
if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
std::prev(data_.submap_data.EndOfTrajectory(trajectory_id)) std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
@ -131,11 +128,22 @@ NodeId PoseGraph3D::AddNode(
data_.submap_data.at(submap_id).submap = insertion_submaps.back(); data_.submap_data.at(submap_id).submap = insertion_submaps.back();
LOG(INFO) << "Inserted submap " << submap_id << "."; LOG(INFO) << "Inserted submap " << submap_id << ".";
} }
return node_id;
}
NodeId PoseGraph3D::AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap3D>>& 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 // We have to check this here, because it might have changed by the time we
// execute the lambda. // execute the lambda.
const bool newly_finished_submap = insertion_submaps.front()->finished(); const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() EXCLUDES(mutex_) {
return ComputeConstraintsForNode(node_id, insertion_submaps, return ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap); newly_finished_submap);
}); });
@ -144,13 +152,21 @@ NodeId PoseGraph3D::AddNode(
void PoseGraph3D::AddWorkItem( void PoseGraph3D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) { const std::function<WorkItem::Result()>& work_item) {
if (work_queue_ != nullptr) { {
const auto now = std::chrono::steady_clock::now(); common::MutexLocker locker(&mutex_);
work_queue_->push_back({now, work_item}); if (work_queue_ != nullptr) {
kWorkQueueDelayMetric->Set( const auto now = std::chrono::steady_clock::now();
common::ToSeconds(now - work_queue_->front().time)); work_queue_->push_back({now, work_item});
} else if (work_item() == WorkItem::Result::kRunOptimization) { kWorkQueueDelayMetric->Set(
work_queue_ = common::make_unique<WorkQueue>(); common::ToSeconds(now - work_queue_->front().time));
return;
}
}
if (work_item() == WorkItem::Result::kRunOptimization) {
{
common::MutexLocker locker(&mutex_);
work_queue_ = common::make_unique<WorkQueue>();
}
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder3D::Result& result) { [this](const constraints::ConstraintBuilder3D::Result& result) {
HandleWorkQueue(result); HandleWorkQueue(result);
@ -177,20 +193,22 @@ void PoseGraph3D::AddTrajectoryIfNeeded(const int trajectory_id) {
void PoseGraph3D::AddImuData(const int trajectory_id, void PoseGraph3D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) { const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_); AddWorkItem([=]() EXCLUDES(mutex_) {
if (!CanAddWorkItemModifying(trajectory_id)) return; common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddImuData(trajectory_id, imu_data); optimization_problem_->AddImuData(trajectory_id, imu_data);
}
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
} }
void PoseGraph3D::AddOdometryData(const int trajectory_id, void PoseGraph3D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) { const sensor::OdometryData& odometry_data) {
common::MutexLocker locker(&mutex_); AddWorkItem([=]() EXCLUDES(mutex_) {
if (!CanAddWorkItemModifying(trajectory_id)) return; common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddOdometryData(trajectory_id, odometry_data); optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
}
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
} }
@ -198,26 +216,28 @@ void PoseGraph3D::AddOdometryData(const int trajectory_id,
void PoseGraph3D::AddFixedFramePoseData( void PoseGraph3D::AddFixedFramePoseData(
const int trajectory_id, const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) { const sensor::FixedFramePoseData& fixed_frame_pose_data) {
common::MutexLocker locker(&mutex_); AddWorkItem([=]() EXCLUDES(mutex_) {
if (!CanAddWorkItemModifying(trajectory_id)) return; common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddFixedFramePoseData(trajectory_id, optimization_problem_->AddFixedFramePoseData(trajectory_id,
fixed_frame_pose_data); fixed_frame_pose_data);
}
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
} }
void PoseGraph3D::AddLandmarkData(int trajectory_id, void PoseGraph3D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) { const sensor::LandmarkData& landmark_data) {
common::MutexLocker locker(&mutex_); AddWorkItem([=]() EXCLUDES(mutex_) {
if (!CanAddWorkItemModifying(trajectory_id)) return; common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { if (CanAddWorkItemModifying(trajectory_id)) {
for (const auto& observation : landmark_data.landmark_observations) { for (const auto& observation : landmark_data.landmark_observations) {
data_.landmark_nodes[observation.id].landmark_observations.emplace_back( data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
PoseGraphInterface::LandmarkNode::LandmarkObservation{ PoseGraphInterface::LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time, trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform, observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight}); observation.translation_weight, observation.rotation_weight});
}
} }
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
@ -294,6 +314,7 @@ WorkItem::Result PoseGraph3D::ComputeConstraintsForNode(
const NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const Submap3D>> insertion_submaps, std::vector<std::shared_ptr<const Submap3D>> insertion_submaps,
const bool newly_finished_submap) { const bool newly_finished_submap) {
common::MutexLocker locker(&mutex_);
const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data; const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data;
const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses( const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps); node_id.trajectory_id, constant_data->time, insertion_submaps);
@ -424,34 +445,43 @@ void PoseGraph3D::HandleWorkQueue(
trajectory_id_to_last_optimized_node_id); trajectory_id_to_last_optimized_node_id);
} }
common::MutexLocker locker(&mutex_); {
for (const Constraint& constraint : result) { common::MutexLocker locker(&mutex_);
UpdateTrajectoryConnectivity(constraint); for (const Constraint& constraint : result) {
} UpdateTrajectoryConnectivity(constraint);
DeleteTrajectoriesIfNeeded(); }
TrimmingHandle trimming_handle(this); DeleteTrajectoriesIfNeeded();
for (auto& trimmer : trimmers_) { TrimmingHandle trimming_handle(this);
trimmer->Trim(&trimming_handle); for (auto& trimmer : trimmers_) {
} trimmer->Trim(&trimming_handle);
trimmers_.erase( }
std::remove_if(trimmers_.begin(), trimmers_.end(), trimmers_.erase(
[](std::unique_ptr<PoseGraphTrimmer>& trimmer) { std::remove_if(trimmers_.begin(), trimmers_.end(),
return trimmer->IsFinished(); [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
}), return trimmer->IsFinished();
trimmers_.end()); }),
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; bool process_work_queue = true;
while (process_work_queue) { while (process_work_queue) {
if (work_queue_->empty()) { std::function<WorkItem::Result()> work_item;
work_queue_.reset(); {
return; 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 = process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
work_queue_->front().task() == WorkItem::Result::kDoNotRunOptimization;
work_queue_->pop_front();
} }
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. // We have to optimize again.
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder3D::Result& result) { [this](const constraints::ConstraintBuilder3D::Result& result) {
@ -497,16 +527,19 @@ void PoseGraph3D::WaitForAllComputations() {
} }
void PoseGraph3D::DeleteTrajectory(const int trajectory_id) { void PoseGraph3D::DeleteTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); {
auto it = data_.trajectories_state.find(trajectory_id); common::MutexLocker locker(&mutex_);
if (it == data_.trajectories_state.end()) { auto it = data_.trajectories_state.find(trajectory_id);
LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " if (it == data_.trajectories_state.end()) {
<< trajectory_id; LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: "
return; << trajectory_id;
return;
}
it->second.deletion_state =
InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
} }
it->second.deletion_state = AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION; common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK(data_.trajectories_state.at(trajectory_id).state != CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::ACTIVE); TrajectoryState::ACTIVE);
CHECK(data_.trajectories_state.at(trajectory_id).state != CHECK(data_.trajectories_state.at(trajectory_id).state !=
@ -520,8 +553,8 @@ void PoseGraph3D::DeleteTrajectory(const int trajectory_id) {
} }
void PoseGraph3D::FinishTrajectory(const int trajectory_id) { void PoseGraph3D::FinishTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { common::MutexLocker locker(&mutex_);
CHECK(!IsTrajectoryFinished(trajectory_id)); CHECK(!IsTrajectoryFinished(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED; data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;
@ -539,9 +572,12 @@ bool PoseGraph3D::IsTrajectoryFinished(const int trajectory_id) const {
} }
void PoseGraph3D::FreezeTrajectory(const int trajectory_id) { void PoseGraph3D::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); {
data_.trajectory_connectivity_state.Add(trajectory_id); common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { data_.trajectory_connectivity_state.Add(trajectory_id);
}
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
CHECK(!IsTrajectoryFrozen(trajectory_id)); CHECK(!IsTrajectoryFrozen(trajectory_id));
data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN; data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
@ -565,15 +601,18 @@ void PoseGraph3D::AddSubmapFromProto(
std::shared_ptr<const Submap3D> submap_ptr = std::shared_ptr<const Submap3D> submap_ptr =
std::make_shared<const Submap3D>(submap.submap_3d()); std::make_shared<const Submap3D>(submap.submap_3d());
common::MutexLocker locker(&mutex_); {
AddTrajectoryIfNeeded(submap_id.trajectory_id); common::MutexLocker locker(&mutex_);
if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; AddTrajectoryIfNeeded(submap_id.trajectory_id);
data_.submap_data.Insert(submap_id, InternalSubmapData()); if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
data_.submap_data.at(submap_id).submap = submap_ptr; data_.submap_data.Insert(submap_id, InternalSubmapData());
// Immediately show the submap at the 'global_submap_pose'. data_.submap_data.at(submap_id).submap = submap_ptr;
data_.global_submap_poses_3d.Insert( // Immediately show the submap at the 'global_submap_pose'.
submap_id, optimization::SubmapSpec3D{global_submap_pose}); data_.global_submap_poses_3d.Insert(
AddWorkItem([this, submap_id, global_submap_pose]() REQUIRES(mutex_) { submap_id, optimization::SubmapSpec3D{global_submap_pose});
}
AddWorkItem([this, submap_id, global_submap_pose]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
data_.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); optimization_problem_->InsertSubmap(submap_id, global_submap_pose);
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
@ -587,13 +626,16 @@ void PoseGraph3D::AddNodeFromProto(const transform::Rigid3d& global_pose,
std::shared_ptr<const TrajectoryNode::Data> constant_data = std::shared_ptr<const TrajectoryNode::Data> constant_data =
std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data())); std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data()));
common::MutexLocker locker(&mutex_); {
AddTrajectoryIfNeeded(node_id.trajectory_id); common::MutexLocker locker(&mutex_);
if (!CanAddWorkItemModifying(node_id.trajectory_id)) return; AddTrajectoryIfNeeded(node_id.trajectory_id);
data_.trajectory_nodes.Insert(node_id, if (!CanAddWorkItemModifying(node_id.trajectory_id)) return;
TrajectoryNode{constant_data, global_pose}); 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 = const auto& constant_data =
data_.trajectory_nodes.at(node_id).constant_data; data_.trajectory_nodes.at(node_id).constant_data;
optimization_problem_->InsertTrajectoryNode( optimization_problem_->InsertTrajectoryNode(
@ -617,28 +659,30 @@ void PoseGraph3D::SetTrajectoryDataFromProto(
} }
const int trajectory_id = data.trajectory_id(); const int trajectory_id = data.trajectory_id();
common::MutexLocker locker(&mutex_); AddWorkItem([this, trajectory_id, trajectory_data]() EXCLUDES(mutex_) {
if (!CanAddWorkItemModifying(trajectory_id)) return; common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id, trajectory_data]() REQUIRES(mutex_) { if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data); optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data);
}
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
} }
void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id, void PoseGraph3D::AddNodeToSubmap(const NodeId& node_id,
const SubmapId& submap_id) { const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) {
if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) { if (CanAddWorkItemModifying(submap_id.trajectory_id)) {
data_.submap_data.at(submap_id).node_ids.insert(node_id); data_.submap_data.at(submap_id).node_ids.insert(node_id);
}
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
} }
void PoseGraph3D::AddSerializedConstraints( void PoseGraph3D::AddSerializedConstraints(
const std::vector<Constraint>& constraints) { const std::vector<Constraint>& constraints) {
common::MutexLocker locker(&mutex_); AddWorkItem([this, constraints]() EXCLUDES(mutex_) {
AddWorkItem([this, constraints]() REQUIRES(mutex_) { common::MutexLocker locker(&mutex_);
for (const auto& constraint : constraints) { for (const auto& constraint : constraints) {
CHECK(data_.trajectory_nodes.Contains(constraint.node_id)); CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
CHECK(data_.submap_data.Contains(constraint.submap_id)); CHECK(data_.submap_data.Contains(constraint.submap_id));
@ -663,10 +707,10 @@ void PoseGraph3D::AddSerializedConstraints(
} }
void PoseGraph3D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) { void PoseGraph3D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
common::MutexLocker locker(&mutex_);
// C++11 does not allow us to move a unique_ptr into a lambda. // C++11 does not allow us to move a unique_ptr into a lambda.
PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); 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); trimmers_.emplace_back(trimmer_ptr);
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });
@ -674,13 +718,14 @@ void PoseGraph3D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
void PoseGraph3D::RunFinalOptimization() { void PoseGraph3D::RunFinalOptimization() {
{ {
common::MutexLocker locker(&mutex_); AddWorkItem([this]() EXCLUDES(mutex_) {
AddWorkItem([this]() REQUIRES(mutex_) { common::MutexLocker locker(&mutex_);
optimization_problem_->SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations()); options_.max_num_final_iterations());
return WorkItem::Result::kRunOptimization; return WorkItem::Result::kRunOptimization;
}); });
AddWorkItem([this]() REQUIRES(mutex_) { AddWorkItem([this]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
optimization_problem_->SetMaxNumIterations( optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options() options_.optimization_problem_options()
.ceres_solver_options() .ceres_solver_options()
@ -847,8 +892,8 @@ std::map<std::string, transform::Rigid3d> PoseGraph3D::GetLandmarkPoses()
void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id, void PoseGraph3D::SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) { const transform::Rigid3d& global_pose) {
common::MutexLocker locker(&mutex_); AddWorkItem([=]() EXCLUDES(mutex_) {
AddWorkItem([=]() REQUIRES(mutex_) { common::MutexLocker locker(&mutex_);
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose; data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
return WorkItem::Result::kDoNotRunOptimization; return WorkItem::Result::kDoNotRunOptimization;
}); });

15
cartographer/mapping/internal/3d/pose_graph_3d.h
View File

@ -162,11 +162,18 @@ class PoseGraph3D : public PoseGraph {
// Handles a new work item. // Handles a new work item.
void AddWorkItem(const std::function<WorkItem::Result()>& work_item) void AddWorkItem(const std::function<WorkItem::Result()>& work_item)
REQUIRES(mutex_); EXCLUDES(mutex_);
// Adds connectivity and sampler for a trajectory if it does not exist. // Adds connectivity and sampler for a trajectory if it does not exist.
void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_); void AddTrajectoryIfNeeded(int trajectory_id) REQUIRES(mutex_);
// Appends the new node and submap (if needed) to the internal data stuctures.
NodeId AppendNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
const std::vector<std::shared_ptr<const Submap3D>>& insertion_submaps,
const transform::Rigid3d& optimized_pose) EXCLUDES(mutex_);
// Grows the optimization problem to have an entry for every element of // Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<SubmapId> InitializeGlobalSubmapPoses( std::vector<SubmapId> InitializeGlobalSubmapPoses(
@ -178,7 +185,7 @@ class PoseGraph3D : public PoseGraph {
WorkItem::Result ComputeConstraintsForNode( WorkItem::Result ComputeConstraintsForNode(
const NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const Submap3D>> insertion_submaps, std::vector<std::shared_ptr<const Submap3D>> insertion_submaps,
bool newly_finished_submap) REQUIRES(mutex_); bool newly_finished_submap) EXCLUDES(mutex_);
// Computes constraints for a node and submap pair. // Computes constraints for a node and submap pair.
void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id)
@ -194,7 +201,7 @@ class PoseGraph3D : public PoseGraph {
// Runs the optimization, executes the trimmers and processes the work queue. // Runs the optimization, executes the trimmers and processes the work queue.
void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result) void HandleWorkQueue(const constraints::ConstraintBuilder3D::Result& result)
REQUIRES(mutex_); EXCLUDES(mutex_);
// Runs the optimization. Callers have to make sure, that there is only one // Runs the optimization. Callers have to make sure, that there is only one
// optimization being run at a time. // optimization being run at a time.
@ -240,7 +247,7 @@ class PoseGraph3D : public PoseGraph {
// Current optimization problem. // Current optimization problem.
std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_; std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem_;
constraints::ConstraintBuilder3D constraint_builder_ GUARDED_BY(mutex_); constraints::ConstraintBuilder3D constraint_builder_;
// List of all trimmers to consult when optimizations finish. // List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_); std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);