diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc index 6c1cdd2..39b9787 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.cc @@ -140,7 +140,7 @@ std::vector FindSubmapIdsToTrim( const SubmapCoverageGrid2D& coverage_grid, const std::set& all_submap_ids, uint16 fresh_submaps_count, uint16 min_covered_cells_count) { - std::map cells_covered_by_submap; + std::map submap_to_covered_cells_count; for (const auto& cell : coverage_grid.cells()) { std::vector> submaps_per_cell( cell.second); @@ -158,21 +158,20 @@ std::vector FindSubmapIdsToTrim( submaps_per_cell.end()); } for (const std::pair& submap : submaps_per_cell) { - ++cells_covered_by_submap[submap.first]; + ++submap_to_covered_cells_count[submap.first]; } } std::vector submap_ids_to_keep; - for (const auto& id_to_cells_count : cells_covered_by_submap) { + for (const auto& id_to_cells_count : submap_to_covered_cells_count) { if (id_to_cells_count.second < min_covered_cells_count) continue; submap_ids_to_keep.push_back(id_to_cells_count.first); } DCHECK(std::is_sorted(submap_ids_to_keep.begin(), submap_ids_to_keep.end())); - std::vector result; std::set_difference(all_submap_ids.begin(), all_submap_ids.end(), submap_ids_to_keep.begin(), submap_ids_to_keep.end(), - std::inserter(result, result.begin())); + std::back_inserter(result)); return result; } @@ -196,7 +195,6 @@ void OverlappingSubmapsTrimmer2D::Trim(Trimmable* pose_graph) { for (const SubmapId& id : submap_ids_to_remove) { pose_graph->MarkSubmapAsTrimmed(id); } - finished_ = true; } } // namespace mapping diff --git a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc index f60f161..39cc6d6 100644 --- a/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc +++ b/cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d_test.cc @@ -164,7 +164,7 @@ TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) { // // The background submap should be trimmed, since it has only 3 cells // not-covered by another submap. -TEST_F(OverlappingSubmapsTrimmer2DTest, ) { +TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) { AddSquareSubmap(Rigid2d::Identity(), 0 /* submap_index */, 2 /* num_cells */, true /* is_finished */); AddSquareSubmap(Rigid2d::Translation(Eigen::Vector2d(1., 1.)),