trimmer机制 - 移除重复子图

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
8
if (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() ) );
}

三个参数:

  1. fresh_submaps_count = 2 最新的2个submaps
  2. min_covered_area = 3.0 最小未被覆盖的面积
  3. 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
2
3
4
5
6
7
8
9
10
void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer)
{
// C++11 does not allow us to move a unique_ptr into a lambda.
PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
trimmers_.emplace_back(trimmer_ptr);
return WorkItem::Result::kDoNotRunOptimization;
});
}

PoseGraphTrimmer的派生类有 PureLocalizationTrimmerOverlappingSubmapsTrimmer2D。前者属于纯定位部分,这里不看。后者其实主要就实现了一个Trim函数。

回头看trimmers_.erase这一句,发现OverlappingSubmapsTrimmer2D::IsFinished()永远返回 false,所以这一句只有纯定位时才会执行,建图不执行。