Apollo 5.0源码学习笔记（二）| 感知模块 | 融合模块

本系列博客旨在记录自己在学习百度无人驾驶开源框架Apollo的心得和体会，欢迎大家阅读和点赞，并提出宝贵意见，大家相互学习，如需转载，请注明出处，谢谢！

这篇文章主要介绍Apollo 5.0感知模块中的融合模块。由于不同传感器各有优缺点，传感器融合的必要性不言而喻，Apollo融合模块做的是目标级融合，即首先不同传感器自己先进行目标检测检测和目标跟踪，得到目标序列后作为融合模块的输入，进行进一步的目标推理，从而得到更加准确可靠的感知结果。

一、融合入口类

在上一篇文章Apollo 5.0源码学习笔记（一）| 感知模块 | 感知框架总览我提到了感知模块所有子模块的入口类都定义在modules/perception/onboard/component文件夹下对应源文件中，如融合模块的入口类就是定义在modules/perception/onboard/component/fusion_component.h中的FusionComponent类。
输入： SensorFrameMessage类型消息；
输出： PerceptionObstacles类型消息；
FusionComponent类的参数配置protobuf文件为：modules/perception/onboard/proto/fusion_component_config.proto，具体参数实现定义在文件modules/perception/production/conf/perception/fusion/fusion_component_conf.pb.txt中：

fusion_method: "ProbabilisticFusion"
fusion_main_sensor: "velodyne128"
object_in_roi_check: true
radius_for_roi_object_check: 120
output_obstacles_channel_name: "/apollo/perception/obstacles"
output_viz_fused_content_channel_name: "/perception/inner/visualization/FusedObjects"

从配置参数可以看出，融合的主传感器设置为velodyne128，融合方法使用的是ProbabilisticFusion，发布融合后目标障碍物消息的话题名为/apollo/perception/obstacles。

FusionComponent类主要负责接收融合所需要的来自不同传感器的目标序列消息，然后利用ProbabilisticFusion融合类完成实际融合工作，得到融合后感知结果，最后把融合结果在FusionComponent类中发布，供其他模块使用。

二、fusion下基础类含义说明

在利用不同传感器感知结果来进行目标级融合时，会涉及到对很多单个目标、数据帧所有目标以及多帧目标等数据的存储问题，因此会定义很多数据结构来完成这个工作，这给阅读源码带来了很多困惑，因此此处首先对这些基础自定义数据结构进行一个简单总结，方便后面的解释。

这些基础类主要定义在fusion/base文件夹下：
SensorObject类——某单一传感器的一个目标，其实就是包含了某个传感器的相关属性定义和一个base::Object成员变量；
SensorFrame类——某传感器的一帧所有目标数据，定义了std::vector<SensorObject>成员变量；
Sensor类——某传感器历史多帧信息，定义有std::deque<SensorFramePtr>类型成员变量，并提供获取最新数据帧的一些接口函数；
SensorDataManager类——最终在ProbabilisticFusion类中作为成员变量用来存储<sensor_id, SensorPtr>键值对，即存储多个传感器的多帧历史信息的类，定义有std::unorderd_map<std::string, SensorPtr> sensors_成员变量，作为ProbabilisticFusion类的数据输入缓存；
Track类——单个跟踪目标，在该类中完成大部分跟踪目标状态更新工作；
Scene类——前景和背景跟踪目标的管理，定义有std::vector<TrackPtr> forground_tracks_和std::vector<TrackPtr> background_tracks_；
PbfTracker类——单个跟踪目标，也是主要的融合算法发生过程所在，定义了不同的融合算法类。

二、融合类ProbabilisticFusion解析

融合主要过程是通过ProbabilisticFusion类完成的，该类定义在modules/perception/fusion/lib/fusion_system/probabilistic_fusion/probabilistic_fusion.h中，该类继承自BaseFusionSystem类。下面按照一些主要函数进行分别解析。

ProbabilisticFusion::Init函数

主要是对该类进行一些参数配置，并根据这些配置的参数，来实例化具体的算法类。
参数配置protobuf文件定义在modules/perception/proto/probabilistic_fusion_config.proto，具体实例化的参数定义在modules/perception/production/data/perception/fusion/probabilistic_fusion.pt中：

use_lidar: true
use_radar: true
use_camera: true
tracker_method: "PbfTracker"
data_association_method: "HMAssociation"
gate_keeper_method: "PbfGatekeeper"
prohibition_sensors: "radar_front"

max_lidar_invisible_period: 0.25
max_radar_invisible_period: 0.50
max_camera_invisible_period: 0.75

max_cached_frame_num: 50

可以看出实际融合的传感器包括lidar、radar和camera，以及实际使用的跟踪算法、数据关联算法、门限保持方法的具体类名，另外还定义了其他一些参数。

ProbabilisticFusion::CreateNewTracks函数

目的：创建新跟踪目标
prohibition_sensors: "radar_front"在modules/perception/production/data/perception/fusion/probabilistic_fusion.pt文件中定义；
首先判断当前检测帧是否是prohibition_sensors中一种，如果是，则不用来创建跟踪目标，可以看出，不用radar来创建跟踪目标。
如果真实需要创建新的跟踪目标，首先从TrackPool中获取共享指针track，然后利用当前检测结果Initialize该Track跟踪目标之后添加到scenes_中；
然后创建一个PbfTracker，Init该track，添加到成员变量trackers_中，即trackers_保存PbfTracker序列，PbfTracker中存储的是TrackPtr；

由于track一直是同一个指针，其实scenes_和trackers_存储的是同一跟踪目标序列，分开在不同的类中对跟踪目标进行更新。

PbfTracker::Init函数通过调用InitMethods函数，从而利用track指针构造了每个算法类，例如motion_fusion_等，这样就直接能够在motion_fusion_`中通过指针更新track的内容了。

新版本的融合类没有把所有有关跟踪目标更新操作都放在PbfTracker类中了，而是在PbfTracker类中保存一个Track指针，由各个motion_fusion_、shape_fusion_等类共同保存该指针（即是同一个跟踪目标），因此把更新操作分担给了其他各个功能类完成，在Track中只是进行匹配上传感器的信息更新和管理。

主要保存目标属性的成员变量：Track::fused_object_是FusedObjectPtr类型，其主要保存的变量是ObjectPtr。

ProbabilisticFusion::UpdateAssignedTracks函数

目的：更新匹配对跟踪目标
利用匹配上的检测目标，更新跟踪目标。

调用的是PbfTracker::UpdateWithMeasurement函数，依次完成存在性、运动、形状和类型的融合；

最后调用track_->UpdateWithSensorObject，在该函数内部，主要是利用当前匹配的检测目标，来更新跟踪目标中存储的当前检测目标map内容，一共存储了lidar_objects_,radar_objects_和camera_objects_三种；

函数内部依次调用：
1、DstExistanceFusion::UpdateWithMeasurement:
利用DS理论完成对跟踪目标存在性的判定，设置Track中的toic_p_、existance_prob_属性；

2、KalmanMotionFusion::UpdateWithMeasurement：
利用Kalman滤波，完成对目标状态滤波，设置Track中的
anchor_point、center、velocity、acceleration、center_uncertainty、velocity_uncertainty、acceleration_uncertainty属性；

3、PbfShapeFusion::UpdateWithMeasurement：
根据匹配的测量量，完成对目标的形状推理，设置Track中的size、direction、theta、polygon属性，同时也更新了Track的center、anchor_point属性；

4、DstTypeFusion::UpdateWithMeasurement：
利用DS理论，完成对跟踪目标的type、sub_type、type_probs属性更新