【Baidu Apollo】4.1 感知

首先非常感谢知乎上几位大牛和Apollo开发者社区分享的相关资料，整个Baidu Apollo的系列文章，笔者这里做一些资料的收集整理，希望可以和大家共同学习。主要的参考来源包括：

• 知乎冀渊的专栏眼中的世界
• Apollo阿波罗自动驾驶开发者社区
• Apollo 开发者在线文档
• CSDN 作者 知行合一2018
• CSDN 作者 DinnerHowe
• Baidu Apollo GitHub

今天的感知模块的内容整理自冀渊的文章:
Apollo 2.0 框架及源码分析(二) | 感知模块 | Lidar
Apollo 2.0 框架及源码分析(三) | 感知模块 | Radar & Fusion

Apollo 感知模块 (Perception) 实现框架

无人驾驶系统车端的部分，大致可分为3块内容，感知，决策，控制。其中，感知是其余两者的基础，是无人驾驶中极为重要的一个部分。

无人驾驶中所用到得传感器各有长处和短板，单一的传感器难以满足复杂场景的需求，因此使用多种传感器进行相互融合（sensor funsion）就显得十分有必要了。

下图是 Apollo 公开课对无人驾驶中常见传感器的介绍，从图中看出，各个传感器擅长的情况不一，而依靠传感器的融合则可以应对大多数情况。

Apollo 的这张图整体表述不错，十分清晰。
冀渊认为图中对 Lidar 在 Lane Tracking 上的描述有点欠妥。他认为可以根据路面和车道线对 Lidar 射线反射强度的不同来追踪车道。因此 Lidar 在车道线追踪的能力应该可以算得上是 Fair 而不是和 Radar 一样的 Poor。

另外，雷锋网的这篇文章中也提到了基于激光雷达回波信号检测车道线的可能性。专栏 | 如何利用激光雷达检测车道线？这里提供了4种方法
Apollo 2.0 感知的整体框架如下图所示，分为 3D障碍物感知 和 交通灯感知 两大部分。

其实这两者在 Apollo 官方文档和讲座中，已经有了比较清晰的讲解。交通信号灯感知和3D 障碍物感知
Apollo 官方公开课中的对感知部分的功能是用下图描述的。

从图中可知，在 Apollo 中感知模块有以下几个职能：

1. 探测物体 （是否有障碍物）
2. 对物体分类 （障碍物是什么）
3. 语义解析 (障碍物从背景中分割)
4. 物体追踪 (障碍物追踪)

本篇接下来会叙述 Apollo 3D障碍物感知部分是如何实现上述职能的。

障碍物感知部分的框架如下图所示。障碍物感知主要依靠的是 Lidar 和 Radar 两者的感知结果的相互融合。该部分输入为传感器的原始数据，根据接收到的数据的来源的不同，进行不同的处理，最终输出融合后的结果。

从代码上看，障碍物感知由3个主体部分组成: Lidar、Radar 和 fusion。接下来，本篇将重点描述其中的 Lidar 部分的实现原理。

Lidar 部分

官方对 Lidar 部分的原理解释得极其清楚。大家结合上文列出的参考文档和PPT资料 ，应该就会对 Lidar 部分的整体工作原理会有个清晰的认识了。

如上图 PPT 所示，Lidar 部分简单来说是这样工作的：

