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

Pose graph: do not mark all submaps finished when finalizing a trajectory. (#784) 

I think this was a mistake in #563 (@jihoonl). Not all submaps should be marked as finished, only those pertaining to the trajectory being finalized.
master
Juraj Oršulić 2018-01-04 10:11:28 +01:00 committed by Wally B. Feed
parent dbb3f7cde4
commit 9ee65293d2
2 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
cartographer/mapping_2d/pose_graph.cc
View File

@ -381,9 +381,8 @@ void PoseGraph::FinishTrajectory(const int trajectory_id) {
CHECK_EQ(finished_trajectories_.count(trajectory_id), 0); CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
finished_trajectories_.insert(trajectory_id); finished_trajectories_.insert(trajectory_id);
auto submap_data = optimization_problem_.submap_data(); for (const auto& submap : submap_data_.trajectory(trajectory_id)) {
for (auto submap_id_data : submap_data) { submap_data_.at(submap.id).state = SubmapState::kFinished;
submap_data_.at(submap_id_data.id).state = SubmapState::kFinished;
} }
// TODO(jihoonl): Refactor HandleWorkQueue() logic from // TODO(jihoonl): Refactor HandleWorkQueue() logic from
// ComputeConstraintsForNode and call from here // ComputeConstraintsForNode and call from here

5
cartographer/mapping_3d/pose_graph.cc
View File

@ -398,9 +398,8 @@ void PoseGraph::FinishTrajectory(const int trajectory_id) {
CHECK_EQ(finished_trajectories_.count(trajectory_id), 0); CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
finished_trajectories_.insert(trajectory_id); finished_trajectories_.insert(trajectory_id);
auto submap_data = optimization_problem_.submap_data(); for (const auto& submap : submap_data_.trajectory(trajectory_id)) {
for (auto submap_id_data : submap_data) { submap_data_.at(submap.id).state = SubmapState::kFinished;
submap_data_.at(submap_id_data.id).state = SubmapState::kFinished;
} }
// TODO(jihoonl): Refactor HandleWorkQueue() logic from // TODO(jihoonl): Refactor HandleWorkQueue() logic from
// ComputeConstraintsForNode and call from here // ComputeConstraintsForNode and call from here