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流程细节图
    

代码流程速览

1. 每帧单独融合

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

2. 将Obj按照前景、背景分拣

    DecomposeFrameObjects(objects, &foreground_objects, &background_objects);

3. 前景融合

    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);
      }
      }

4. 移除丢失的跟踪

    track_manager_->RemoveLostTracks();

局部分析

1. 预处理

   • 前面几篇大致的讲解了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对目标位置进行修正。

2. 触发传感器

   • 进入融合函数ProbabilisticFusion::Fuse()后，可以看到有一个publish_sensor_id_的存在，这里是什么意思呢？这也是ProbabilisticFusion和async_fusion的最大区别，前者需要事先设定一个触发传感器，预先将每一帧数据分类存储，直到触发传感器传来数据，则将事先存储好的数据取出进行融合。而后者则是不管传感器类型，来一帧就融一帧。

     if (GetSensorType(multi_sensor_objects[i].sensor_type) == publish_sensor_id_)

3. 关系矩阵

   • 在调用HM匹配前，调用了一个函数ComputeAssociationMat，这是用来计算track_objs和sensor_objs之间两两的匹配度。这一步至关重要，直接关系到后续的HM融合的效果好坏。前面我说了，在Apollo的整个体系中，使用HM求解的匹配关系是核心，而这里的HM是求解最小匹配度，而这个匹配度就是ComputeAssociationMat求得的。如何去求匹配度呢，很显然，最直接的方法就是用两个obj的中心距离来反映，但是这样毕竟比较粗糙，我们的世界是三维的，obj还会有速度、朝向等参数，所以Apollo目前使用的方法也是将这一系列的参数按照不同的权重加进来，一起求解得到最后两两之间的匹配度。目前来说，2.5版本的这个方法是有bug的，所以说，后续如果对容和效果不满意，这里应该是重点。

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

   • 在Apollo中，如果有相同类型的不同设备，系统是否会对其数据做区分？怎么区分？其实，稍微看了Apollo的都清楚，在Apollo里面有sensor_type和sensor_id，很明显，这里是用来区分传感器的类型和设备号的。但是仔细看代码，我们发现目前，在整体的框架中，整个Apollo中用的是std::string GetSensorType(SensorType sensor_type)来获取sensor_id，这很奇葩，完全想不通为什么要这么做。我这里做的修改是，在PbfSensorFrame、SensorObjects等之前转换的过程中，直接使用Obj的sensor_id，而不是上述奇葩方法。

5. 传感器偏爱

   • 整个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检测到的数据做大量的条件检测才能允许发布。

6. 数据类型

   • 在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，并且多了传感器外参等。

7. 数据结构

   • 这个其实没什么好说的，Apollo里面的数据结构风格很统一，不外乎map+queue、map+deque、map+vector这几种方式之一。

8. 变量名解释

   • idx = ind = index
   • CIPV = closest in-path vehicle

总结

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