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

128
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};
}
NodeId PoseGraph2D::AddNode(
NodeId PoseGraph2D::AppendNode(
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 std::vector<std::shared_ptr<const Submap2D>>& 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<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
// 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<WorkItem::Result()>& work_item) {
if (work_queue_ == nullptr) {
if (work_item() == WorkItem::Result::kRunOptimization) {
work_queue_ = common::make_unique<WorkQueue>();
constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder2D::Result& result) {
HandleWorkQueue(result);
});
}
} else {
{
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;
}
}
if (work_item() == WorkItem::Result::kRunOptimization) {
{
common::MutexLocker locker(&mutex_);
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,
const sensor::ImuData& imu_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
if (!CanAddWorkItemModifying(trajectory_id)) return;
AddWorkItem([=]() REQUIRES(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) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
if (!CanAddWorkItemModifying(trajectory_id)) return;
AddWorkItem([=]() REQUIRES(mutex_) {
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
}
return WorkItem::Result::kDoNotRunOptimization;
});
}
@ -207,16 +223,17 @@ void PoseGraph2D::AddFixedFramePoseData(
void PoseGraph2D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
if (!CanAddWorkItemModifying(trajectory_id)) return;
AddWorkItem([=]() REQUIRES(mutex_) {
if (CanAddWorkItemModifying(trajectory_id)) {
for (const auto& observation : landmark_data.landmark_observations) {
data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
LandmarkNode::LandmarkObservation{
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<std::shared_ptr<const Submap2D>> 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<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps);
@ -409,6 +427,7 @@ void PoseGraph2D::HandleWorkQueue(
trajectory_id_to_last_optimized_node_id);
}
{
common::MutexLocker locker(&mutex_);
for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint);
@ -426,17 +445,25 @@ void PoseGraph2D::HandleWorkQueue(
trimmers_.end());
num_nodes_since_last_loop_closure_ = 0;
}
size_t work_queue_size;
bool process_work_queue = true;
while (process_work_queue) {
std::function<WorkItem::Result()> work_item;
{
common::MutexLocker locker(&mutex_);
if (work_queue_->empty()) {
work_queue_.reset();
return;
}
process_work_queue =
work_queue_->front().task() == WorkItem::Result::kDoNotRunOptimization;
work_item = work_queue_->front().task;
work_queue_->pop_front();
work_queue_size = work_queue_->size();
}
LOG(INFO) << "Remaining work items in queue: " << work_queue_->size();
process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
}
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,6 +509,7 @@ 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()) {
@ -491,7 +519,9 @@ void PoseGraph2D::DeleteTrajectory(const int trajectory_id) {
}
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) {
AddWorkItem([this, trajectory_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, trajectory_id]() REQUIRES(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_) {
}
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,11 +581,13 @@ void PoseGraph2D::AddSubmapFromProto(
const SubmapId submap_id = {submap.submap_id().trajectory_id(),
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 =
transform::Project2D(global_submap_pose);
{
common::MutexLocker locker(&mutex_);
const std::shared_ptr<const Submap2D> submap_ptr =
std::make_shared<const Submap2D>(submap.submap_2d(),
&conversion_tables_);
AddTrajectoryIfNeeded(submap_id.trajectory_id);
if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
data_.submap_data.Insert(submap_id, InternalSubmapData());
@ -560,9 +595,11 @@ void PoseGraph2D::AddSubmapFromProto(
// 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();
}
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<const TrajectoryNode::Data> constant_data =
std::make_shared<const TrajectoryNode::Data>(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});
}
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) {
AddWorkItem([this, node_id, submap_id]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
if (!CanAddWorkItemModifying(submap_id.trajectory_id)) return;
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);
}
return WorkItem::Result::kDoNotRunOptimization;
});
}
void PoseGraph2D::AddSerializedConstraints(
const std::vector<Constraint>& constraints) {
AddWorkItem([this, constraints]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this, constraints]() REQUIRES(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<PoseGraphTrimmer> 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<PoseGraphTrimmer> trimmer) {
void PoseGraph2D::RunFinalOptimization() {
{
AddWorkItem([this]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([this]() REQUIRES(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<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
void PoseGraph2D::SetLandmarkPose(const std::string& landmark_id,
const transform::Rigid3d& global_pose) {
AddWorkItem([=]() EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) {
data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
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_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<WorkItem::Result()>& 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<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
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<SubmapId> InitializeGlobalSubmapPoses(
@ -175,7 +184,7 @@ class PoseGraph2D : public PoseGraph {
WorkItem::Result ComputeConstraintsForNode(
const NodeId& node_id,
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.
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::OptimizationProblem2D> optimization_problem_;
constraints::ConstraintBuilder2D constraint_builder_ GUARDED_BY(mutex_);
constraints::ConstraintBuilder2D constraint_builder_;
// List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);