From 32583e0c5d40aae6ef4712194f4c59d012d891e1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 11 May 2017 15:08:27 +0200 Subject: [PATCH] Use trajectory IDs for global samplers. (#278) Related to #256. PAIR=SirVer --- cartographer/mapping_2d/sparse_pose_graph.cc | 6 +++--- cartographer/mapping_2d/sparse_pose_graph.h | 3 +-- cartographer/mapping_3d/sparse_pose_graph.cc | 6 +++--- cartographer/mapping_3d/sparse_pose_graph.h | 3 +-- 4 files changed, 8 insertions(+), 10 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 8263704..5c05aec 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -128,8 +128,8 @@ void SparsePoseGraph::AddScan( : nullptr; // Make sure we have a sampler for this trajectory. - if (!global_localization_samplers_[trajectory]) { - global_localization_samplers_[trajectory] = + if (!global_localization_samplers_[trajectory_id]) { + global_localization_samplers_[trajectory_id] = common::make_unique( options_.global_sampling_ratio()); } @@ -178,7 +178,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, // Only globally match against submaps not in this trajectory. if (scan_trajectory_id != submap_trajectory_id && - global_localization_samplers_[scan_trajectory]->Pulse()) { + global_localization_samplers_[scan_trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_states_[submap_index].submap, scan_index, scan_trajectory_id, &trajectory_connectivity_, diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 48c78a2..b3b52a5 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -179,8 +179,7 @@ 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> + std::unordered_map> global_localization_samplers_ GUARDED_BY(mutex_); // Number of scans added since last loop closure. diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 40dcad6..59bc756 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -123,8 +123,8 @@ void SparsePoseGraph::AddScan( insertion_submaps.front()->finished ? insertion_submaps.front() : nullptr; // Make sure we have a sampler for this trajectory. - if (!global_localization_samplers_[trajectory]) { - global_localization_samplers_[trajectory] = + if (!global_localization_samplers_[trajectory_id]) { + global_localization_samplers_[trajectory_id] = common::make_unique( options_.global_sampling_ratio()); } @@ -178,7 +178,7 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, // Only globally match against submaps not in this trajectory. if (scan_trajectory_id != submap_trajectory_id && - global_localization_samplers_[scan_trajectory]->Pulse()) { + global_localization_samplers_[scan_trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_states_[submap_index].submap, scan_index, scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 274fc29..db0efc9 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -179,8 +179,7 @@ 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> + std::unordered_map> global_localization_samplers_ GUARDED_BY(mutex_); // Number of scans added since last loop closure.