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

Fine-grained locking for PoseGraph2D constraints. (#1327) 

This is #1279 for 2D.
master
Wolfgang Hess 2018-07-25 15:26:29 +02:00 committed by Wally B. Feed
parent 4ba9d9168b
commit f61a944938
2 changed files with 104 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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<const Submap2D*>(
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<const Submap2D*>(
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<const Submap2D*>(
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<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);
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<SubmapId> submap_ids;
std::vector<SubmapId> finished_submap_ids;
std::set<NodeId> 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_;

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

@ -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.