Fined-grained locking for PoseGraph3D constraints (#1279)
When preparing constraints make holding of the PoseGraph3D mutex fine-grained to address lock contention from issue #1250. That includes not holding the mutex when acquring the constraint builder mutex.master
parent
d4376e765b
commit
7b48b66a65
|
@ -245,8 +245,6 @@ void PoseGraph3D::AddLandmarkData(int trajectory_id,
|
||||||
|
|
||||||
void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
|
void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
|
||||||
const SubmapId& submap_id) {
|
const SubmapId& submap_id) {
|
||||||
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
|
|
||||||
|
|
||||||
const transform::Rigid3d global_node_pose =
|
const transform::Rigid3d global_node_pose =
|
||||||
optimization_problem_->node_data().at(node_id).global_pose;
|
optimization_problem_->node_data().at(node_id).global_pose;
|
||||||
|
|
||||||
|
@ -256,7 +254,15 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
|
||||||
const transform::Rigid3d global_submap_pose_inverse =
|
const transform::Rigid3d global_submap_pose_inverse =
|
||||||
global_submap_pose.inverse();
|
global_submap_pose.inverse();
|
||||||
|
|
||||||
|
bool maybe_add_local_constraint = false;
|
||||||
|
bool maybe_add_global_constraint = false;
|
||||||
|
const TrajectoryNode::Data* constant_data;
|
||||||
|
const Submap3D* submap;
|
||||||
std::vector<TrajectoryNode> submap_nodes;
|
std::vector<TrajectoryNode> submap_nodes;
|
||||||
|
{
|
||||||
|
common::MutexLocker locker(&mutex_);
|
||||||
|
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
|
||||||
|
|
||||||
for (const NodeId& submap_node_id :
|
for (const NodeId& submap_node_id :
|
||||||
data_.submap_data.at(submap_id).node_ids) {
|
data_.submap_data.at(submap_id).node_ids) {
|
||||||
submap_nodes.push_back(TrajectoryNode{
|
submap_nodes.push_back(TrajectoryNode{
|
||||||
|
@ -274,39 +280,32 @@ void PoseGraph3D::ComputeConstraint(const NodeId& node_id,
|
||||||
last_connection_time +
|
last_connection_time +
|
||||||
common::FromSeconds(
|
common::FromSeconds(
|
||||||
options_.global_constraint_search_after_n_seconds())) {
|
options_.global_constraint_search_after_n_seconds())) {
|
||||||
// If the node and the submap belong to the same trajectory or if there has
|
// If the node and the submap belong to the same trajectory or if there
|
||||||
// been a recent global constraint that ties that node's trajectory to the
|
// has been a recent global constraint that ties that node's trajectory to
|
||||||
// submap's trajectory, it suffices to do a match constrained to a local
|
// the submap's trajectory, it suffices to do a match constrained to a
|
||||||
// search window.
|
// local search window.
|
||||||
constraint_builder_.MaybeAddConstraint(
|
maybe_add_local_constraint = true;
|
||||||
submap_id,
|
|
||||||
static_cast<const Submap3D*>(
|
|
||||||
data_.submap_data.at(submap_id).submap.get()),
|
|
||||||
node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
|
|
||||||
submap_nodes, global_node_pose, global_submap_pose);
|
|
||||||
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
|
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
|
||||||
// In this situation, 'global_node_pose' and 'global_submap_pose' have
|
// In this situation, 'global_node_pose' and 'global_submap_pose' have
|
||||||
// orientations agreeing on gravity. Their relationship regarding yaw is
|
// orientations agreeing on gravity. Their relationship regarding yaw is
|
||||||
// arbitrary. Finding the correct yaw component will be handled by the
|
// arbitrary. Finding the correct yaw component will be handled by the
|
||||||
// matching procedure in the FastCorrelativeScanMatcher, and the given yaw
|
// matching procedure in the FastCorrelativeScanMatcher, and the given yaw
|
||||||
// is essentially ignored.
|
// is essentially ignored.
|
||||||
constraint_builder_.MaybeAddGlobalConstraint(
|
maybe_add_global_constraint = true;
|
||||||
submap_id,
|
}
|
||||||
static_cast<const Submap3D*>(
|
constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
|
||||||
data_.submap_data.at(submap_id).submap.get()),
|
submap = static_cast<const Submap3D*>(
|
||||||
node_id, data_.trajectory_nodes.at(node_id).constant_data.get(),
|
data_.submap_data.at(submap_id).submap.get());
|
||||||
submap_nodes, global_node_pose.rotation(),
|
|
||||||
global_submap_pose.rotation());
|
|
||||||
}
|
}
|
||||||
}
|
|
||||||
|
|
||||||
void PoseGraph3D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
|
if (maybe_add_local_constraint) {
|
||||||
const auto& submap_data = data_.submap_data.at(submap_id);
|
constraint_builder_.MaybeAddConstraint(
|
||||||
for (const auto& node_id_data : optimization_problem_->node_data()) {
|
submap_id, submap, node_id, constant_data, submap_nodes,
|
||||||
const NodeId& node_id = node_id_data.id;
|
global_node_pose, global_submap_pose);
|
||||||
if (submap_data.node_ids.count(node_id) == 0) {
|
} else if (maybe_add_global_constraint) {
|
||||||
ComputeConstraint(node_id, submap_id);
|
constraint_builder_.MaybeAddGlobalConstraint(
|
||||||
}
|
submap_id, submap, node_id, constant_data, submap_nodes,
|
||||||
|
global_node_pose.rotation(), global_submap_pose.rotation());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -314,9 +313,14 @@ 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) {
|
||||||
|
std::vector<SubmapId> submap_ids;
|
||||||
|
std::vector<SubmapId> finished_submap_ids;
|
||||||
|
std::set<NodeId> newly_finished_submap_node_ids;
|
||||||
|
{
|
||||||
common::MutexLocker locker(&mutex_);
|
common::MutexLocker locker(&mutex_);
|
||||||
const auto& constant_data = data_.trajectory_nodes.at(node_id).constant_data;
|
const auto& constant_data =
|
||||||
const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
|
data_.trajectory_nodes.at(node_id).constant_data;
|
||||||
|
submap_ids = InitializeGlobalSubmapPoses(
|
||||||
node_id.trajectory_id, constant_data->time, insertion_submaps);
|
node_id.trajectory_id, constant_data->time, insertion_submaps);
|
||||||
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
|
CHECK_EQ(submap_ids.size(), insertion_submaps.size());
|
||||||
const SubmapId matching_id = submap_ids.front();
|
const SubmapId matching_id = submap_ids.front();
|
||||||
|
@ -329,38 +333,53 @@ WorkItem::Result PoseGraph3D::ComputeConstraintsForNode(
|
||||||
optimization::NodeSpec3D{constant_data->time, local_pose, global_pose});
|
optimization::NodeSpec3D{constant_data->time, local_pose, global_pose});
|
||||||
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
|
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
|
||||||
const SubmapId submap_id = submap_ids[i];
|
const SubmapId submap_id = submap_ids[i];
|
||||||
// Even if this was the last node added to 'submap_id', the submap will only
|
// Even if this was the last node added to 'submap_id', the submap will
|
||||||
// be marked as finished in 'data_.submap_data' further below.
|
// only be marked as finished in 'data_.submap_data' further below.
|
||||||
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive);
|
CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive);
|
||||||
data_.submap_data.at(submap_id).node_ids.emplace(node_id);
|
data_.submap_data.at(submap_id).node_ids.emplace(node_id);
|
||||||
const transform::Rigid3d constraint_transform =
|
const transform::Rigid3d constraint_transform =
|
||||||
insertion_submaps[i]->local_pose().inverse() * local_pose;
|
insertion_submaps[i]->local_pose().inverse() * local_pose;
|
||||||
data_.constraints.push_back(
|
data_.constraints.push_back(Constraint{
|
||||||
Constraint{submap_id,
|
submap_id,
|
||||||
node_id,
|
node_id,
|
||||||
{constraint_transform, options_.matcher_translation_weight(),
|
{constraint_transform, options_.matcher_translation_weight(),
|
||||||
options_.matcher_rotation_weight()},
|
options_.matcher_rotation_weight()},
|
||||||
Constraint::INTRA_SUBMAP});
|
Constraint::INTRA_SUBMAP});
|
||||||
}
|
}
|
||||||
|
// TODO(gaschler): Consider not searching for constraints against
|
||||||
// TODO(gaschler): Consider not searching for constraints against trajectories
|
// trajectories scheduled for deletion.
|
||||||
// scheduled for deletion.
|
// TODO(danielsievers): Add a member variable and avoid having to copy
|
||||||
|
// them out here.
|
||||||
for (const auto& submap_id_data : data_.submap_data) {
|
for (const auto& submap_id_data : data_.submap_data) {
|
||||||
if (submap_id_data.data.state == SubmapState::kFinished) {
|
if (submap_id_data.data.state == SubmapState::kFinished) {
|
||||||
CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
|
CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
|
||||||
ComputeConstraint(node_id, submap_id_data.id);
|
finished_submap_ids.emplace_back(submap_id_data.id);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (newly_finished_submap) {
|
||||||
|
const SubmapId newly_finished_submap_id = submap_ids.front();
|
||||||
|
InternalSubmapData& finished_submap_data =
|
||||||
|
data_.submap_data.at(newly_finished_submap_id);
|
||||||
|
CHECK(finished_submap_data.state == SubmapState::kActive);
|
||||||
|
finished_submap_data.state = SubmapState::kFinished;
|
||||||
|
newly_finished_submap_node_ids = finished_submap_data.node_ids;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
for (const auto& submap_id : finished_submap_ids) {
|
||||||
|
ComputeConstraint(node_id, submap_id);
|
||||||
|
}
|
||||||
|
|
||||||
if (newly_finished_submap) {
|
if (newly_finished_submap) {
|
||||||
const SubmapId finished_submap_id = submap_ids.front();
|
const SubmapId newly_finished_submap_id = submap_ids.front();
|
||||||
InternalSubmapData& finished_submap_data =
|
|
||||||
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
|
// We have a new completed submap, so we look into adding constraints for
|
||||||
// old nodes.
|
// old nodes.
|
||||||
ComputeConstraintsForOldNodes(finished_submap_id);
|
for (const auto& node_id_data : optimization_problem_->node_data()) {
|
||||||
|
const NodeId& node_id = node_id_data.id;
|
||||||
|
if (newly_finished_submap_node_ids.count(node_id) == 0) {
|
||||||
|
ComputeConstraint(node_id, newly_finished_submap_id);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
constraint_builder_.NotifyEndOfNode();
|
constraint_builder_.NotifyEndOfNode();
|
||||||
++num_nodes_since_last_loop_closure_;
|
++num_nodes_since_last_loop_closure_;
|
||||||
|
|
|
@ -189,11 +189,7 @@ class PoseGraph3D : public PoseGraph {
|
||||||
|
|
||||||
// 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)
|
||||||
REQUIRES(mutex_);
|
EXCLUDES(mutex_);
|
||||||
|
|
||||||
// Adds constraints for older nodes whenever a new submap is finished.
|
|
||||||
void ComputeConstraintsForOldNodes(const SubmapId& submap_id)
|
|
||||||
REQUIRES(mutex_);
|
|
||||||
|
|
||||||
// Deletes trajectories waiting for deletion. Must not be called during
|
// Deletes trajectories waiting for deletion. Must not be called during
|
||||||
// constraint search.
|
// constraint search.
|
||||||
|
|
Loading…
Reference in New Issue