前端 2. 传感器数据的同步及融合-RangeDataCollator
如果只用一个雷达,可以不看这段

激光雷达点云数据均为作为主要输入,使用时无需考虑具体几个传感器和类型,可认为是一个雷达产生的点云数据。但实际cartographer通过RangeDataCollator类将多种传感器进行了融合,并进行了时间同步,最后形成对应的时间戳,pose和点云集合。在真正使用时通过畸变校准,并构建scan match和插入submap的传感器数据rangedata。

有多个雷达的点云信息,它们的各个点可能时间会重合,因此需要将所有雷达的点云信息进行时间的整理,保证所有点的时间是单调的。多个雷达数据合并到一起,它们的原点可能不一样,因此要保存各自的原点。

头文件部分:

1
2
3
4
5
6
7
8
9
10
11
12
  // 插入集合
sensor::TimedPointCloudOriginData AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data);

private:
// 融合
sensor::TimedPointCloudOriginData CropAndMerge();
// 期望处理传感器类型清单
const std::set<std::string> expected_sensor_ids_;
// 同步和融合后集合,每种传感器至多一帧点云数据
std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data)
{
// 检测该点云数据 sensor_id 是否在期望的sensor_ids里面
CHECK_NE(expected_sensor_ids_.count(sensor_id), 0);
// TODO(gaschler): These two cases can probably be one.
// 此传感器类型数据已有
if (id_to_pending_data_.count(sensor_id) != 0)
{
current_start_ = current_end_;
// When we have two messages of the same sensor, move forward the older of
// the two (do not send out current).
/* 如果当前融合时间段内融合结果中已包含同一ID的传感器数据,则应采用最新的点云数据进行替换
但是结束的时间戳仍然采用原来的时间刻度,开始进行数据截取合并CropAndMerge()并返回 */
// std::map<std::string, sensor::TimedPointCloudData> id_to_pending_data_;
current_end_ = id_to_pending_data_.at(sensor_id).time;
auto result = CropAndMerge();
id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
return result;
}
id_to_pending_data_.emplace(sensor_id, timed_point_cloud_data);
// 若现在收到的数据类型未全,即期望收到种类未全,直接退出,无需融合
if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
return {};
}
current_start_ = current_end_;
// We have messages from all sensors, move forward to oldest.
common::Time oldest_timestamp = common::Time::max();
// 找传感器数据中最早的时间戳
for (const auto& pair : id_to_pending_data_) {
oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
}
/* current_end_是下次融合的开始时间,是本次融合的最后时间刻度
但其实是此次融合所有传感器中最早的时间戳*/
current_end_ = oldest_timestamp;
return CropAndMerge();
}

进一步处理在CropAndMerge,这个函数十分复杂。 简单理解如果有一个传感器频率较高,已经来过一帧并进行了缓存,另外一个未来,
这个传感器又来一帧,则先进行截取合并送出结果(实际上就是上帧缓存的点云直接发出),
然后将新来的一帧替换换来的buffer。

参考: cartographer 代码思想解读 RangeDataCollator