自动驾驶 Apollo 源码分析系列，感知篇：感知融合代码的基本流程

Posted 2021-06-29 frank909

说起自动驾驶感知系统，大家都会谈论到感知融合，这涉及到不同传感器数据在时间、空间的对齐和融合，最终的结果将提升自动驾驶系统的感知能力，因为我们都知道单一的传感器都是有缺陷的。本篇文章梳理 Apollo 6.0 中的感知数据融合基本流程。

感知架构

文章开始前，还是需要先看一看 Apollo 中感知的整体架构。

它有这么多传感器。

那感知融合，最终要融合什么呢？

Apollo 的感知系统的结果要以分为 2 个大类：

• 障碍物检测(车、行人或者其它交通要素)
• 红绿灯检测

借助于激光雷达和深度学习，Apollo 的感知模块能够输出障碍物的 3D 信息。

Fusion 流程

1. Fusion 模块在哪里启动？

之前说过 Apollo 的组件是在 CyberRT 的框架中运行，关键概念是 Component。

Perception 是一个大的 Component，它包含了很多子 Component，而数据融合作为一个子 Component 存在。

这个是在
modules/perception/production/dag/dag_streaming_perception.dag
中定义的。

所以，我们需要去找 FusionComponent。
并且，我们知道了它的配置在 fusion_component_conf.pb.txt 中。

能够得到以下信息：

1. 融合方法：ProbabilisticFusion
2. 主要参与融合的传感器：Lidar 和 2 个焦距不一样的 Camera
3. 融合结果存放到 obstacles 当中。

2. FusionComponent 的初始化

Fusion Component 的地址是这个：
modules/perception/onboard/component/fusion_component.cc

Init 方法也没有几行代码

第一步，加载 config 参数，文章前面刚已经张贴了。

对于融合方法、传感器名称都是直接用 std::string 类型保存。

我们需要关心代码后面部分的 InitAlgorithmPlugin() 方法，这个是用来初始化 Component 涉及的算法的。

看来 fusion::ObstacleMultiSensorFusion 运用了设计模式，猜测应该是一个简单的工厂模式类，我们去看看究竟。

所以我们需要找 BaseFusionSystemRegisterer，但有意思的地方是直接找不到的。
只找到这个。

关键在后面两段代码，应该是宏定义。

这需要追踪 Perception 这个模块的 Register 逻辑 。

文件路径：modules/perception/lib/registerer/registerer.h

果然是宏定义，顺着这个定义，我们来实例进行翻译一下。

PERCEPTION_REGISTER_REGISTERER(BaseFusionSystem)这一行展开会发生什么呢？

会定义一个新的类

class BaseFusionSystem{
	typedef ::apollo::perception::lib::Any Any;                       
    typedef ::apollo::perception::lib::FactoryMap FactoryMap;  
public:                                                            
    static BaseFusion *GetInstanceByName(const ::std::string &name) { 
      FactoryMap &map =                                               
          ::apollo::perception::lib::GlobalFactoryMap()[BaseFusion]; 
      FactoryMap::iterator iter = map.find(name);                     
      if (iter == map.end()) {                                        
        for (auto c : map) {                                          
          AERROR << "Instance:" << c.first;                           
        }                                                             
        AERROR << "Get instance " << name << " failed.";              
        return nullptr;                                               
      }                                                               
      Any object = iter->second->NewInstance();                       
      return *(object.AnyCast<base_class *>());                       
    }                                                                           
}

我们再看这行代码：

fusion_ = BaseFusionSystemRegisterer::GetInstanceByName(param.fusion_method);

这就对应得上了。
但有个细节，BaseFusionSystemRegister 中需要 FactoryMap 去查找对应 name 的 Object。

这个操作同样和宏定义脱离不了关系。

这是 registerer.h 中的定义。
而 BaseFusionSystemRegisterer.h 中最后的代码是：

#define FUSION_REGISTER_FUSIONSYSTEM(name) \\
  PERCEPTION_REGISTER_CLASS(BaseFusionSystem, name)

