Apollo感知分析之跟踪对象信息融合

自动驾驶使用的感知类的传感器，主要有激光雷达、毫米波雷达、摄像头、组合导航。

激光雷达安装在车顶，360度同轴旋转，可以提供周围一圈的点云信息。另外，激光雷达不光用于感知，也可用在定位和高精度地图的测绘。

毫米波雷达安装在保险杠上，与激光雷达原理类似，通过观察电磁波回波入射波的差异来计算速度和距离。

组合导航分为两部分，一部分是GNSS板卡，另一部分是INS。当车辆行驶到林荫路或是建筑物附近，GPS会产生偏移或是信号屏蔽的情况，这时可通过与INS进行组合运算解决问题。

而在Apollo多传感器融合定位模块的融合框架中，

包括两部分：惯性导航解算、Kalman滤波；

融合定位的结果会反过来用于GNSS定位和点云定位的预测；

融合定位的输出是一个6-dof的位置和姿态，以及协方差矩阵。

自动驾驶中感知学习最大问题是系统对模块的要求，对准确率/召回率/响应延时等，要求很高，牵扯到安全。如果在自动驾驶中的感知学习中，出现一些障碍物的漏检、误检，就会会带来安全问题。漏检会带来碰撞，影响事故；误检会造成一些急刹，带来乘车体验的问题。

那么，在在进行几何计算+时序计算后，如何进行环视融合？

以下是Apollo社区开发者朱炎亮在Github-Apollo-Note上分享的《跟踪对象信息融合》，感谢他为我们在融合这一步所做的详细注解和释疑。

面对复杂多变、快速迭代的开发环境，只有开放才会带来进步，Apollo社区正在被开源的力量唤醒。

在上一步HM对象跟踪步骤中，Apollo对每个时刻检测到的Object与跟踪列表中的TrackedObject进行匈牙利算法的二分图匹配，匹配结果分三类：

如果成功匹配，那么使用卡尔曼滤波器更新信息(重心位置、速度、加速度)等信息；

如果失配，缺少对应的TrackedObject，将Object封装成TrackedObject，加入跟踪列表；

对于跟踪列表中当前时刻目标缺失的TrackedObject(Object都无法与之匹配)，使用上时刻速度，跟新当前时刻的重心位置与加速度等信息(无法使用卡尔曼滤波更新，缺少观测状态)。对于那些时间过长的丢失的跟踪目标，将他们从跟踪队列中删除。

而在跟踪对象信息融合的阶段，Apollo的主要工作是，给定一个被跟踪的物体序列：

(Time_1,TrackedObject_1),

(Time_2,TrackedObject_2),

...,

(Time_n,TrackedObject_n)

每次执行LidarSubNode的回调，都会刷新一遍跟踪列表，那么一个被跟踪物体的信息也将会被刷新(重心位置，速度，加速度，CNN物体分割--前景概率，CNN物体分割--各类别概率)。N次调用都会得到N个概率分布(N个CNN物体分割--前景概率score，N个CNN物体分割--各类物体概率Probs)，我们需要在每次回调的过程中确定物体的类别属性，当然最简单的方法肯定是。但是CNN分割可能会有噪声，所以最好的办法是将N次结果联合起来进行判断！

跟踪物体的属性可以分为4类：

UNKNOWN--未知物体

PEDESTRIAN--行人

BICYCLE--自行车辆

VEHICLE--汽车车辆

E.g. 5次跟踪的结果显示某物体的类别/Probs分别为：

如果直接对每次跟踪使用argmax(Probs)直接得到结果，有时候会有误差，上表frame3的时候因为误差结果被认为是汽车，所以需要根据前面的N次跟踪结果一起联合确定物体类别属性。Apollo使用维特比算法求解隐状态概率。这里做一个简单地描述，具体参考维特比Viterbi算法:

维特比算法前提是状态链是马尔可夫链，即下一时刻的状态仅仅取决于当前状态。(假设隐状态数量为m，观测状态数量为n)，隐状态分别为s1,s2,s3,....sm; 可观测状态分别为o1,o2,...,on。 则有：

状态转移矩阵P(mxm): P[i,j]代表状态i到状态j转移的概率。$\sum_^m P[i,j] = 1$ 发射概率矩阵R(mxn): R[i,j]代表隐状态i能被观测到为j现象的概率。 $\sum_^n P[i,j] = 1$

现在假设初始时刻的m个隐状态概率为：(s0_1, s0_2, ..., s0_m)，第一时刻观测到的可观察状态为ok，那么如何求第一时刻的隐状态：

1、上时刻隐状态si，第一时刻隐状态sj的联合概率为：

$$ p(s1_j, s0_i) = p(prv_state=s0_i) * P(sj|si) $$

因此可以得到p(s1_1, s0_i), p(s1_2, s0_i), ..., p(s1_m, s0_i)，也可以得到p(s1_j, s0_1), p(s1_j, s0_2), p(s1_j, s0_3),..., p(s1_j, s0_m)。最终是一个mxm的联合概率矩阵。

2、同时上时刻隐状态si，第一时刻隐状态sj情况下可观测到观察状态ok的概率为：

$$ p(ok|s1_j, s0_i) = p(s1_j, s0_i) * R(ok|sj) $$

同理可得到p(ok|s1_0, s0_i), p(ok|s1_1, s0_i), p(ok|s1_2, s0_i),..., p(ok|s1_m, s0_i)，也是一个mxm的条件概率矩阵。

若最终的条件概率矩阵中p(ok|s1_jj, s0_ii) 值最大，那么就可以得出结论这个时刻的隐状态为s_jj。

