Use trajectory IDs for global samplers. (#278)

Related to #256.

PAIR=SirVer
master
Wolfgang Hess 2017-05-11 15:08:27 +02:00 committed by GitHub
parent 1a9b18db69
commit 32583e0c5d
4 changed files with 8 additions and 10 deletions

View File

@ -128,8 +128,8 @@ void SparsePoseGraph::AddScan(
: nullptr; : nullptr;
// Make sure we have a sampler for this trajectory. // Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory]) { if (!global_localization_samplers_[trajectory_id]) {
global_localization_samplers_[trajectory] = global_localization_samplers_[trajectory_id] =
common::make_unique<common::FixedRatioSampler>( common::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio()); options_.global_sampling_ratio());
} }
@ -178,7 +178,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
// Only globally match against submaps not in this trajectory. // Only globally match against submaps not in this trajectory.
if (scan_trajectory_id != submap_trajectory_id && if (scan_trajectory_id != submap_trajectory_id &&
global_localization_samplers_[scan_trajectory]->Pulse()) { global_localization_samplers_[scan_trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_states_[submap_index].submap, scan_index, submap_id, submap_states_[submap_index].submap, scan_index,
scan_trajectory_id, &trajectory_connectivity_, scan_trajectory_id, &trajectory_connectivity_,

View File

@ -179,8 +179,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_); mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_);
// We globally localize a fraction of the scans from each trajectory. // We globally localize a fraction of the scans from each trajectory.
std::unordered_map<const mapping::Submaps*, std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
std::unique_ptr<common::FixedRatioSampler>>
global_localization_samplers_ GUARDED_BY(mutex_); global_localization_samplers_ GUARDED_BY(mutex_);
// Number of scans added since last loop closure. // Number of scans added since last loop closure.

View File

@ -123,8 +123,8 @@ void SparsePoseGraph::AddScan(
insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr; insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr;
// Make sure we have a sampler for this trajectory. // Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory]) { if (!global_localization_samplers_[trajectory_id]) {
global_localization_samplers_[trajectory] = global_localization_samplers_[trajectory_id] =
common::make_unique<common::FixedRatioSampler>( common::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio()); options_.global_sampling_ratio());
} }
@ -178,7 +178,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index,
// Only globally match against submaps not in this trajectory. // Only globally match against submaps not in this trajectory.
if (scan_trajectory_id != submap_trajectory_id && if (scan_trajectory_id != submap_trajectory_id &&
global_localization_samplers_[scan_trajectory]->Pulse()) { global_localization_samplers_[scan_trajectory_id]->Pulse()) {
constraint_builder_.MaybeAddGlobalConstraint( constraint_builder_.MaybeAddGlobalConstraint(
submap_id, submap_states_[submap_index].submap, scan_index, submap_id, submap_states_[submap_index].submap, scan_index,
scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_); scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_);

View File

@ -179,8 +179,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_); mapping::TrajectoryConnectivity trajectory_connectivity_ GUARDED_BY(mutex_);
// We globally localize a fraction of the scans from each trajectory. // We globally localize a fraction of the scans from each trajectory.
std::unordered_map<const mapping::Submaps*, std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
std::unique_ptr<common::FixedRatioSampler>>
global_localization_samplers_ GUARDED_BY(mutex_); global_localization_samplers_ GUARDED_BY(mutex_);
// Number of scans added since last loop closure. // Number of scans added since last loop closure.