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

Fix multi-trajectory loop closure attempts. (#131) 

Before, old scans with new submaps were always matched as if they
belong to connected trajectories.
master
Wolfgang Hess 2016-11-18 10:36:30 +01:00 committed by GitHub
parent 3f0eeec429
commit e703b1cea3
6 changed files with 102 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -127,7 +127,7 @@ void SparsePoseGraph::AddScan(
}
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(j, submaps, matching_submap, insertion_submaps,
ComputeConstraintsForScan(j, matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
}
@ -140,6 +140,39 @@ void SparsePoseGraph::AddWorkItem(std::function<void()> work_item) {
}
}
void SparsePoseGraph::ComputeConstraint(const int scan_index,
const int submap_index) {
const transform::Rigid2d relative_pose =
submap_transforms_[submap_index].inverse() *
point_cloud_poses_[scan_index];
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
const mapping::Submaps* const submap_trajectory =
submap_states_[submap_index].trajectory;
// Only globally match against submaps not in this trajectory.
if (scan_trajectory != submap_trajectory &&
global_localization_samplers_[scan_trajectory]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_,
&trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 &&
reverse_connected_components_.count(submap_trajectory) > 0 &&
reverse_connected_components_.at(scan_trajectory) ==
reverse_connected_components_.at(submap_trajectory);
if (scan_trajectory == submap_trajectory ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
&trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns,
relative_pose);
}
}
}
void SparsePoseGraph::ComputeConstraintsForOldScans(
const mapping::Submap* submap) {
const int submap_index = GetSubmapIndex(submap);
@ -148,20 +181,13 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
const int num_nodes = point_cloud_poses_.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) {
const transform::Rigid2d relative_pose =
submap_transforms_[submap_index].inverse() *
point_cloud_poses_[scan_index];
constraint_builder_.MaybeAddConstraint(
submap_index, submap, scan_index,
&trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns,
relative_pose);
ComputeConstraint(scan_index, submap_index);
}
}
}
void SparsePoseGraph::ComputeConstraintsForScan(
int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submap* matching_submap,
int scan_index, const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) {
@ -194,42 +220,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
Constraint::INTRA_SUBMAP});
}
// Determine if this scan should be globally localized.
const bool run_global_localization =
global_localization_samplers_[scan_trajectory]->Pulse();
CHECK_LT(submap_states_.size(), std::numeric_limits<int>::max());
const int num_submaps = submap_states_.size();
for (int submap_index = 0; submap_index != num_submaps; ++submap_index) {
if (submap_states_[submap_index].finished) {
CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0);
const transform::Rigid2d relative_pose =
submap_transforms_[submap_index].inverse() *
point_cloud_poses_[scan_index];
const auto* submap_trajectory = submap_states_[submap_index].trajectory;
// Only globally match against submaps not in this trajectory.
if (run_global_localization && scan_trajectory != submap_trajectory) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_,
&trajectory_nodes_[scan_index].constant_data->laser_fan_2d.returns);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 &&
reverse_connected_components_.count(submap_trajectory) > 0 &&
reverse_connected_components_.at(scan_trajectory) ==
reverse_connected_components_.at(submap_trajectory);
if (scan_trajectory == submap_trajectory ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
&trajectory_nodes_[scan_index]
.constant_data->laser_fan_2d.returns,
relative_pose);
}
}
ComputeConstraint(scan_index, submap_index);
}
}

7
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -127,12 +127,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(
int scan_index, const mapping::Submaps* scan_trajectory,
const mapping::Submap* matching_submap,
int scan_index, const mapping::Submap* matching_submap,
std::vector<const mapping::Submap*> insertion_submaps,
const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
// Computes constraints for a scan and submap pair.
void ComputeConstraint(const int scan_index, const int submap_index)
REQUIRES(mutex_);
// Adds constraints for older scans whenever a new submap is finished.
void ComputeConstraintsForOldScans(const mapping::Submap* submap)
REQUIRES(mutex_);

16
cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc
View File

@ -196,22 +196,24 @@ void ConstraintBuilder::ComputeConstraint(
if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
filtered_point_cloud, options_.global_localization_min_score(),
&score, &pose_estimate)) {
CHECK_GT(score, options_.global_localization_min_score());
trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
} else {
return;
}
} else {
if (!submap_scan_matcher->fast_correlative_scan_matcher->Match(
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, filtered_point_cloud, options_.min_score(), &score,
&pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
} else {
return;
}
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);
}
}
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);
}
// Use the CSM estimate as both the initial and previous pose. This has the