Apollo也采取类似的Viterbi算法做隐状态的修正。

从上述代码可以看到使用CRF进行隐状态(物体类别)修正，主要步骤为：

将TrackedObject加入到sequence(sequence数据结构为map

筛选，对于背景物体直接标记为UNKNOWN::UNMOVABLE类别

新序列生成，sequence[track_id]里面的已存储的记录过多，而修正只需要最近一段时间的序列即可，所以使用GetTrackInTemporalWindow函数可以获取近期((默认20)以内的所有物体跟踪序列

CRF隐状态矫正，使用上述new_sequence配合Viterbi算法进行矫正

上述的难点就在于Step 4，因此本节从代码分析描述CRF修正的原理。在FuseWithCCRF函数中共分两步

Step1. 状态平滑(物体状态整流)

Step2. Viterbi算法推离状态

从上述代码可以得到以下结论：

原始CNN分割与后处理得到当前跟踪物体对应4类的概率为object->type_probs

原始CNN分割与后处理得到当前跟踪物体前景概率为conf=object->score，那么背景的概率为1-conf

平滑公式为：

single_prob = iter->second * single_prob + epsilon single_prob = conf * single_prob + (1.0 - conf) * confidence_smooth_matrix_ * single_prob

iter->second(CNNSegClassifier Matrix)矩阵为：

0.9095 0.0238 0.0190 0.0476

0.3673 0.5672 0.0642 0.0014

0.1314 0.0078 0.7627 0.0980

0.3383 0.0017 0.0091 0.6508

confidence_smooth_matrix_(Confidence)矩阵为：

1.00 0.00 0.00 0.00

0.40 0.60 0.00 0.00

0.40 0.00 0.60 0.00

0.50 0.00 0.00 0.50

Viterbi算法推理代码如下：

上述代码中，fused_oneshot_probs_是每个时刻独立的4类概率，经过平滑和log(·)处理。transition_matrix_是状态转移矩阵P，维度为4x4，经过log(·)处理，fused_sequence_probs_为Viterbi算法推理后的修正状态(也就是真实的隐状态)。

那么根据上时刻的真实的隐状态fused_sequence_probs_[i-1]和状态转移矩阵transition_matrix_，可以求解开始提到的mxm(4x4)联合状态矩阵，其中矩阵中元素fused_sequence_probs_[i][j]的求解方式为：

p(si_j, si-1_k) = p(prv_state=si-1_k) * P(sj|sk)

对应代码：

上述存在几个问题：

问题1： 为什么代码用的加法？

因为代码中是将乘法转换到log(·)做运算，上述已提到transition_matrix_和fused_oneshot_probs_都是经过log(·)处理。那么上述公式等价于：

log p(si_j, si-1_k) = log p(prv_state=si-1_k) + log P(sj|sk)

只要最终将log p(si_j, si-1_k) 经过 exp(·)处理还原真实的概率即可。

问题2.s_alpha_是什么意思？

有待后续深入研究，这里还不曾研究透。

问题3. 为什么代码会额外多乘一个概率

p(ok|sj)--used_oneshot_probs_[i](right)

Apollo中对于当前状态的推理就是，求解前后两个时刻关联最紧密(上时刻各状态中与当前时刻状态sj变换概率最大的状态si)，本质就是求解开始例子中提到联合概率矩阵。在开始的例子中，我们举例隐状态s共m个。紧接着计算前后两个时刻状态的联合概率矩阵。在这个例子中，隐状态是onehot类型，也就是[0,0,0,si-1=1,0,0,0]，那么当我们的隐状态不是onehot，也就是这种类型[0.1,0.1,0.1,si-1=0.6,0,0,0.1]，那么如何求解联合概率矩阵？

做法也很类似，只需要将原先计算目标：

p(si_j, si-1_k) = p(prv_state=sk) * P(sj|si)

变成计算目标：

p_new(si_j, si-1_k) = p(prv_state=sk) * P(sj|si) * p(current_state=sj)

上述代码中const double prob就是联合概率矩阵:p_new(si_j, si-1_k)，最终取最大元素对应的(ii,jj)组，也就是(left，right)组，ii(left)就是当前时刻的隐状态：

从上述代码和注释，可以很明显的看到求解的过程，fused_sequence_probs_是各个时刻物体的真实修正状态，state_back_trace_是一条由后往前连接的最佳回溯状态链，如下图：

Apollo状态转移矩阵为：

0.34 0.22 0.33 0.11

0.03 0.90 0.05 0.02

0.03 0.05 0.90 0.02

0.06 0.01 0.03 0.90

经过上述计算得到的fused_sequence_probs_矩阵就是所有时刻跟踪物体的状态概率(log(·)处理过，所以必须经过exp(·)还原概率)，最后就是求解当前时刻，也就是sequence最后一列各类物体的概率。

上面代码之所以有FromEigenVector过程，主要是Apollo原始定义了6中object type：

UNKNOWN--未知物体

UNKNOWN_MOVABLE--未知可移动物体

UNKNOWN_UNMOVABLE--未知不可移动物体

PEDESTRIAN--行人

BICYCLE--自行车辆

VEHICLE--汽车车辆

实际只用到了0,3,4,5四类。

自Apollo平台开放已来，我们收到了大量开发者的咨询和反馈，越来越多开发者基于Apollo擦出了更多的火花，并愿意将自己的成果贡献出来，这充分体现了Apollo『贡献越多，获得越多』的开源精神。为此我们开设了『开发者说』板块，希望开发者们能够踊跃投稿，更好地为广大自动驾驶开发者营造一个共享交流的平台！

* 以上内容为开发者原创，不代表百度官方言论。