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

Introduce timeout for global constraints. (#517) 

This PR introduces a new option that specifies the number of seconds
after which global matcher searches are performed if no recent global
constraints have been found between the submap's and the node's
trajectory.
master
Christoph Schütte 2017-09-14 12:11:54 +02:00 committed by Wolfgang Hess
parent 31b5a6f1a9
commit 5896ead32e
8 changed files with 106 additions and 63 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/mapping/proto/sparse_pose_graph_options.proto
View File

@ -50,4 +50,9 @@ message SparsePoseGraphOptions {
// Whether to output histograms for the pose residuals.
optional bool log_residual_histograms = 9;
// If for the duration specified by this option no global contraint has been
// added between two trajectories, loop closure searches will be performed
// globally rather than in a smaller search window.
optional double global_constraint_search_after_n_seconds = 10;
}

3
cartographer/mapping/sparse_pose_graph.cc
View File

@ -57,6 +57,9 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
parameter_dictionary->GetDouble("global_sampling_ratio"));
options.set_log_residual_histograms(
parameter_dictionary->GetBool("log_residual_histograms"));
options.set_global_constraint_search_after_n_seconds(
parameter_dictionary->GetDouble(
"global_constraint_search_after_n_seconds"));
return options;
}

73
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -173,29 +173,35 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
// Only globally match against submaps not in this trajectory.
if (node_id.trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
const common::Time scan_time = GetLatestScanTime(node_id, submap_id);
const common::Time last_connection_time =
trajectory_connectivity_state_.LastConnectionTime(
node_id.trajectory_id, submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_time <
last_connection_time + common::FromSeconds(
options_.global_constraint_search_after_n_seconds())) {
// If the scan and the submap belong to the same trajectory or if there has
// been a recent global constraint that ties that scan's trajectory to the
// submap's trajectory, it suffices to do a match constrained to a local
// search window.
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.pose;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
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, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get());
} else {
if (trajectory_connectivity_state_.TransitivelyConnected(
node_id.trajectory_id, submap_id.trajectory_id)) {
const transform::Rigid2d initial_relative_pose =
optimization_problem_.submap_data()
.at(submap_id.trajectory_id)
.at(submap_id.submap_index)
.pose.inverse() *
optimization_problem_.node_data()
.at(node_id.trajectory_id)
.at(node_id.node_index)
.pose;
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(),
initial_relative_pose);
}
}
}
@ -300,21 +306,28 @@ void SparsePoseGraph::ComputeConstraintsForScan(
}
}
common::Time SparsePoseGraph::GetLatestScanTime(
const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) const {
common::Time time =
trajectory_nodes_.at(node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(submap_id);
if (!submap_data.node_ids.empty()) {
const mapping::NodeId last_submap_node_id =
*submap_data_.at(submap_id).node_ids.rbegin();
time = std::max(
time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
}
return time;
}
void SparsePoseGraph::UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result) {
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
common::Time time =
trajectory_nodes_.at(constraint.node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(constraint.submap_id);
if (!submap_data.node_ids.empty()) {
const mapping::NodeId last_submap_node_id =
*submap_data_.at(constraint.submap_id).node_ids.rbegin();
time = std::max(
time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
}
const common::Time time =
GetLatestScanTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id,
time);

3
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -99,6 +99,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
std::vector<std::vector<mapping::TrajectoryNode>> GetTrajectoryNodes()
override EXCLUDES(mutex_);
std::vector<Constraint> constraints() override EXCLUDES(mutex_);
common::Time GetLatestScanTime(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) const
REQUIRES(mutex_);
private:
// The current state of the submap in the background threads. When this

1
cartographer/mapping_2d/sparse_pose_graph_test.cc
View File

@ -130,6 +130,7 @@ class SparsePoseGraphTest : public ::testing::Test {
max_num_final_iterations = 200,
global_sampling_ratio = 0.01,
log_residual_histograms = true,
global_constraint_search_after_n_seconds = 10.0,
})text");
sparse_pose_graph_ = common::make_unique<SparsePoseGraph>(
mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()),

79
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -193,33 +193,39 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
inverse_submap_pose * trajectory_nodes_.at(submap_node_id).pose});
}
// Only globally match against submaps not in this trajectory.
if (node_id.trajectory_id != submap_id.trajectory_id &&
global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
// In this situation, 'initial_relative_pose' is:
//
// submap <- global map 2 <- global map 1 <- tracking
// (agreeing on gravity)
//
// Since they possibly came from two disconnected trajectories, the only
// guaranteed connection between the tracking and the submap frames is
// an agreement on the direction of gravity. Therefore, excluding yaw,
// 'initial_relative_pose.rotation()' is a good estimate of the relative
// orientation of the point cloud in the submap frame. Finding the correct
// yaw component will be handled by the matching procedure in the
// FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
constraint_builder_.MaybeAddGlobalConstraint(
const common::Time scan_time = GetLatestScanTime(node_id, submap_id);
const common::Time last_connection_time =
trajectory_connectivity_state_.LastConnectionTime(
node_id.trajectory_id, submap_id.trajectory_id);
if (node_id.trajectory_id == submap_id.trajectory_id ||
scan_time <
last_connection_time + common::FromSeconds(
options_.global_constraint_search_after_n_seconds())) {
// If the scan and the submap belong to the same trajectory or if there has
// been a recent global constraint that ties that scan's trajectory to the
// submap's trajectory, it suffices to do a match constrained to a local
// search window.
constraint_builder_.MaybeAddConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
initial_relative_pose.rotation());
} else {
if (trajectory_connectivity_state_.TransitivelyConnected(
node_id.trajectory_id, submap_id.trajectory_id)) {
constraint_builder_.MaybeAddConstraint(
initial_relative_pose);
} else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) {
// In this situation, 'initial_relative_pose' is:
//
// submap <- global map 2 <- global map 1 <- tracking
// (agreeing on gravity)
//
// Since they possibly came from two disconnected trajectories, the only
// guaranteed connection between the tracking and the submap frames is
// an agreement on the direction of gravity. Therefore, excluding yaw,
// 'initial_relative_pose.rotation()' is a good estimate of the relative
// orientation of the point cloud in the submap frame. Finding the correct
// yaw component will be handled by the matching procedure in the
// FastCorrelativeScanMatcher, and the given yaw is essentially ignored.
constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_data_.at(submap_id).submap.get(), node_id,
trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes,
initial_relative_pose);
}
initial_relative_pose.rotation());
}
}
@ -320,21 +326,28 @@ void SparsePoseGraph::ComputeConstraintsForScan(
}
}
common::Time SparsePoseGraph::GetLatestScanTime(
const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) const {
common::Time time =
trajectory_nodes_.at(node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(submap_id);
if (!submap_data.node_ids.empty()) {
const mapping::NodeId last_submap_node_id =
*submap_data_.at(submap_id).node_ids.rbegin();
time = std::max(
time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
}
return time;
}
void SparsePoseGraph::UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result) {
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
common::Time time =
trajectory_nodes_.at(constraint.node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(constraint.submap_id);
if (!submap_data.node_ids.empty()) {
const mapping::NodeId last_submap_node_id =
*submap_data_.at(constraint.submap_id).node_ids.rbegin();
time = std::max(
time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
}
const common::Time time =
GetLatestScanTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id,
time);

4
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -164,6 +164,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) REQUIRES(mutex_);
common::Time GetLatestScanTime(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) const
REQUIRES(mutex_);
// Logs histograms for the translational and rotational residual of scan
// poses.
void LogResidualHistograms() REQUIRES(mutex_);

1
configuration_files/sparse_pose_graph.lua
View File

@ -79,4 +79,5 @@ SPARSE_POSE_GRAPH = {
max_num_final_iterations = 200,
global_sampling_ratio = 0.003,
log_residual_histograms = true,
global_constraint_search_after_n_seconds = 10.,
}