77
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -122,9 +122,8 @@ int SparsePoseGraph::AddScan(
}
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForScan(time, j, submaps, matching_submap,
insertion_submaps, finished_submap, pose,
covariance);
ComputeConstraintsForScan(time, j, matching_submap, insertion_submaps,
finished_submap, pose, covariance);
});
return j;
}
@ -147,6 +146,38 @@ void SparsePoseGraph::AddImuData(common::Time time,
});
}
void SparsePoseGraph::ComputeConstraint(const int scan_index,
const int submap_index) {
const transform::Rigid3d relative_pose =
submap_transforms_[submap_index].inverse() *
optimization_problem_.node_data().at(scan_index).point_cloud_pose;
const mapping::Submaps* const scan_trajectory =
trajectory_nodes_[scan_index].constant_data->trajectory;
const mapping::Submaps* const submap_trajectory =
submap_states_[submap_index].trajectory;
// Only globally match against submaps not in this trajectory.
if (scan_trajectory != submap_trajectory &&
global_localization_samplers_[scan_trajectory]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_,
trajectory_nodes_);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 &&
reverse_connected_components_.count(submap_trajectory) > 0 &&
reverse_connected_components_.at(scan_trajectory) ==
reverse_connected_components_.at(submap_trajectory);
if (scan_trajectory == submap_trajectory ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
trajectory_nodes_, relative_pose);
}
}
}
void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
const int submap_index = GetSubmapIndex(submap);
const auto& node_data = optimization_problem_.node_data();
@ -155,20 +186,15 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
const int num_nodes = node_data.size();
for (int scan_index = 0; scan_index < num_nodes; ++scan_index) {
if (submap_states_[submap_index].scan_indices.count(scan_index) == 0) {
const transform::Rigid3d relative_pose =
submap_transforms_[submap_index].inverse() *
node_data[scan_index].point_cloud_pose;
constraint_builder_.MaybeAddConstraint(submap_index, submap, scan_index,
trajectory_nodes_, relative_pose);
ComputeConstraint(scan_index, submap_index);
}
}
}
void SparsePoseGraph::ComputeConstraintsForScan(
const common::Time time, const int scan_index,
const Submaps* scan_trajectory, const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps, const Submap* finished_submap,
const transform::Rigid3d& pose,
const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) {
GrowSubmapTransformsAsNeeded(insertion_submaps);
const int matching_index = GetSubmapIndex(matching_submap);
@ -194,39 +220,12 @@ void SparsePoseGraph::ComputeConstraintsForScan(
Constraint::INTRA_SUBMAP});
}
// Determine if this scan should be globally localized.
const bool run_global_localization =
global_localization_samplers_[scan_trajectory]->Pulse();
CHECK_LT(submap_states_.size(), std::numeric_limits<int>::max());
const int num_submaps = submap_states_.size();
for (int submap_index = 0; submap_index != num_submaps; ++submap_index) {
if (submap_states_[submap_index].finished) {
CHECK_EQ(submap_states_[submap_index].scan_indices.count(scan_index), 0);
const transform::Rigid3d relative_pose =
submap_transforms_[submap_index].inverse() * optimized_pose;
const auto* submap_trajectory = submap_states_[submap_index].trajectory;
// Only globally match against submaps not in this trajectory.
if (run_global_localization && scan_trajectory != submap_trajectory) {
constraint_builder_.MaybeAddGlobalConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
scan_trajectory, submap_trajectory, &trajectory_connectivity_,
trajectory_nodes_);
} else {
const bool scan_and_submap_trajectories_connected =
reverse_connected_components_.count(scan_trajectory) > 0 &&
reverse_connected_components_.count(submap_trajectory) > 0 &&
reverse_connected_components_.at(scan_trajectory) ==
reverse_connected_components_.at(submap_trajectory);
if (scan_trajectory == submap_trajectory ||
scan_and_submap_trajectories_connected) {
constraint_builder_.MaybeAddConstraint(
submap_index, submap_states_[submap_index].submap, scan_index,
trajectory_nodes_, relative_pose);
}
}
ComputeConstraint(scan_index, submap_index);
}
}

10
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -129,12 +129,15 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Adds constraints for a scan, and starts scan matching in the background.
void ComputeConstraintsForScan(
common::Time time, int scan_index, const Submaps* scan_trajectory,
const Submap* matching_submap,
common::Time time, int scan_index, const Submap* matching_submap,
std::vector<const Submap*> insertion_submaps,
const Submap* finished_submap, const transform::Rigid3d& pose,
const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
// Computes constraints for a scan and submap pair.
void ComputeConstraint(const int scan_index, const int submap_index)
REQUIRES(mutex_);
// Adds constraints for older scans whenever a new submap is finished.
void ComputeConstraintsForOldScans(const Submap* submap) REQUIRES(mutex_);
@ -167,7 +170,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_);
// We globally localize a fraction of the scans from each trajectory.
std::unordered_map<const Submaps*, std::unique_ptr<common::FixedRatioSampler>>
std::unordered_map<const mapping::Submaps*,
std::unique_ptr<common::FixedRatioSampler>>
global_localization_samplers_ GUARDED_BY(mutex_);
// Number of scans added since last loop closure.

10
cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc
View File

@ -146,7 +146,8 @@ void ConstraintBuilder::WhenDone(
const std::function<void(const ConstraintBuilder::Result&)> callback) {
common::MutexLocker locker(&mutex_);
CHECK(when_done_ == nullptr);
when_done_.reset(new std::function<void(const Result&)>(callback));
when_done_ =
common::make_unique<std::function<void(const Result&)>>(callback);
++pending_computations_[current_computation_];
const int current_computation = current_computation_;
thread_pool_->Schedule(
@ -223,13 +224,14 @@ void ConstraintBuilder::ComputeConstraint(
CHECK(!match_full_submap) << "match_full_submap not supported for 3D.";
if (!submap_scan_matcher->fast_correlative_scan_matcher->Match(
if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
initial_pose, filtered_point_cloud, point_cloud, options_.min_score(),
&score, &pose_estimate)) {
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
} else {
return;
}
// We've reported a successful local match.
CHECK_GT(score, options_.min_score());
{
common::MutexLocker locker(&mutex_);
score_histogram_.Add(score);