Apollo的感知融合模块解析

下文主要对百度Apollo的感知模块的fusion部分进行细致深入的了解，我将结合代码、流程图等一起分析，尽可能的将我的认知记录下来，分享大家。需要注意的是，Apollo版本迭代很快，我这里的分析是根据2.5_release版本进行分析的，如果后续版本有所变动，以实际代码为准。

导读

下文我将按照如下流程对模块进行分析：
1. 文字简述
2. 流程图
3. 用函数表达流程
4. 局部细节分析
5. 总结

分析

文字简述

Apollo的融合模块入口自然是在perception.cc里面，这个应不必多说。在perception.cc里面注册了两种融合方法：RegisterFactoryAsyncFusionSubnode()和RegisterFactoryFusionSubnode()。这两种方法在底层实现上大同小异，只是上层的数据流程有所区别。在2.5版本的启动脚本可看到，目前启用的是dag_streaming_lowcost.config，在这个文件内，启动的是FusionSubnode，而不是AsyncFusionSubnode。
在底层实现上，Apollo使用的HM（匈牙利算法）参考，对各个传感器提取到的各个Object进行匹配，最后将匹配后的数据用kalman进行融合，分配对应的track_id，将处理后的数据分类存储，为新的匹配对象进行创建跟踪对象（track_id），并保存。当然，收尾还是要移除跟踪失败的object。

流程图

单纯的用大段文字总结不是我的风格，也不便于理解，我还是喜欢用清晰的流程图来反应程序流程，后续再结合代码分析具体细节，能够更直观的重现代码开发的过程，也可以更好的帮助读者理解。
- Apollo之fusion模块整体流程
- Apollo的fusion模块的数据缓存架构
- FuseFrame流程细节图

代码流程速览

每帧单独融合

 for (size_t i = 0; i < frames.size(); ++i) {
     FuseFrame(frames[i]);
   }

将Obj按照前景、背景分拣

 DecomposeFrameObjects(objects, &foreground_objects, &background_objects);

前景融合
```
 FuseForegroundObjects(&foreground_objects, ref_point, frame->sensor_type,
                       frame->sensor_id, frame->timestamp);
```
1. 匈牙利算法匹配
```
matcher_->Match(tracks, *foreground_objects, options, &assignments,
                &unassigned_tracks, &unassigned_objects,
                &track2measurements_dist, &measurement2tracks_dist);
```
  1. IdAssign
    - 根据fusion_tracks和sensor_objects的交集，将数据分组成unassigned_fusion_tracks和unassigned_sensor_objects，也是HM前的预处理。
  2. 计算关联矩阵
    - 这是HM之前最重要的步骤，直接影响后面融合的效果。这里的HM求得的最优匹配关系就是匹配和最小，而每一条匹配的边长是多少，就是这里的关联矩阵求得的值。如果以后想要对匹配结果优化，这里是一个很重要的地方，这个关联系数能否正确的反应匹配关系？
  3. HM匹配
    1. 拆分成多个二分图（最小连同区），分别求解最优匹配
```
  ComputeConnectedComponents(association_mat, max_dist, &fusion_components, &sensor_components);
```
    1. 对每个子二分图求解最优匹配
```
MinimizeAssignment(loc_mat, &fusion_idxs, &sensor_idxs);
```
2. Update
  - 根据前文HM求得的匹配对，使用kalman进行融合，得到PbfTrack，将跟踪目标、未跟踪上的等数据分类处理。
```
UpdateAssignedTracks(&tracks, *foreground_objects, assignments,       track2measurements_dist);

UpdateUnassignedTracks(&tracks, unassigned_tracks, track2measurements_dist,
                       sensor_type, sensor_id, timestamp);
```
3. CreateNewTracks
  - 为新的跟踪目标创建PbfTrack，这里需要注意的是，代码逻辑仅仅允许是Camera检测并跟踪上的的Obj才可以创建新的PbfTrack，也就是说，Camera的置信度比较高。
```
if (FLAGS_use_navigation_mode) {
  if (is_camera(sensor_type)) {
    CreateNewTracks(*foreground_objects, unassigned_objects);
  }
} else {
  CreateNewTracks(*foreground_objects, unassigned_objects);
}
}
```
移除丢失的跟踪
```
 track_manager_->RemoveLostTracks();
```

局部分析

预处理
- 前面几篇大致的讲解了Apollo的基本架构，每个传感器为一个subnode，在每个subnode里面，Apollo对其数据也做了一定的预处理，这里我们以camera为例：
```
//-- @Zuo yolo
detector_->Multitask(img, CameraDetectorOptions(), &objects, &mask);

//-- @Zuo obj 2d -> 3d
converter_->Convert(&objects);

//-- @Zuo 根据外参将obj从相机坐标系转换到车辆坐标系
transformer_->Transform(&objects);

tracker_->Associate(img, timestamp, &objects);

//-- @Zuo 使用kalman进行跟踪
filter_->Filter(timestamp, &objects);
```
  - camera里面的预处理流程大致为：yolo提取目标 -> 2d目标转为3d目标 -> 根据传感器外参从传感器坐标系转换到车辆坐标系 -> 目标跟踪（dlf/kcf） -> 使用kalman对目标位置进行修正。
