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

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

能够得到以下信息：

融合方法：ProbabilisticFusion
主要参与融合的传感器：Lidar 和 2 个焦距不一样的 Camera
融合结果存放到 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 过时后需要丢弃。
现在来看代码

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

目标之间数据关联
更新和新数据匹配上的 Tracks
更新未和数据匹配上的 Tracks
为未匹配到的新数据创建新的 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 方法。

4. 发送结果

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

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

总结

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