在PoseGraph2D::HandleWorkQueue
里,RunOptimization();
之后1
2
3
4
5
6
7
8
9
10
11
12
13// 子图的裁剪,如果没有裁剪器就不裁剪
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle);
}
// 这里只有纯定位时才执行
// 裁剪器完成状态,删除裁剪器
trimmers_.erase(
std::remove_if(trimmers_.begin(), trimmers_.end(),
[](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
return trimmer->IsFinished();
}),
trimmers_.end());trimmers_
的添加是 PoseGraph2D
构造函数 —— PoseGraph2D::AddTrimmer
先看构造函数部分1
2
3
4
5
6
7
8if (options.has_overlapping_submaps_trimmer_2d() )
{
const auto& trimmer_options = options.overlapping_submaps_trimmer_2d();
AddTrimmer(absl::make_unique<OverlappingSubmapsTrimmer2D>(
trimmer_options.fresh_submaps_count(),
trimmer_options.min_covered_area(),
trimmer_options.min_added_submaps_count() ) );
}
三个参数:
- fresh_submaps_count = 2 最新的2个submaps
- min_covered_area = 3.0 最小未被覆盖的面积
- min_added_submaps_count = 5 代表在5个submaps之后才会进行该项操作,这个参数不实用,主要是上面两个
Trims submaps that have less than min_covered_cells_count
cells not overlapped by at least fresh_submaps_count
submaps. 也就是说一些子图如果只有min_covered_cells_count
个栅格没有被fresh_submaps_count
个子图覆盖,那么这些子图被裁剪掉。
1 | void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) |
类PoseGraphTrimmer
的派生类有 PureLocalizationTrimmer
和 OverlappingSubmapsTrimmer2D
。前者属于纯定位部分,这里不看。后者其实主要就实现了一个Trim
函数。
回头看trimmers_.erase
这一句,发现OverlappingSubmapsTrimmer2D::IsFinished()
永远返回 false,所以这一句只有纯定位时才会执行,建图不执行。