1. 输入为 Lidar 得到的点云数据，输出为检测到的障碍物
2. 由 HDmap 来确定 ROI (region of interest），过滤 ROI 区域外的点
3. 处理点云数据，探测并识别障碍物 (由 AI 完成)
4. 障碍物追踪

冀渊对代码也做了讲解

入口函数位置: apollo/modules/perception/obstacle/onboard/lidar_process_subnode.cc

void LidarProcessSubnode::OnPointCloud(
    const sensor_msgs::PointCloud2& message);  

根据代码的注释，这里分成7步对 Lidar 的流程进行叙述。

1. 坐标及格式转换 (get velodyne2world transform)

Apollo 使用了开源库 Eigen 进行高效的矩阵计算，使用了 PCL 点云库对点云进行处理。

该部分中，Apollo 首先计算转换矩阵 velodyne_trans，用于将 Velodyne 坐标转化为世界坐标。之后将 Velodyne 点云转为 PCL 点云库格式，便于之后的计算。

2. 获取ROI区域 (call hdmap to get ROI)

核心函数位置: obstacle/onboard/hdmap_input.cc

bool HDMapInput::GetROI(const PointD& pointd, const double& map_radius,
                        HdmapStructPtr* mapptr);

查询 HDmap, 根据 Velodyne 的世界坐标以及预设的半径 (FLAG_map_radius) 来获取 ROI 区域。

首先获取指定范围内的道路以及道路交叉点的边界，将两者进行融合后的结果存入 ROI 多边形中。该区域中所有的点都位于世界坐标系。

3. 调用ROI过滤器 (call roi_filter）

核心函数位置：obstacle/lidar/roi_filter/hdmap_roi_filter/hdmap_roi_filter.cc

bool HdmapROIFilter::Filter(const pcl_util::PointCloudPtr& cloud,
                            const ROIFilterOptions& roi_filter_options,
                            pcl_util::PointIndices* roi_indices);

官方文档对该部分是这么描述的:

高精地图 ROI 过滤器（往下简称“过滤器”）处理在ROI之外的激光雷达点，去除背景对象，如路边建筑物和树木等，剩余的点云留待后续处理。
一般来说，Apollo 高精地图 ROI过滤器有以下三步：
1. 坐标转换
2. ROI LUT构造
3. ROI LUT点查询

蓝色线条标出了高精地图ROI的边界，包含路表与路口。红色加粗点表示对应于激光雷达传感器位置的地方坐标系原始位置。2D网格由8*8个绿色正方形组成，在ROI中的单元格，为蓝色填充的正方形，而之外的是黄色填充的正方形。

ROI 过滤器部分涉及到了 扫描线法 和 位图编码 两个技术。具体来看，该部分分以下几步：

a. 坐标转换

将地图 ROI 多边形和点云转换至激光雷达传感器位置的地方坐标系。

b. 确定地图多边形主方向

比较所有点的 x、y 方向的区间范围，取区间范围较小的方向为主方向。并将地图多边形 (map_polygons) 转换为待加工的多边形 (raw polygons)。

c. 建立位图

将 raw polygons 转化为位图 (bitmap) 中的格点，位图有以下特点：

• 位图范围, 以 Lidar 为原点的一片区域 (-range, range)*(-range, range) 内，range 默认 70米
• 位图用于以格点 (grid) 的方式存储 ROI 信息。若某格点值为真，代表此格点属于 ROI。
• 默认的格点大小为 cell_size 0.25米。
• 在列方向上，1bit 代表 1grid。为了加速操作，Apollo 使用 uint64_t 来一次操纵64个grids。

为了在位图中画出一个多边形，以下3个步骤需要被完成

i. 获得主方向有效范围

ii.将多边形转换为扫描线法所需的扫描间隔：将多变形在主方向上分解为线(多边形->片段->线），计算每条线的扫描间隔。

iii. 基于扫描间隔在位图中画格点

关于扫描线，这里推荐笔者参考的一篇解析 扫描线算法完全解析

d. ROI 点查询

通过检查 grid 的值，确定在位图中得每一个 grid 是否属于 ROI。

4. 调用分割器 (segmentor)

入口函数所在文件cnn_segmentation.cc

bool CNNSegmentation::Segment(const pcl_util::PointCloudPtr& pc_ptr,
                              const pcl_util::PointIndices& valid_indices,
                              const SegmentationOptions& options,
                              vector<ObjectPtr>* objects)

分割器采用了 caffe 框架的深度完全卷积神经网络（FCNN） 对障碍物进行分割

简单来说有以下四步：

a. 通道特征提取

计算以 Lidar 传感器某一范围内的各个单元格 (grid) 中与点有关的8个统计量，将其作为通道特征（channel feature）输入到 FCNN。

1. 单元格中点的最大高度
2. 单元格中最高点的强度
3. 单元格中点的平均高度
4. 单元格中点的平均强度
5. 单元格中的点数
6. 单元格中心相对于原点的角度
7. 单元格中心与原点之间的距离
8. 二进制值标示单元格是空还是被占用如

计算时默认只使用 ROI 区域内的点，也可使用整个 Lidar 范围内的点，使用标志位 use_full_cloud_ 作为开关。

b. 基于卷积神经网络的障碍物预测

• 与 caffe 相关的 FCNN 源码貌似是不开源的
• Apllo 官方叙述了其工作原理，摘录如下

完全卷积神经网络由三层构成：下游编码层（特征编码器）、上游解码层（特征解码器）、障碍物属性预测层（预测器）

特征编码器将通道特征图像作为输入，并且随着特征抽取的增加而连续下采样其空间分辨率。 然后特征解码器逐渐对特征图像
上采样到输入2D网格的空间分辨率，可以恢复特征图像的空间细节，以促进单元格方向的障碍物位置、速度属性预测。
根据具有非线性**（即ReLu）层的堆叠卷积/分散层来实现 下采样和 上采样操作

基于 FCNN 的预测，Apollo 获取了每个单元格的四个预测信息，分别用于之后的障碍物聚类和后期处理。

c. 障碍物集群 （Cluster2D）

核心函数位置 obstacle/lidar/segmentation/cnnseg/cluster2d.h

void Cluster(const caffe::Blob<float>& category_pt_blob,
               const caffe::Blob<float>& instance_pt_blob,
               const apollo::perception::pcl_util::PointCloudPtr& pc_ptr,
               const apollo::perception::pcl_util::PointIndices& valid_indices,
               float objectness_thresh, bool use_all_grids_for_clustering);

Apollo基于单元格中心偏移预测构建有向图，采用压缩的联合查找算法（Union Find algorithm ）基于对象性预测有效查找连接组件，构建障碍物集群。

（a）红色箭头表示每个单元格对象中心偏移预测；蓝色填充对应于物体概率不小于0.5的对象单元。
（b）固体红色多边形内的单元格组成候选对象集群。

d. 后期处理

涉及的函数 obstacle/lidar/segmentation/cnnseg/cluster2d.h

void Filter(const caffe::Blob<float>& confidence_pt_blob,
              const caffe::Blob<float>& height_pt_blob);
void Classify(const caffe::Blob<float>& classify_pt_blob);
void GetObjects(const float confidence_thresh, const float height_thresh,
                  const int min_pts_num, std::vector<ObjectPtr>* objects);

聚类后，Apollo获得一组包括若干单元格的候选对象集，每个候选对象集包括若干单元格。根据每个候选群体的检测置信度分数和物体高度，来确定最终输出的障碍物集/分段。从代码中可以看到 CNN分割器最终识别的物体类型有三种：小机动车、大机动车、非机动车和行人

在obstacle/lidar/segmentation/cnnseg/cluster2d.h中

enum MetaType {
  META_UNKNOWN,
  META_SMALLMOT,
  META_BIGMOT,
  META_NONMOT,
  META_PEDESTRIAN,
  MAX_META_TYPE
};

5. 障碍物边框构建

入口函数位置 obstacle/lidar/object_builder/min_box/min_box.cc

void BuildObject(ObjectBuilderOptions options, ObjectPtr object)

边界框的主要目的还是预估障碍物（例如，车辆）的方向。同样地，边框也用于可视化障碍物。
如图，Apollo确定了一个6边界边框，将选择具有最小面积的方案作为最终的边界框

6. 障碍物追踪

入口函数位置：obstacle/lidar/tracker/hm_tracker/hm_tracker.cc

  // @brief track detected objects over consecutive frames
  // @params[IN] objects: recently detected objects
  // @params[IN] timestamp: timestamp of recently detected objects
  // @params[IN] options: tracker options with necessary information
  // @params[OUT] tracked_objects: tracked objects with tracking information
  // @return true if track successfully, otherwise return false
  bool Track(const std::vector<ObjectPtr>& objects, double timestamp,
             const TrackerOptions& options,
             std::vector<ObjectPtr>* tracked_objects); 

障碍物追踪可分两大部分，即 数据关联 和 跟踪动态预估。Apollo 使用了名为 HM tracker 的对象跟踪器。实现原理：

在HM对象跟踪器中，匈牙利算法(Hungarian algorithm)用于检测到跟踪关联，并采用 鲁棒卡尔曼滤波器(Robust
Kalman Filter) 进行运动估计。

数据关联

数据关联的过程是确定传感器接收到的量测信息和目标源对应关系的过程，是多传感多目标跟踪系统最核心且最重要的过程 [11]。

Apollo 首先建立关联距离矩阵，用于计算每个对象 (object ) 和 每个轨迹 (track )之间的关联距离。之后使用 匈牙利算法 为 object和 track 进行最优分配。

计算关联距离时，Apollo 考虑了以下5个关联特征，来评估 object 和 track 的运动及外观一致性，并为其分配了不同的权重。

由上表可以看出，Apollo 在计算关联距离时，重点考虑的还是几何距离和两者的形状相似度。计算得到类似下图的关联距离矩阵后，使用匈牙利算法将 Object 与 Track 做匹配。

关于匈牙利算法的实现原理，这里有篇很有趣的解释 趣写算法系列之–匈牙利算法 - CSDN博客

跟踪动态预估 （Track Motion Estimation）

使用卡尔曼滤波来对 track 的状态进行估计，使用鲁棒统计技术来剔除异常数据带来的影响。

不了解卡尔曼滤波原理的同学请参考：卡尔曼滤波器的原理以及在matlab中的实现 。这一部分的滤波整体看来是一个标准的卡尔曼滤波。在此基础上，Apollo 团队加入了一些修改，根据官方文档，Apollo 的跟踪动态预估有以下三个亮点 :

观察冗余

在一系列重复观测中选择速度测量，即滤波算法的输入，包括锚点移位、边界框中心偏移、边界框角点移位等。冗余观测将为滤波测量带来额外的鲁棒性，因为所有观察失败的概率远远小于单次观察失败的概率。

卡尔曼更新的观测值为速度。每次观测三个速度值 :

锚点移位速度、边界框中心偏移速度 和 边界框角点位移速度。

从三个速度中，根据运动的一致性，选出与之前观测速度偏差最小的速度为最终的观测值。

根据最近3次的速度观测值，计算出加速度的观测值。

分解

高斯滤波算法 （Gaussian Filter algorithms）总是假设它们的高斯分布产生噪声。然而，这种假设可能在运动预估问题中失败，因为其测量的噪声可能来自直方分布。 为了克服更新增益的过度估计，在过滤过程中使用故障阈值。

这里的故障阈值应该对应着程序中的 breakdown_threshold_。

该参数被用于以下两个函数中，当更新的增益过大时，它被用来克服增益的过度估计：

• KalmanFilter::UpdateVelocity
• KalmanFilter::UpdateAcceleration

两者的区别在于：

速度的故障阈值是动态计算的，与速度误差协方差矩阵有关

velocity_gain *= breakdown_threshold_; 

加速度的故障阈值是定值，默认为2

acceleration_gain *= breakdown_threshold;  

更新关联质量 （UpdateQuality）

原始卡尔曼滤波器更新其状态不区分其测量的质量。 然而，质量是滤波噪声的有益提示，可以估计。例如，在关联步骤中计算的距离可以是一个合理的测量质量估计。 根据关联质量更新过滤算法的状态，增强了运动估计问题的鲁棒性和平滑度

更新关联质量 update_quality 默认为 1.0，当开启适应功能时 (s_use_adaptive ==true）Apollo 使用以下两种策略来计算更新关联质量：

• 根据 object 自身的属性 — 关联分数 (association_score) 来计算
• 根据新旧两个 object 点云数量的变化

首先根据这两种策略分别计算更新关联质量，之后取得分小的结果来控制滤波器噪声。

7. 障碍物类型融合 （call type fuser）

这个函数貌似再3.0的版本里删除了。

该部分负责对 object 序列 (object sequence) 进行类型 (type) 的融合。

object 的type 如下代码所示:

enum ObjectType {
  UNKNOWN = 0,
  UNKNOWN_MOVABLE = 1,
  UNKNOWN_UNMOVABLE = 2,
  PEDESTRIAN = 3,
  BICYCLE = 4,
  VEHICLE = 5,
  MAX_OBJECT_TYPE = 6,
};

Apollo 将被追踪的objects 视为序列。

当 object 为 background 时，其类型为 “UNKNOW_UNMOVABLE”。

当 object 为 foreground 时，使用线性链条件随机场(Linear chain Conditional Random Fields) 和 维特比（Viterbi）算法对 object sequence 进行 object 的类型的融合。

Radar 部分

入口函数位置：obstacle/onboard/radar_process_subnode.cc

void RadarProcessSubnode::OnRadar(const ContiRadar &radar_obs) ;

相比 Lidar 部分的令人惊艳，Apollo 2.0 中 Radar 就稍显诚意不足了。Apollo 2.0 中 Radar 探测器模块名为“ModestRadarDetector”(“适度的雷达探测器”，从起名上就感觉Apollo 2.0 对 Radar 这部分有些不自信）。

Radar 部分大体的实现方式与 Lidar 类似，可以看做是其简化版，其大体流程如下图所示。

从代码实现上来看，Radar 的工作可分为以下4步：

一. 计算 Radar 的坐标转换矩阵

这里有个细节十分有趣。雷达的世界坐标的转换矩阵是由以下代码来实现的。

*radar2world_pose = *velodyne2world_pose *
short_camera_extrinsic_ *  radar_extrinsic_;

这段代码描述了这样一个式子：

Radar 世界坐标 = Lidar 世界坐标 * 短摄像头参数* radar参数。

这个式子隐隐透露着， Apollo 的坐标体系是以 Lidar 为基准的。Apollo 可能认为 Velodyne 的位置是最准确的，因此 Camera 的位置标定参考 Velodyne, Radar 的标定参考 Camera。

二. 获取 ROI 区域

从高精度地图读取 ROI 区域，并将其转化为地图多边形 (map_polygons) 备用。Radar 和 Lidar 使用了相同的函数来获取 ROI 区域和地图多边形，获得的 ROI 地图多边形处于世界坐标下。

函数位置: obstacle/onboard/hdmap_input.cc

void HdmapROIFilter::MergeHdmapStructToPolygons(
    const HdmapStructConstPtr& hdmap_struct_ptr,
    std::vector<PolygonDType>* polygons)

函数位置: obstacle/lidar/roi_filter/hdmap_roi_filter/hdmap_roi_filter.cc

void HdmapROIFilter::MergeHdmapStructToPolygons(
    const HdmapStructConstPtr& hdmap_struct_ptr,
    std::vector<PolygonDType>* polygons) 

三. 计算汽车线速度

四. 探测障碍物

核心函数位置：obstacle/radar/modest/modest_radar_detector.cc

bool ModestRadarDetector::Detect(const ContiRadar &raw_obstacles,
                                 const std::vector<PolygonDType> &map_polygons,
                                 const RadarDetectorOptions &options,
                                 std::vector<ObjectPtr> *objects) ;

这部分为 Radar 的核心部分，这段又可分为如下4步。

1. 从原始数据构造 object

实现函数位置: obstacle/radar/modest/object_builder.cc

void ObjectBuilder::Build(const ContiRadar &raw_obstacles,
                          const Eigen::Matrix4d &radar_pose,
                          const Eigen::Vector2d &main_velocity,
                          SensorObjects *radar_objects) ;

该部分涉及到了以下方面的内容

• 判断该障碍物是否为 backgroundobject
• 锚点 (anchor_point) 的坐标系转换: Radar 坐标 -> 世界坐标
• 速度的转化: 相对速度-> 绝对速度；

我们重点阐述下第一项内容。Apollo 提供了三种方式用来判断该障碍物是否为 background：

a. 根据障碍物出现的次数

当障碍物出现次数小于 delay_frames_ （默认值为4）时，认为它是 background。

b. 根据障碍物出现的 存在概率 和 均根方值（rms）

实现函数位置: obstacle/radar/modest/conti_radar_util.cc

bool ContiRadarUtil::IsFp(const ContiRadarObs &contiobs,
                          const ContiParams &params, const int delay_frames,
                          const int tracking_times)

当满足以下两个条件之一时，认为该障碍物是 background

• 障碍物的存在概率小于 Radar 参数预设值
• 当障碍物在车身横纵两个方向上的 距离 或 速度 的 rms 大于 Radar 参数预设值

代码中对这个功能是这样实现的（为方便说明，这里隐去部分代码）。

bool ContiRadarUtil::IsFp(const ContiRadarObs &contiobs,
                          const ContiParams &params, const int delay_frames,
                          const int tracking_times) {
    int cls = contiobs.obstacle_class();
      ...
    if (cls == CONTI_CAR || cls == CONTI_TRUCK) {
      ...
    } else if (cls == CONTI_PEDESTRIAN) {
      ...
    } else if (cls == CONTI_MOTOCYCLE || cls == CONTI_BICYCLE) {
      ...
    } else if (cls == CONTI_POINT || cls == CONTI_WIDE ||
               cls == CONTI_UNKNOWN) {
      ...
    }
      ...
}

从中可以看出，Apollo 对不同类型的障碍物分别进行了处理，也就是说，Apollo 使用的 Radar 可以分辨出车、行人和自行车 等不同的障碍物。

Apollo 推荐使用的毫米波为Continental 的 ARS 408-21。ARS 408-21的介绍文档里也有简单提到，它可以对障碍物进行分类。

但实际上，我对于毫米波雷达的障碍物类型的分辨结果的可靠性和可用性仍存有疑惑。笔者也咨询过同事和朋友，大家认为毫米波雷达也许可以根据回波特性和被检测物的速度来区分障碍物。但这种方式的分辨率、可靠性和可用性都有待提高。另外文档中括号内 “> 120 single cluster”这个条件，我一直不能理解，希望可以和大家一起探讨一下。

c. 根据 障碍物速度 和 车身速度 的夹角 (均在绝对速度下)

当车速和障碍物速度都大于阈值 (velocity_threshold, 默认值 1e-1) 时，计算两个速度的夹角。当夹角在（1/4 Pi, 3/4 Pi）或者 (-3/4 Pi, -1/4 Pi ) 之外的范围时，认为该障碍物是 background。

这里还有一个槽点，Apollo 2.0 在构造 object 外形的时候的代码如下:

object_ptr->length = 1.0;
object_ptr->width = 1.0;
object_ptr->height = 1.0;
object_ptr->type = UNKNOWN; 

从代码中可以看到，Apollo 将障碍物长宽高都设成1米。毫米波雷达确实很难获取到物体的轮廓信息，但这样处理，还是觉得有点简单粗暴了。

另外这里的障碍物类型直接设置为 UNKNOW ，这就让人有些疑惑了。既然之前 Apollo 在构造 object 时已经可以直接从 Radar 获得障碍物类型，为什么这里不使用呢？难道 Apollo 自己其实也认为 Radar 获得的物体类型的可靠性不高么？这样处理岂不是有点前后矛盾？

2. 调用 ROI 滤波器

实现函数位置: obstacle/radar/modest/modest_radar_detector.cc

void RoiFilter(const std::vector<PolygonDType> &map_polygons,
                 std::vector<ObjectPtr>* filter_objects)

Radar 的 ROI 滤波器与 Lidar 有所不同，被简化了许多。

• Radar 没有使用位图和扫描线，而是直接判断每个 object 是否处于地图多边形内
• Radar ROI 滤波器中使用的点和地图多边形的坐标均为世界坐标，而 Lidar 的 ROI 使用了地方坐标。

3. 障碍物追踪

入口函数位置: obstacle/radar/modest/radar_track_manager.cc

// @brief: process radar obstacles
// @param [in]: built radar obstacles
// @return nothing
void Process(const SensorObjects &radar_obs);

Radar 的数据关联相对简单，只考虑 目标 object 和 tracked object 两者的 track_id 和 几何距离 两个因素。

当两者的 track_id 相同，且两者之间距离小于 RADAR_TRACK_THRES （默认 2.5）时，认为两者为同一目标，即目标 object 属于该 track。

几何距离 = 两object 的中心点的距离+object 的速度*时间差。

另外对比 Lidar 在进行数据关联时，计算了障碍物的关联距离且使用了匈牙利算法进行最优分配，Radar 部分的数据关联显得太简单和太单薄了。

除此之外，在进行障碍物追踪时，Radar 的 tracker 也并未存储 object 的历史数据，只保存最新的一个 object，并且也没有使用卡尔曼滤波器对 objects 进行状态估计。

4. 收集 Objects

收集objects， 为最后一步融合做准备。

总得来说，Radar 部分“无愧于” modest 的名称，与 Lidar 部分相比，Radar 的实现显得过于单薄。虽然与 Lidar 相比，Radar 的精确度确实相对较低，并且我也相信 Lidar 是未来的趋势。但现阶段 Radar 在恶劣天气下的表现，在速度的探测上和探测距离上都比 Lidar 要好。在许多量产方案中 (比如奥迪A8，特斯拉) Radar 也都发挥了极其重要的作用。Radar 也可以作为 Lidar 的冗余，在 Lidar 出故障时，用来保证系统的安全。

我期望百度能够让 Radar 发挥更大的作用，让 Apollo 系统得以进一步完善。

融合部分

入口函数位置: obstacle/onboard/fusion_subnode.cc

apollo::common::Status Process(const EventMeta &event_meta,
                                 const std::vector<Event> &events);

核心函数位置：obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc

// @brief: fuse objects from multi sensors(64-lidar, 16-lidar, radar...)
// @param [in]: multi sensor objects.
// @param [out]: fused objects.
// @return true if fuse successfully, otherwise return false
virtual bool Fuse(const std::vector<SensorObjects> &multi_sensor_objects,
                    std::vector<ObjectPtr> *fused_objects);

融合部分总体来看，没有特别大的惊喜，仍是使用了传统的卡尔曼滤波。

Apollo 介绍中说到，他们使用了 object-level 的数据融合，该部分的输入为各传感器处理后的得到的object。

这里顺便一提，多源信息的数据融合中，根据数据抽象层次，融合可分为三个级别[2]：

• 数据级融合 传感器裸数据融合，精度高、实时性差，要求传感器是同类的
• 特征级融合 融合传感器抽象的特征向量(速度，方向等)，数据量小、损失部分信息
• 决策级融合 传感器自身先做出决策，融合决策结果，精度低、通信量小、抗干扰强

Apollo 应该是在特征层面对 objects 进行了融合。每当节点收到新的一帧数据的时候，融合部分就被调用。融合部分的输入为 SensorObjects, 输出为融合后的 object， 其大体的流程如下图所示。

这其中，最重要的步骤自然就是融合了，Apollo 依照传感器的测量结果，根据is_background 标志位，将 objects 分为 ForegroundObjects 和 BackgroundObjects 两类, 融合时只处理 ForegroundObjects。

融合函数位置：obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc

void FuseForegroundObjects(
      std::vector<PbfSensorObjectPtr> *foreground_objects,
      Eigen::Vector3d ref_point, const SensorType &sensor_type,
      const std::string &sensor_id, double timestamp);

从之前的经验我们可以看出，传感器的数据融合有两部分内容比较重要，即 数据关联 和 动态预估。

数据关联

数据关联的接口定义如下：

  // @brief match sensor objects to global tracks build previously
  // @params[IN] fusion_tracks: global tracks
  // @params[IN] sensor_objects: sensor objects
  // @params[IN] options: matcher options for future use
  // @params[OUT] assignments: matched pair of tracks and measurements
  // @params[OUT] unassigned_tracks: unmatched tracks
  // @params[OUT] unassigned_objects: unmatched objects
  // @params[OUT] track2measurements_dist:minimum match distance to measurements
  // for each track
  // @prams[OUT] measurement2track_dist:minimum match distacne to tracks for
  // each measurement
  // @return nothing
  virtual bool Match(const std::vector<PbfTrackPtr> &fusion_tracks,
                     const std::vector<PbfSensorObjectPtr> &sensor_objects,
                     const TrackObjectMatcherOptions &options,
                     std::vector<TrackObjectPair> *assignments,
                     std::vector<int> *unassigned_fusion_tracks,
                     std::vector<int> *unassigned_sensor_tracks,
                     std::vector<double> *track2measurements_dist,
                     std::vector<double> *measurement2track_dist) = 0;

数据关联的实现是在:

obstacle/fusion/probabilistic_fusion/pbf_hm_track_object_matcher.cc

bool PbfHmTrackObjectMatcher::Match(
    const std::vector<PbfTrackPtr> &fusion_tracks,
    const std::vector<PbfSensorObjectPtr> &sensor_objects,
    const TrackObjectMatcherOptions &options,
    std::vector<TrackObjectPair> *assignments,
    std::vector<int> *unassigned_fusion_tracks,
    std::vector<int> *unassigned_sensor_objects,
    std::vector<double> *track2measurements_dist,
    std::vector<double> *measurement2track_dist) ;

其实从文件名称中的”hm”就可以看出一些端倪了，与 Lidar 相似，Fusion 部分的数据关联还是使用了匈牙利算法（Hungarian Matcher）来进行分配的。

但是在计算关联距离时，两者有比较大的不同。Fusion 部分计算的只是两个 object 中心的几何距离。我们稍微看一下这里的代码，有个细节比较有趣。

Fusion 在计算关联距离时，使用了以下代码。

函数位置：obstacle/fusion/probabilistic_fusion/pbf_track_object_distance.cc

float PbfTrackObjectDistance::Compute(
    const PbfTrackPtr &fused_track, const PbfSensorObjectPtr &sensor_object,
    const TrackObjectDistanceOptions &options) {

  //fused_track 已经融合过obj的航迹
  //sensor_object 来自传感器的，待融合的obj

  //获取此帧数据来源于的传感器类型
  const SensorType &sensor_type = sensor_object->sensor_type;
  ADEBUG << "sensor type: " << sensor_type;
  // 获取上次融合的obj
  PbfSensorObjectPtr fused_object = fused_track->GetFusedObject();
  if (fused_object == nullptr) {
    ADEBUG << "fused object is nullptr";
    return (std::numeric_limits<float>::max)();
  }

  Eigen::Vector3d *ref_point = options.ref_point;
  if (ref_point == nullptr) {
    AERROR << "reference point is nullptr";
    return (std::numeric_limits<float>::max)();
  }

  float distance = (std::numeric_limits<float>::max)();
  //获取航迹中最近的来自Lidar的obj
  const PbfSensorObjectPtr &lidar_object = fused_track->GetLatestLidarObject();
  //获取航迹中最近的来自Radar的obj
  const PbfSensorObjectPtr &radar_object = fused_track->GetLatestRadarObject();

  //下面是重点
  if (is_lidar(sensor_type)) {  //如果这次要融合obj是来自源于Lidar
    if (lidar_object != nullptr) {    
    // 如果航迹中已经有来自 Lidar 的obj, 则计算两者的几何距离 
      distance =
          ComputeVelodyne64Velodyne64(fused_object, sensor_object, *ref_point);
    } else if (radar_object != nullptr) {
     // 如果航迹中没有来自 Lidar 的obj, 则计算与 radar obj 的距离,注意这里 
     // sensor_object 和 fused_object 与上面的位置是相反的。原因是这个函数
     // 在计算距几何距离时的实现，是以第一个参数为速度基准，计算v*time_diff
     // 也就是说，当 fused_object 为 radar 时，以 sensor_object 为准。  
      distance =
          ComputeVelodyne64Radar(sensor_object, fused_object, *ref_point);
    } else {
      AWARN << "All of the objects are nullptr";
    }
  } else if (is_radar(sensor_type)) { // 如果这次要融合的obj是来源于Radar
    if (lidar_object != nullptr) { 
      // 如果航迹中已经有来自 Lidar 的obj, 则计算两者的几何距离 
      distance =
          ComputeVelodyne64Radar(fused_object, sensor_object, *ref_point);
    } else if (radar_object != nullptr) {
     // 如果航迹中没有来自 Lidar 的obj, 返回 float 的极值
      distance = std::numeric_limits<float>::max();
      //    distance = compute_radar_radar(fused_object, sensor_object,
      //    *ref_point);
    } else {
      AWARN << "All of the objects are nullptr";
    }
  } else {
    AERROR << "fused sensor type is not support";
  }
  return distance;
}

上面这段代码表明了，Fusion 在计算几何距离时，要求计算的两个 obj 中至少有一个是来自于 Lidar 的，并且以 Lidar 为基准来测量距离。

但只靠几何距离来进行数据关联，容易出现 Miss Match 的问题。即，当两条航迹发生交叉时，只靠几何距离是无法为 object 关联合适的航迹的。我现在还不清楚 Apollo 2.0 打算如何规避这个问题。

动态预估

动态预估还是使用的卡尔曼滤波，这部分有两点比较有趣：

Apollo 的融合部分为了可扩展性使用的是标准卡尔曼滤波Apollo 融合部分的卡尔曼滤波使用了非简化的估计误差协方差矩阵 Pk|k${\mathbf{P}}_{k|k}$ 更新公式

我们对比上图标准卡尔曼滤波的公式，来说明我为什么觉着上述两点比较有趣。

1. 使用标准卡尔曼滤波

从代码细节上看，Apollo 使用了标准卡尔曼滤波，并且在更新来自 Lidar 和 Radar 的 object 的时候，Apollo 对这两者使用了相同的观测矩阵 \mathbf {H}。

这里有趣的地方在于，一般而言 Lidar 和 Radar 的观测矩阵是不同的, 因为两者得到的数据不同 。为了更好地说明这个问题，我建议大家先阅读一下下面这篇文章 扩展卡尔曼滤波EKF与多传感器融合。
这个问题已经得到解答，大陆的Radar提供的也是笛卡尔坐标系的数据，所以用一个观测矩阵是可以的

2. 使用了非简化的估计误差协方差矩阵 Pk|k${\mathbf{P}}_{k|k}$ 更新公式

我们简单看一下区别

标准卡尔曼滤波：Pk|k=(I−KkHk)Pk|k−1${\displaystyle {\mathbf{P}}_{k|k}=(\mathbf{I}\mathbf{-}{\mathbf{K}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}){\mathbf{P}}_{k|k-1}}$
Apollo：Pk|k=(I−KkHk)Pk|k−1(I−KkHk)T+KkRkKTk${\displaystyle {\mathbf{P}}_{k|k}=(\mathbf{I}\mathbf{-}{\mathbf{K}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}){\mathbf{P}}_{k|k-1}}(\mathbf{I}\mathbf{-}{\mathbf{K}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}{)}^{\mathbf{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}$

结合 Wikipedia 上关于卡尔曼滤波的介绍，我先总结下该问题的背景:

• Apollo 使用的估计误差协方差矩阵 Pk|k${\mathbf{P}}_{k|k}$的更新公式是所谓的 Joseph form，而标准卡尔曼滤波通常使用的是简化版的更新公式
• 简化版的更新公式计算量小，实践中应用广，但只在 卡尔曼增益为最优 时有效
• 必须使用Joseph form 的两种情况：
  – 使用了非最优卡尔曼增益
  – 算法精度过低，造成了数值稳定性相关的问题

Apollo 社区回答了两点原因，一是出于算法精度的考虑；二是由于计算单元的强大（且昂贵），非简化版的卡尔曼滤波在计算时也不会消耗太久的时间。综合考虑，Apollo 选择了 Joseph form 的更新方程。

最后，我们用一个表格对障碍物感知部分做个简单的小结

模块入口的选择

我在分析 Apollo 时将重点放在了模块的实现和解决方案上，而非代码的编写技巧。因此关于代码只在本篇最后简单说一下上篇里提到过的函数入口的问题。

Preception 模块的入口

Preception 模块的主入口其实还是很明确的，为：

modules/perception/main.cc

APOLLO_MAIN(apollo::perception::Perception);

这是一个宏，展开之后为

int main(int argc, char **argv) {                            
    google::InitGoogleLogging(argv[0]);                        
    google::ParseCommandLineFlags(&argc, &argv, true);         
    signal(SIGINT, apollo::common::apollo_app_sigint_handler); 
    APP apollo_app_;                                           
    ros::init(argc, argv, apollo_app_.Name());                 
    apollo_app_.Spin();                                        
    return 0;                                                  
  }

关于这段代码的具体含义，强烈建议参考 知行合一2018的文章 Apollo Planning模块源代码分析。值得一提的是，Apollo 大部分模块的都是以类似的形式开始的，分析的方式也类似。

感知模块的初始化代码如下所示：

Status Perception::Init() {
  AdapterManager::Init(FLAGS_perception_adapter_config_filename);

  RegistAllOnboardClass();
  /// init config manager
  ConfigManager* config_manager = ConfigManager::instance();
  if (!config_manager->Init()) {
    AERROR << "failed to Init ConfigManager";
    return Status(ErrorCode::PERCEPTION_ERROR, "failed to Init ConfigManager.");
  }
  AINFO << "Init config manager successfully, work_root: "
        << config_manager->work_root();
  //---------------------------注意这段-------------------------------
  const std::string dag_config_path = apollo::common::util::GetAbsolutePath(
      FLAGS_work_root, FLAGS_dag_config_path);   

  if (!dag_streaming_.Init(dag_config_path)) {
    AERROR << "failed to Init DAGStreaming. dag_config_path:"
           << dag_config_path;
 //-----------------------------分割线---------------------------------------
    return Status(ErrorCode::PERCEPTION_ERROR, "failed to Init DAGStreaming.");
  }
  callback_thread_num_ = 5;

  return Status::OK();
}

程序启动代码为：

Status Perception::Start() {
  dag_streaming_.Start();
  return Status::OK();
}

从这两段中可以看到有向无环图 ( directed acyclic graph , DAG )的身影。其实在官方文档里就有提到，感知模块的框架是基于 DAG 图的，每一个功能都以 DAG 中的 sub-node 的形式出现[7]。

The perception framework is a directed acyclic graph (DAG). There are three components in DAG configuration, including sub-nodes, edges and shared data. Each function is implemented as a sub-node in DAG. The sub-nodes that share data have an edge from producer to customer.

DAG 图由以下几个 Subnode 构成

障碍物感知：
- LidarProcessSubnode
- RadarProcessSubnode
- FusionSubnode

交通灯检测

• TLPreprocessorSubnode
• TLProcSubnode

DAG 图如下图所示：

那么到现在为止，其实各部分的程序实现的入口应该比较明朗了，只要找到各个Subnode的执行代码即可。

Doxygen

百度提供了 Doxygen 的文档系统，十分有助于我们了解具体各部分代码之间的关系[8]。

Doxygen是一种开源跨平台的，以类似JavaDoc风格描述的文档系统，完全支持C、C++、Java、Objective-C和IDL语言，部分支持PHP、C#。注释的语法与Qt-Doc、KDoc和JavaDoc兼容。Doxygen可以从一套归档源文件开始，生成HTML格式的在线类浏览器，或离线的LATEX、RTF参考手册 Doxygen_百度百科

Apollo 的 Doxygen 的网址
如图为障碍物感知 （obstacle） 的 Directory dependency graph。

由图片可见，障碍物感知调用了 Radar, Lidar 和 Fusion 的相关函数，其入口在 onboard 文件夹中。我们来看下 onboard 文件夹中有哪些文件。

.
├── BUILD
├── fusion_subnode.cc
├── fusion_subnode.h
├── hdmap_input.cc
├── hdmap_input.h
├── hdmap_input_test.cc
├── lidar_process.cc
├── lidar_process.h
├── lidar_process_subnode.cc
├── lidar_process_subnode.h
├── lidar_process_test.cc
├── object_shared_data.h
├── obstacle_perception.cc
├── obstacle_perception.h
├── radar_process_subnode.cc
├── radar_process_subnode.h
└── sensor_raw_frame.h

可以推断，这些文件就是障碍物感知涉及到的 Subnode 的执行代码了。我是选取的以下3个文件作为分析的入口。

• Radar -> radar_process_subnode.cc
• Lidar -> lidar_process_subnode.cc
• Fusion -> fusion_subnode.cc

这里其实也遗留了一些问题，由于我并不打算进行交通灯感知的分析，直接跳到这里开始分析障碍物感知部分。因而造成一个问题是， 我不太清楚某些文件的具体作用。

比如 obstacle_perception.cc，这个文件看着很像入口，描述也很像入口, 内容也很像入口，但是和 Subnode 的框架似乎没什么关系。

/**
   * @brief The main process to detect, recognize and track objects
   * based on different kinds of sensor data.
   * @param frame Sensor data of one single frame
   * @param out_objects The obstacle perception results
   * @return True if process successfully, false otherwise
   */
  bool Process(SensorRawFrame* frame, std::vector<ObjectPtr>* out_objects);

这个函数中直接调用了障碍物感知的3个核心函数

1. lidar_perception_->Process(velodyne_frame->timestamp_,
                                    velodyne_frame->cloud_, velodyne_pose);
2. radar_detector_->Detect(radar_frame->raw_obstacles_, map_polygons,
                                options, &objects);
3. fusion_->Fuse(multi_sensor_objs, &fused_objects);

再比如 lidar_process.cc，这个文件内容与 lidar_process_subnode.cc 的内容相似，但也不完全相同，也令人感到疑惑。十分希望大家可以给我一些指点。