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

Take PoseGraph2D mutex inside instead of outside work item. (#1310) 

This is #1264 for 2D.
master
Wolfgang Hess 2018-07-20 13:31:18 +02:00 committed by Wally B. Feed
parent 665b95d5c6
commit 5077224f8e
2 changed files with 159 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

250
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -106,13 +106,11 @@ std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
return {front_submap_id, last_submap_id}; return {front_submap_id, last_submap_id};
} }
NodeId PoseGraph2D::AddNode( NodeId PoseGraph2D::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 Submap2D>>& insertion_submaps) { const std::vector<std::shared_ptr<const Submap2D>>& 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)) {
@ -121,7 +119,6 @@ NodeId PoseGraph2D::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))
@ -133,11 +130,22 @@ NodeId PoseGraph2D::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 PoseGraph2D::AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& 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);
}); });
@ -146,19 +154,25 @@ NodeId PoseGraph2D::AddNode(
void PoseGraph2D::AddWorkItem( void PoseGraph2D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) { const std::function<WorkItem::Result()>& work_item) {
if (work_queue_ == nullptr) { {
if (work_item() == WorkItem::Result::kRunOptimization) { common::MutexLocker locker(&mutex_);
work_queue_ = common::make_unique<WorkQueue>(); if (work_queue_ != nullptr) {
constraint_builder_.WhenDone( const auto now = std::chrono::steady_clock::now();
[this](const constraints::ConstraintBuilder2D::Result& result) { work_queue_->push_back({now, work_item});
HandleWorkQueue(result); kWorkQueueDelayMetric->Set(
}); common::ToSeconds(now - work_queue_->front().time));
return;
} }
} else { }
const auto now = std::chrono::steady_clock::now(); if (work_item() == WorkItem::Result::kRunOptimization) {
work_queue_->push_back({now, work_item}); {
kWorkQueueDelayMetric->Set( common::MutexLocker locker(&mutex_);
common::ToSeconds(now - work_queue_->front().time)); work_queue_ = common::make_unique<WorkQueue>();
}
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, void PoseGraph2D::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 PoseGraph2D::AddOdometryData(const int trajectory_id, void PoseGraph2D::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;
}); });
} }
@ -207,15 +223,16 @@ void PoseGraph2D::AddFixedFramePoseData(
void PoseGraph2D::AddLandmarkData(int trajectory_id, void PoseGraph2D::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(
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;
}); });
@ -272,6 +289,7 @@ WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(
const NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps, std::vector<std::shared_ptr<const Submap2D>> 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);
@ -409,34 +427,43 @@ void PoseGraph2D::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::ConstraintBuilder2D::Result& result) { [this](const constraints::ConstraintBuilder2D::Result& result) {
@ -482,16 +509,19 @@ void PoseGraph2D::WaitForAllComputations() {
} }
void PoseGraph2D::DeleteTrajectory(const int trajectory_id) { void PoseGraph2D::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 !=
@ -505,8 +535,8 @@ void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
} }
void PoseGraph2D::FinishTrajectory(const int trajectory_id) { void PoseGraph2D::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;
@ -524,9 +554,12 @@ bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const {
} }
void PoseGraph2D::FreezeTrajectory(const int trajectory_id) { void PoseGraph2D::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;
@ -548,21 +581,25 @@ void PoseGraph2D::AddSubmapFromProto(
const SubmapId submap_id = {submap.submap_id().trajectory_id(), const SubmapId submap_id = {submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()}; submap.submap_id().submap_index()};
common::MutexLocker locker(&mutex_);
std::shared_ptr<const Submap2D> submap_ptr =
std::make_shared<const Submap2D>(submap.submap_2d(), &conversion_tables_);
const transform::Rigid2d global_submap_pose_2d = const transform::Rigid2d global_submap_pose_2d =
transform::Project2D(global_submap_pose); transform::Project2D(global_submap_pose);
AddTrajectoryIfNeeded(submap_id.trajectory_id); {
if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return; common::MutexLocker locker(&mutex_);
data_.submap_data.Insert(submap_id, InternalSubmapData()); const std::shared_ptr<const Submap2D> submap_ptr =
data_.submap_data.at(submap_id).submap = submap_ptr; std::make_shared<const Submap2D>(submap.submap_2d(),
// Immediately show the submap at the 'global_submap_pose'. &conversion_tables_);
data_.global_submap_poses_2d.Insert( AddTrajectoryIfNeeded(submap_id.trajectory_id);
submap_id, optimization::SubmapSpec2D{global_submap_pose_2d}); if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
bool finished = submap.submap_2d().finished(); 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( 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 = data_.submap_data.at(submap_id).state =
finished ? SubmapState::kFinished : SubmapState::kActive; finished ? SubmapState::kFinished : SubmapState::kActive;
optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d); optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
@ -577,13 +614,16 @@ void PoseGraph2D::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;
const auto gravity_alignment_inverse = transform::Rigid3d::Rotation( const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(
@ -607,18 +647,19 @@ void PoseGraph2D::SetTrajectoryDataFromProto(
void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id, void PoseGraph2D::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 PoseGraph2D::AddSerializedConstraints( void PoseGraph2D::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));
@ -650,10 +691,10 @@ void PoseGraph2D::AddSerializedConstraints(
} }
void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) { void PoseGraph2D::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;
}); });
@ -661,13 +702,14 @@ void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
void PoseGraph2D::RunFinalOptimization() { void PoseGraph2D::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()
@ -807,8 +849,8 @@ std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id, void PoseGraph2D::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;
}); });

17
cartographer/mapping/internal/2d/pose_graph_2d.h
View File

@ -40,6 +40,7 @@
#include "cartographer/mapping/pose_graph.h" #include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_data.h" #include "cartographer/mapping/pose_graph_data.h"
#include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/value_conversion_tables.h"
#include "cartographer/metrics/family_factory.h" #include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/fixed_frame_pose_data.h" #include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h" #include "cartographer/sensor/landmark_data.h"
@ -159,11 +160,19 @@ class PoseGraph2D : 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
// structures.
NodeId AppendNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& 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(
@ -175,7 +184,7 @@ class PoseGraph2D : public PoseGraph {
WorkItem::Result ComputeConstraintsForNode( WorkItem::Result ComputeConstraintsForNode(
const NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps, std::vector<std::shared_ptr<const Submap2D>> 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)
@ -191,7 +200,7 @@ class PoseGraph2D : 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::ConstraintBuilder2D::Result& result) 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 // Waits until we caught up (i.e. nothing is waiting to be scheduled), and
// all computations have finished. // all computations have finished.
@ -237,7 +246,7 @@ class PoseGraph2D : public PoseGraph {
// Current optimization problem. // Current optimization problem.
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_; std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
constraints::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_); constraints::ConstraintBuilder2D 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_);