Apollo 5.0源码学习笔记（三）| 感知模块 | 融合模块 | 数据关联

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

在上一篇博客Apollo 5.0源码学习笔记（二）| 感知模块 | 融合模块中总体概述了Apollo 5.0 中感知融合模块的整个处理流程，但其中还有很多细节没有涉及，其中非常重要的一个细节就是数据关联。

数据关联是tracking-by-detection跟踪模式中非常重要的一个环节，其通过不同的度量准则计算跟踪目标与当前测量量（检测目标）之间的“距离”，然后利用这些“距离”值解决一个二分图匹配问题，从而得到数据关联结果。

数据关联结果包括三类，分别是：匹配对、未匹配的检测目标和未匹配的跟踪目标。
匹配对即该跟踪目标在当前帧认为有一个测量量与之匹配，那么我们就可以利用这个新的测量量来更新跟踪目标状态，例如将该测量量作为Kalman滤波的测量输入，更新跟踪目标状态；
未匹配的检测目标即该检测目标没有与任何跟踪目标匹配上，则可以认为是新产生的检测目标，从而可以利用该检测目标创建一个新的跟踪目标，加入到原有跟踪目标序列中；
未匹配的跟踪目标即该跟踪目标当前没有与任何检测目标匹配上，则可以认为该跟踪目标当前帧没有被检测到，即出现了丢失（漏检）。

掌握了上述的数据关联的基本思路，也就不难理解Apollo代码中数据关联的处理流程了。

HMTrackersObjectsAssociation——数据关联类

Apollo 5.0感知融合模块的数据关联算法由类HMTrackersObjectsAssociation实现，定义在modules/perception/fusion/lib/data_association/hm_data_association/hm_tracks_objects_match.h中。

HMTrackersObjectsAssociation类继承自基类BaseDataAssociation。

输入输出接口：

输入： 当前检测帧目标、跟踪目标序列
输出： 数据关联结果（包括匹配对、未匹配跟踪和未匹配检测索引、以及每个跟踪目标到检测目标最近距离向量、每个测量目标到跟踪目标最近距离向量）

总体流程：

Associate函数为主接口函数；
首先取出当前检测帧和跟踪目标序列中属于ForegroundObjects目标，得到sensor_objects和fusion_tracks两个向量；
如果上述两个向量任一一个为空，则赋予初始值直接返回；

利用检测帧传感器id和时间戳，对TrackObjectDistance类进行ResetProjectionCache，主要是重置TrackObjectDistance类中projection_cache_成员；

调用IdAssign对数据关联结果进行初始构造；
然后利用HMTrackersObjectsAssociation::ComputeAssociationDistanceMat函数对未匹配的跟踪和未匹配的检测目标计算匹配矩阵：

初始化association_result->track2measurements_dist长度为num_track，全部为0，初始化association_result->measurement2track_dist长度为num_measurement，全部为0；

定义track_ind_g2l存储原始输入跟踪目标序列参与后续匹配的索引位置对应在实际参与后续匹配的序列中的索引位置，不参与后续匹配的位置置为-1；
定义measurement_ind_g2l存储检测目标的global对应在local中的位置；
然后调用HMTrackersObjectsAssociation::MinimizeAssignment函数对匹配矩阵进行最优匹配，得到匹配结果；
利用二分图理论最小化距离矩阵匹配结果，得到最终匹配结果；

调用HMTrackersObjectsAssociation::PostIdAssign对未匹配的检测和未匹配的跟踪目标，进行进一步的后处理，得到新的匹配对；
调用HMTrackersObjectsAssociation::GenerateUnassignedData函数针对新增加的匹配对，重新调整新的未匹配的结果向量；
调用HMTrackersObjectsAssociation::ComputeDistance函数计算匹配目标距离，填充association_result->track2measurements_dist和association_result->measurement2track_dist；

HMTrackersObjectsAssociation::IdAssign函数：

输入： 检测目标序列、跟踪目标序列、当前检测是否是毫米波雷达
输出： 匹配对向量、未匹配跟踪向量、未匹配检测向量
概述： 该函数主要目的是在进行数据关联计算前优先匹配了检测结果中track_id相同的目标（因为Apollo中lidar、radar、camear这些传感器在各自的检测模块中也进行了tracking，因此每个目标也带有track_id等信息）。主要是利用相机和激光雷达的检测结果，毫米波的不用。

处理流程：
首先判断如果序列为空或者是毫米波雷达（do_nothing)，则给输出赋初始值返回；
遍历所有跟踪目标，取出该跟踪目标中保存的上一帧与当前检测目标传感器相同的匹配目标，并用一个sensor_id_2_track_ind也就是一个map<track_id,track_index>记录这个检测目标的track_id号，即sensor_id_2_track_ind这个字典记录了每个跟踪目标的track_id以及这个跟踪目标在现在跟踪目标序列中的位置索引，方便后续定位；

遍历所有检测目标，然后从这些检测目标中判断track_id与sensor_id_2_track_ind中track_id相同目标，认为这两个检测目标和跟踪目标匹配上，其实就是从跟踪目标中首先匹配检测目标中track_id相同的目标；

最后记录那些没有匹配上的跟踪目标和检测目标索引，传出；

HMTrackersObjectsAssociation::ComputeAssociationDistanceMat函数：

输入： 检测目标序列、跟踪目标序列、当前检测帧到绝对坐标系的平移量ref_point、未匹配上的跟踪目标索引、未匹配上的检测目标索引；
输出： 匹配矩阵；
处理流程：
判断跟踪目标与每个检测目标中心点距离，如果距离小于30m，则调用TrackObjectDistance::Compute函数计算得到距离，否则距离取s_match_distance_thresh_（4m），赋值给距离矩阵；

TrackObjectDistance::Compute函数

输入： 一个跟踪目标、一个检测目标
输出： 匹配距离值
概述： 通过判断当前检测是来自哪个传感器，以及跟踪目标是否有某个传感器的检测结果，来选择不同的距离计算函数；
处理流程：
优先判断检测是否是激光雷达，然后依次判断跟踪目标中如果含有激光雷达、毫米波雷达或相机，则依次调用对应的距离计算函数完成距离计算，并取最终最小的距离作为输出距离；
1、当前检测是激光雷达目标：
如果跟踪目标有激光雷达检测结果，则ComputeLidarLidar，首先通过激光雷达目标间中心点距离是否大于10m来判断是否进一步用轮廓中心点来进行匹配距离计算，否则返回float最大值；
如果跟踪目标有毫米波雷达检测结果，则ComputeLidarRadar，和激光雷达一样的处理方式；
如果跟踪目标有相机检测结果，首先给出一个激光雷达跟踪ID是否相同的判断，然后调用ComputeLidarCamera计算得到距离；
2、当前检测是毫米波雷达目标：