触发传感器
- 进入融合函数ProbabilisticFusion::Fuse()后，可以看到有一个publish_sensor_id_的存在，这里是什么意思呢？这也是ProbabilisticFusion和async_fusion的最大区别，前者需要事先设定一个触发传感器，预先将每一帧数据分类存储，直到触发传感器传来数据，则将事先存储好的数据取出进行融合。而后者则是不管传感器类型，来一帧就融一帧。
```
if (GetSensorType(multi_sensor_objects[i].sensor_type) == publish_sensor_id_)
```
关系矩阵
- 在调用HM匹配前，调用了一个函数ComputeAssociationMat，这是用来计算track_objs和sensor_objs之间两两的匹配度。这一步至关重要，直接关系到后续的HM融合的效果好坏。前面我说了，在Apollo的整个体系中，使用HM求解的匹配关系是核心，而这里的HM是求解最小匹配度，而这个匹配度就是ComputeAssociationMat求得的。如何去求匹配度呢，很显然，最直接的方法就是用两个obj的中心距离来反映，但是这样毕竟比较粗糙，我们的世界是三维的，obj还会有速度、朝向等参数，所以Apollo目前使用的方法也是将这一系列的参数按照不同的权重加进来，一起求解得到最后两两之间的匹配度。目前来说，2.5版本的这个方法是有bug的，所以说，后续如果对容和效果不满意，这里应该是重点。
如何区分同类型的不同设备
- 在Apollo中，如果有相同类型的不同设备，系统是否会对其数据做区分？怎么区分？其实，稍微看了Apollo的都清楚，在Apollo里面有sensor_type和sensor_id，很明显，这里是用来区分传感器的类型和设备号的。但是仔细看代码，我们发现目前，在整体的框架中，整个Apollo中用的是std::string GetSensorType(SensorType sensor_type)来获取sensor_id，这很奇葩，完全想不通为什么要这么做。我这里做的修改是，在PbfSensorFrame、SensorObjects等之前转换的过程中，直接使用Obj的sensor_id，而不是上述奇葩方法。
传感器偏爱
- 整个fusion流程走一遍之后就知道，Apollo对Camera是非常之偏爱，对Radar是非常之苛刻。例如：
  - Radar不能创建新的Obj
```
if (FLAGS_use_navigation_mode) {
   if (is_camera(sensor_type)) {
       CreateNewTracks(*foreground_objects, unassigned_objects);
   }
} else {
   CreateNewTracks(*foreground_objects, unassigned_objects);
}
```
  - 在AbleToPublish()中，对Radar检测到的数据做大量的条件检测才能允许发布。
数据类型
- 在Apollo里面主要用到下面几种数据类型来管理目标：Object、SensorObjects、PbfSensorObject、PbfSensorFrame。
  - Object：是其它类型的基类型，为最初Yolo检测类型出来的原始数据存储方式。主要包括一些几何参数等。
  - SensorObjects：是Object的集合，用来管理同一帧下相同传感器的所有Object集合。用std::vector来管理Object。因为它是跟着传感器走的，所以，肯定包含了传感器的相关参数，如传感器外参等。
  - PbfSensorObject：是用于管理跟踪Object。它虽然也是在Object的基础上进行了包装，但是可以理解为是跟Object同一级别的，不过它的应用场景是在跟踪里面的数据存储。相应的，它扩展了相关功能，如invisible_period，用来将超过一定时间的Obj从跟踪队列中删除。
  - PbfSensorFrame：是在PbfSensorObject的基础上扩展的，他和PbfSensorObject的关系，类似于SensorObjects和Object的关系。用来管理同一传感器的同一帧的所有被跟踪上的Obj。也是用std::vector来管理PbfSensorObject，并且多了传感器外参等。
数据结构
- 这个其实没什么好说的，Apollo里面的数据结构风格很统一，不外乎map+queue、map+deque、map+vector这几种方式之一。
变量名解释
- idx = ind = index
- CIPV = closest in-path vehicle

总结

在整个融合体系中，从数据分类的角度看，就两大块——tracks和sensors。前者为已经跟踪上的Obj，后者为待融合的Obj。
融合的关键是HM，而HM的关键是求所有Sensor_object和Track_object两两之间的匹配分数。这个匹配分数怎么求才能更加真实的反应两者之间的匹配紧密度？这可能是后续优化的关键点。
在用HM求得匹配关系后，根据Kalman进行两两融合，然后再更新Tracks，至此，整个Fusion模块基本完结。

Apollo的感知融合模块解析

Apollo的感知融合模块解析

导读

分析

文字简述

流程图

代码流程速览

局部分析

预处理

触发传感器

关系矩阵

如何区分同类型的不同设备

传感器偏爱

数据类型

数据结构

总结

猜你喜欢