结合前面的配置信息，我们知道在 ObstacleMultiSensorFusion::Init(）中 fusion_ 将由 ProbobilisticFusion 实现，那么这个类在哪里呢？

modules/perception/fusion/lib/fusion_system/probabilistic_fusion/probabilistic_fusion.cc

成员变量有 trackers_、macher_、gate_keeper_ 这些都是目标跟踪相关的，到这里目标跟踪的轮廓慢慢浮现。

3. Fusion 的流程框架

回到 FusionComponent 中来，我们知道核心方法是 Proc。

核心的方法是 InternalProc，那么 InternalProc 中的核心代码是什么呢？

又跳到了 fusion 中的 Process，好吧，我们再跳转到对应的代码当中。

然后跟踪到这里。

代码中也注释的比较，明白主要是 4 步。

工程量比较大，下面分开讲解。

3.1 AddSensorMeasurements

执行对象是 SensorManager。

核心代码是 frame_ptr，这样看来 SensorPtr 也要看看它的内部长什么样子。

class Sensor {
 public:
  Sensor() = delete;
  ...省略部分...
private:
  FRIEND_TEST(SensorTest, test);

  base::SensorInfo sensor_info_;

  double latest_query_timestamp_ = 0.0;

  std::deque<SensorFramePtr> frames_;

  static size_t kMaxCachedFrameNum;
}

现在我们能够知道，Sensor 中会保存一个最大缓存数值,上一次查询时间，SensorInfo 信息。

代码非常简单，添加 Frame 时直接向 dequeue中添加，如果缓存满了则直接删掉头部的数据，也就是过期的数据。

3.2 getLatesFrame()

这一步是要提取上一次缓存的 Frame 数据。

前面说过 Apollo 中感知融合的 Sensor 有 3 个，那么进行数据融合时就不得不考虑这一次要参与融合的 Frame 是哪个 sensor 的数据。
所以，上面的代码会提取每一个 sensor 上一次缓存的数据，然后调用 std::sort()
方面依据时间戳大小进行排序。
而 sensor 中怎么获取上一帧代码，那实在是太简单了。

3.3 FuseFrame

取到数据后就要开始进行融合了。

3 个步骤：

• 前景目标融合跟踪
• 背景目标融合跟踪
• 移除已丢失的目标

从这里开始就要涉及到算法部分了，目标跟踪是要确认目标状态的。

上图的蓝色圆点代表传感器检测到的目标位置;
刚开始的时候，融合算法创建一个 Track1 对应融合的目标，黄色方框代表不同时刻这个目标的融合后的益状态;
但到了 T1 的时候，Sensor 检测的目标位置和 Track1 中融合的位置差距太大了，所以这个 Track1 已经不能代表当前对象了，所以需要新建立一个 Track2,执行后续的操作;
而 Track1 过时后需要丢弃。
现在来看代码

标红的地方基本上可以对应我刚刚陈述的目标跟踪思路：

1. 目标之间数据关联
2. 更新和新数据匹配上的 Tracks
3. 更新未和数据匹配上的 Tracks
4. 为未匹配到的新数据创建新的 Tracks

数据关联是目标跟踪中一个重要的领域，经典的算法有 NN、JPDA、HM 等。

Apollo 6.0 中用的是 HM，也就是匈牙利算法。我之前的文章写过这个算法及相应的 demo 代码。
【小算法】二分图匹配之匈牙利算法详解(图例说明，代码亲测可用)

因为 Apollo 是要进行多目标跟踪的，这里就涉及到多目标匹配，匈牙利算法的思路就是将要匹配的两组数据创建一个二分图，对应的到目标跟踪粗略地讲就是新的数据在图的一边，然后历史数组在图的另外一边，所以叫做二分图。

我们假设左边是历史目标，右边是新目标。
红线代表 Match 匹配关系，如果两个节点没有连线就代表没有匹配。
我们为了便于编程，将上图稍作变化。

虚线代表未匹配关系，然后匈牙利算法就是去寻找这么一条路径，路径上保留各个节点之间的匹配关系。
这一过程会耗时较长，本质上是不断进行深度优先比较，遇到冲突时需要进行协调。
具体算法细节请参阅我上面提到的博文。
Apollo 中对应的代码路径是：

modules/perception/fusion/lib/data_association/hm_data_association/hm_tracks_objects_match.cc

其中的 HMTrackersObjectsAssociation::Associate() 中代码过长，下一篇文章我会专门来分析这一段代码。

总之，数据关联最核心的问题其实是距离的计算，合适的距离决定了数据关联的质量。距离不单指物理上的距离，也可以包括用量化的数值对一个目标在类别、外形、颜色的差异化表达。
这一部分细节下一篇文章再讲吧。

后面的对于 background 操作也差不多，就不细讲了。

3.5 CollectFusedObjects()

先通过 gate_keeper 判断能不能将数据发布出去，如果能的话再执行 CollectObjectsByTrack 方法。

相关代码定义在这里：

代码很长，其实就是定义了一些规则，融合后的数据哪些不能发。

1. 不在视野范围内 Lidar、Camera、Radar 数据不能发。
2. 前向 Radar 不能发。
3. 后向雷达目标 Range 要大于指定阈值，速度的 Norm 值要大于 4,Track 概率的置信度要大于阈值。
4. Camera 要发数据的话，要保证是 3d 数据，当然 TrafficCone 也就是锥形桶可以发，其它的类要求比较严格，要保证距离大于阈值，并且在夜晚环境不能发。

决定好哪些 FusedTrack 数据可以发之后，通过 ProbabilisticFusion::CollectObjectsByTrack() 执行最后的操作。
代码比较简单，就是一些简单的赋值动作。

4. 发送结果

Fusion 执行完毕后，将视线跳转到 FusionComponent::Proc() 中来。

将融合后的数据发送出去。
自此，单个周期的数据融合代码流程就分析完毕。

总结

本篇文章只是粗略梳理了 Apollo 6.0 中的感知融合代码，可以得到一个大致的流程框架，这有利于初学者依葫芦画瓢弄一个自己的框架。
但有一点需要明白的是，数据融合是一个系统性的工程问题，依赖于传感器的标定、传感器本身数据的可靠性、数据关联算法、目标跟踪滤波算法、场景的细分处理以及代码的高效实现。
我们可以看到感知融合这一部分 C++ 代码写得比较复杂，运用了大量的设计模式思想，这给同学们阅读代码时增加了难度，建议阅读时多画一下图，脑袋晕乎时休息一下。
最后，接下来的文章将分析核心的数据关联算法、目标跟踪滤波算法的设计与实现。