自动驾驶Apollo6.0源码阅读-V2X篇:V2X融合代码的基本流程
Fusion流程
1.Fusion模块在哪里启动?
这个是在/home/mogo/lxy/Gitte/apollo/modules/v2x/dag/v2x_perception_fusion.dag中定义的。
所以,我们需要去找V2XFusionComponent。并且知道了它的配置v2x_fusion_tracker.conf中;
能够得到以下信息:
- 融合参数
2.Fusion Component的初始化?
Fusion Component的地址是这个:
modules/v2x/fusion/apps/v2x_fusion_component.h
Init
第一步,创建接收器v2x_obstacle_topic
第二步,创建接收器localization_topic
第三步,创建接收器perception_obstacle_topic
3.Fusion 的流程框架
核心方法是Proc
核心的方法是V2XMessageFusionProcess,那么V2XMessageFusionProcess中的核心代码是什么呢?
流程图
有趣的是,
std::vector<Object> fused_objects;
std::vector<Object> v2x_fused_objects;
std::vector<std::vector<Object>> fusion_result;
这三个每次进来都会重新定义。
下面分开讲解核心代码
3.1 fusion_.CombineNewResource(perception_objects, &fused_objects, &fusion_result);
3.2 fusion_.CombineNewResource(v2x_objects, &fused_objects, &fusion_result);
第一步,遍历fused_objects,如果fused_objects.size()<1,将new_objects的目标赋值到fused_objects;并且将每个new_objects以vector形式push_back给fusion_result;
(new_objects,可以是perception_objects,也可以是v2x_objects)
第二步,计算fused_objects与new_objects的代价矩阵,存入association_mat;
第三步,采用匈牙利方法获得代价矩阵的最小分配结果。
第四步,更新fused_objects和fusion_objects.
3.3 fusion_.GetV2xFusionObjects(fusion_result, &v2x_fused_objects);
主要工作是重新整合fused_objects的type,整合完之后,序列化就发布出去。
该模块的融合,没有进行跟踪,仅进行了匹配工作。
4.compute Associate Matrix
ComputeAssociateMatrix
可以很清晰的看到,关键两个函数是:CheckDisScore()和CheckTypeScore(),下面展开讲这两个:
CheckDisScore()
这个函数,核心在CheckODistance()
**std::hypot()**函数返回所传递的参数平方和的平方根(相当于2维欧式距离),最终DisScore为:
s
c
o
r
e
=
2.5
×
m
a
x
{
0
,
s
c
o
r
e
p
a
r
a
m
s
.
m
a
x
m
a
t
c
h
d
i
s
t
a
n
c
e
(
)
−
d
i
s
}
score = 2.5 \times max \{0,score_params_.max_match_distance() - dis \}
score=2.5×max{0,scoreparams.maxmatchdistance()−dis}
CheckTypeScore
根据new_objects与fused_objects的类型区别,选择不同的计算score的方法。
回到代价矩阵计算方法:
如果score大于等于阈值则输出score,否则为0;