From f61a944938ca55a010128f09c24e7e9d6123b25b Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 25 Jul 2018 15:26:29 +0200 Subject: [PATCH] Fine-grained locking for PoseGraph2D constraints. (#1327) This is #1279 for 2D. --- .../mapping/internal/2d/pose_graph_2d.cc | 184 ++++++++++-------- .../mapping/internal/2d/pose_graph_2d.h | 6 +- 2 files changed, 104 insertions(+), 86 deletions(-) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 88dbcdf..0c6055e 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -34,6 +34,7 @@ #include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h" #include "cartographer/sensor/compressed_point_cloud.h" #include "cartographer/sensor/internal/voxel_filter.h" +#include "cartographer/transform/transform.h" #include "glog/logging.h" namespace cartographer { @@ -240,48 +241,47 @@ void PoseGraph2D::AddLandmarkData(int trajectory_id, void PoseGraph2D::ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) { - CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); + bool maybe_add_local_constraint = false; + bool maybe_add_global_constraint = false; + const TrajectoryNode::Data* constant_data; + const Submap2D* submap; + { + common::MutexLocker locker(&mutex_); + CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished); - const common::Time node_time = GetLatestNodeTime(node_id, submap_id); - const common::Time last_connection_time = - data_.trajectory_connectivity_state.LastConnectionTime( - node_id.trajectory_id, submap_id.trajectory_id); - if (node_id.trajectory_id == submap_id.trajectory_id || - node_time < - last_connection_time + - common::FromSeconds( - options_.global_constraint_search_after_n_seconds())) { - // If the node and the submap belong to the same trajectory or if there - // has been a recent global constraint that ties that node's trajectory to - // the submap's trajectory, it suffices to do a match constrained to a - // local search window. + const common::Time node_time = GetLatestNodeTime(node_id, submap_id); + const common::Time last_connection_time = + data_.trajectory_connectivity_state.LastConnectionTime( + node_id.trajectory_id, submap_id.trajectory_id); + if (node_id.trajectory_id == submap_id.trajectory_id || + node_time < + last_connection_time + + common::FromSeconds( + options_.global_constraint_search_after_n_seconds())) { + // If the node and the submap belong to the same trajectory or if there + // has been a recent global constraint that ties that node's trajectory to + // the submap's trajectory, it suffices to do a match constrained to a + // local search window. + maybe_add_local_constraint = true; + } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { + maybe_add_global_constraint = true; + } + constant_data = data_.trajectory_nodes.at(node_id).constant_data.get(); + submap = static_cast( + data_.submap_data.at(submap_id).submap.get()); + } + + if (maybe_add_local_constraint) { const transform::Rigid2d initial_relative_pose = optimization_problem_->submap_data() .at(submap_id) .global_pose.inverse() * optimization_problem_->node_data().at(node_id).global_pose_2d; constraint_builder_.MaybeAddConstraint( - submap_id, - static_cast( - data_.submap_data.at(submap_id).submap.get()), - node_id, data_.trajectory_nodes.at(node_id).constant_data.get(), - initial_relative_pose); - } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { - constraint_builder_.MaybeAddGlobalConstraint( - submap_id, - static_cast( - data_.submap_data.at(submap_id).submap.get()), - node_id, data_.trajectory_nodes.at(node_id).constant_data.get()); - } -} - -void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) { - const auto& submap_data = data_.submap_data.at(submap_id); - for (const auto& node_id_data : optimization_problem_->node_data()) { - const NodeId& node_id = node_id_data.id; - if (submap_data.node_ids.count(node_id) == 0) { - ComputeConstraint(node_id, submap_id); - } + submap_id, submap, node_id, constant_data, initial_relative_pose); + } else if (maybe_add_global_constraint) { + constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id, + constant_data); } } @@ -289,60 +289,82 @@ 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); - CHECK_EQ(submap_ids.size(), insertion_submaps.size()); - const SubmapId matching_id = submap_ids.front(); - const transform::Rigid2d local_pose_2d = transform::Project2D( - constant_data->local_pose * - transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); - const transform::Rigid2d global_pose_2d = - optimization_problem_->submap_data().at(matching_id).global_pose * - constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() * - local_pose_2d; - optimization_problem_->AddTrajectoryNode( - matching_id.trajectory_id, - optimization::NodeSpec2D{constant_data->time, local_pose_2d, - global_pose_2d, - constant_data->gravity_alignment}); - for (size_t i = 0; i < insertion_submaps.size(); ++i) { - const SubmapId submap_id = submap_ids[i]; - // Even if this was the last node added to 'submap_id', the submap will - // only be marked as finished in 'data_.submap_data' further below. - CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive); - data_.submap_data.at(submap_id).node_ids.emplace(node_id); - const transform::Rigid2d constraint_transform = - constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() * + std::vector submap_ids; + std::vector finished_submap_ids; + std::set newly_finished_submap_node_ids; + { + common::MutexLocker locker(&mutex_); + const auto& constant_data = + data_.trajectory_nodes.at(node_id).constant_data; + submap_ids = InitializeGlobalSubmapPoses( + node_id.trajectory_id, constant_data->time, insertion_submaps); + CHECK_EQ(submap_ids.size(), insertion_submaps.size()); + const SubmapId matching_id = submap_ids.front(); + const transform::Rigid2d local_pose_2d = + transform::Project2D(constant_data->local_pose * + transform::Rigid3d::Rotation( + constant_data->gravity_alignment.inverse())); + const transform::Rigid2d global_pose_2d = + optimization_problem_->submap_data().at(matching_id).global_pose * + constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() * local_pose_2d; - data_.constraints.push_back( - Constraint{submap_id, - node_id, - {transform::Embed3D(constraint_transform), - options_.matcher_translation_weight(), - options_.matcher_rotation_weight()}, - Constraint::INTRA_SUBMAP}); - } + optimization_problem_->AddTrajectoryNode( + matching_id.trajectory_id, + optimization::NodeSpec2D{constant_data->time, local_pose_2d, + global_pose_2d, + constant_data->gravity_alignment}); + for (size_t i = 0; i < insertion_submaps.size(); ++i) { + const SubmapId submap_id = submap_ids[i]; + // Even if this was the last node added to 'submap_id', the submap will + // only be marked as finished in 'data_.submap_data' further below. + CHECK(data_.submap_data.at(submap_id).state == SubmapState::kActive); + data_.submap_data.at(submap_id).node_ids.emplace(node_id); + const transform::Rigid2d constraint_transform = + constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() * + local_pose_2d; + data_.constraints.push_back( + Constraint{submap_id, + node_id, + {transform::Embed3D(constraint_transform), + options_.matcher_translation_weight(), + options_.matcher_rotation_weight()}, + Constraint::INTRA_SUBMAP}); + } - // TODO(gaschler): Consider not searching for constraints against trajectories - // scheduled for deletion. - for (const auto& submap_id_data : data_.submap_data) { - if (submap_id_data.data.state == SubmapState::kFinished) { - CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0); - ComputeConstraint(node_id, submap_id_data.id); + // TODO(gaschler): Consider not searching for constraints against + // trajectories 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) { + if (submap_id_data.data.state == SubmapState::kFinished) { + CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0); + 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) { - const SubmapId 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; + const SubmapId newly_finished_submap_id = submap_ids.front(); // We have a new completed submap, so we look into adding constraints for // 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(); ++num_nodes_since_last_loop_closure_; diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.h b/cartographer/mapping/internal/2d/pose_graph_2d.h index d579585..3b71847 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.h +++ b/cartographer/mapping/internal/2d/pose_graph_2d.h @@ -188,11 +188,7 @@ class PoseGraph2D : public PoseGraph { // Computes constraints for a node and submap pair. void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id) - REQUIRES(mutex_); - - // Adds constraints for older nodes whenever a new submap is finished. - void ComputeConstraintsForOldNodes(const SubmapId& submap_id) - REQUIRES(mutex_); + EXCLUDES(mutex_); // Deletes trajectories waiting for deletion. Must not be called during // constraint search.