自动驾驶Apollo6.0源码阅读－感知篇：感知融合代码的基本流程

最新推荐文章于 2024-09-26 17:44:40 发布

刘诺夫斯基

最新推荐文章于 2024-09-26 17:44:40 发布

阅读量2.3k

点赞数 3

分类专栏： Apollo学习文章标签：自动驾驶人工智能机器学习

本文链接：https://blog.csdn.net/ControlLearner/article/details/122973217

版权

Apollo学习专栏收录该内容

5 篇文章 8 订阅

订阅专栏

自动驾驶Apollo6.0源码阅读－感知篇：感知融合代码的基本流程

Fusion

Fusion

── fusion
   ├── app 入口
   ├── base 定义的数据类(需清晰了解)
   ├── common 证据推理，IF，KF等方法
   └── lib
       ├── data_association 
       │   └── hm_data_association 关联匹配算法
       ├── data_fusion
       │   ├── existence_fusion
       │   │   └── dst_existence_fusion 存在性证据推理更新
       │   ├── motion_fusion
       │   │   └── kalman_motion_fusion 卡尔曼更新运动属性
       │   ├── shape_fusion
       │   │   └── pbf_shape_fusion     更新形状属性
       │   ├── tracker
       │   │   └── pbf_tracker          航迹类
       │   └── type_fusion
       │       └── dst_type_fusion      类别证据推理更新
       ├── dummy 
       ├── fusion_system
       │   └── probabilistic_fusion 概率融合入口
       ├── gatekeeper
       │   └── pbf_gatekeeper 门限
       │       └── proto
       └── interface

Fusion模块在哪儿启动？

(dag文件有一个或者多个module_config，而每个module_config中对应一个或者多个components。)
关键概念：component
Perception 是一个大的 Component，它包含了很多子 Component，而数据融合作为一个子 Component 存在;
通过modules/perception/production/dag/dag_streaming_perception.dag中定义：
在这里插入图片描述

所以我们需要去找FusionComponent，并且知道了其配置在fusion_component_conf.pb.txt中：
在这里插入图片描述

能够得到以下信息：

融合方法：ProbabilisticFusion
主传感器：velodyne128
融合结果存放：/perception/vehicle/obstacles

FusionComponent的初始化

路径：modules/perception/onboard/component/fusion_component.cc
FusionComponent类主要负责接收融合所需要的来自不同传感器的目标序列信息，然后利用ProbabilisticFusion融合类完成实际融合操作，得到融合后的感知结果，最后把融合结果在FusionComponent发布，供其他模块使用。
在这里插入图片描述

比较关心的应该是InitAlgorithmPlugin（）内的具体方法，这个是用来初始化 Component 涉及的算法的。
在这里插入图片描述
融合具体的实现方法明显在ObstacleMultiSensorFusion类中。

(具体未完全理解，先跳过了。)ObstacleMultiSensorFusion关键在于BaseFusionSystemRegisterer类;
结合前面的配置信息，我们知道在 ObstacleMultiSensorFusion::Init(）中 fusion_ 将由 ProbobilisticFusion 实现，那么这个类在哪里呢？
路径：/modules/perception/fusion/lib/fusion_system/probabilistic_fusion/probabilistic_fusion.cc
在这里插入图片描述

概率融合方法：ProbobilisticFusion

在这里插入图片描述
公共的一个Init()和一个Fuse()

Init()

bool ProbabilisticFusion::Init(const FusionInitOptions& init_options)
主要是读取实例化参数，参数定义在：modules/perception/production/data/perception/fusion/probabilistic_fusion.pt
在这里插入图片描述
可以看出实际融合的传感器包括lidar、radar和camera，以及实际使用的跟踪算法、数据关联算法、门限保持方法的具体类名，禁止单独创建航迹(track)的sensor，另外还定义了其他一些参数。
成员变量有 trackers_、macher_、gate_keeper_ 这些都是目标跟踪相关的;

Fusion 的流程框架

回到FusionComponent类，清晰的知道核心方法为Proc：
在这里插入图片描述
核心的核心InternalProc中核心代码是：

又需跳转到fusion_->Process

fusion_->Fuse()

代码中有相应的注释，大概流程主要是4步：
在这里插入图片描述
工程量比较大，下面分开讲解。

1.save frame data

保存传感器进来的一帧数据，存储到deque容器中。
PS.

只有当主传感器数据进来一帧之后，将started_ = true才会存储其他传感器的数据。
如果当前帧不是主传感器，不会进行后续的操作，将return.

关键函数：AddSensorMeasurements
执行对象是 SensorManager

2.query related sensor_frames for fusion

查询所有传感器最新一帧的数据，插入到frames中，按时间排序。
关键函数“：GetLatestFrames
在这里插入图片描述

3.perform fusion on related frames

FuseFrame
在这里插入图片描述
关键函数：FuseForegroundTrack(frame)

前景融合主要分为四个步骤：
1.目标之间数据关联[后续展开]
2.更新和新数据匹配上的 Tracks
3.更新未和数据匹配上的 Tracks
4.为未匹配到的新数据创建新的 Tracks

4. collect fused objects

在这里插入图片描述
先通过 gate_keeper 判断能不能将数据发布出去，如果能的话再执行 CollectObjectsByTrack 方法。
大致规则：

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

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