标签：Pose errors；Late collaboration；Object Detection；V2V
论文标题：An Efficient and Robust Object-Level Cooperative Perception Framework for Connected and Automated Driving
发表会议/期刊：
问题：自动驾驶的协同感知存在两个主要的挑战：1）通信带宽有限，数据传输量大导致信息延迟；2）不够准确的车辆定位和Pose信息。
1 方法
为了最大限度减少通信网络的负担，采用后期协作的方式以避免信息传输延迟；针对不准确的pose信息，提出了一个对不准确的pose具有鲁棒性的协同感知框架。
协同感知框架
假设每个CAV的位置和偏航角都具有高斯噪声： P k ^ = [ x k + δ p , y k + δ p , z k + δ p ] T P_{\hat{k}} = [x_k + δ_p, y_k + δ_p, z_k + δ_p]^T Pk^=[xk+δp,yk+δp,zk+δp]T； E k ^ = [ θ k , ψ k , φ k + δ φ ] T E_{\hat{k}} = [θ_k, ψ_k, φ_k + δ_φ]^T Ek^=[θk,ψk,φk+δφ]T；其中， δ p δ_p δp和 δ φ δ_φ δφ表示高斯噪声。每辆CAV经过数据收集和目标检测之后，产生了一组3D边界框 P k \mathcal{P}^k Pk，并通过V2V通信与Ego车辆共享 P k \mathcal{P}^k Pk和pose信息（ P k ^ P_{\hat{k}} Pk^和 E k ^ E_{\hat{k}} Ek^）。
1）粗略匹配：Ego基于发送车辆和自身的pose计算相对变换矩阵 T k ^ 0 T^0_{\hat{k}} Tk^0，然后将接收到的边界框转换到Ego坐标系下，实现粗略匹配（绿色方框被转换到黄色方框的附近，但没有精确对齐）。
2）Object关联：提取接收到的边界框（绿色）和Ego本地检测到的边界框（黄色）的几何中心（生成点集： P c 0 = { p 1 0 , . . . , p M 0 } ∈ R 3 × M \mathcal{P}_c^0 = \{p^0_1, ..., p^0_M\} ∈ \mathbb{R}^{3×M} Pc0={
p10,...,pM0}∈R3×M， Q c 0 = { q 1 0 , . . . , q M 0 } ∈ R 3 × N \mathcal{Q}_c^0 = \{q^0_1, ..., q^0_M\} ∈ \mathbb{R}^{3×N} Qc0={
q10,...,qM0}∈R3×N），并将来自相同object的点关联起来，其他的点被过滤掉。
3）精细匹配：使用匹配上的点来计算转换矩阵 T k ^ k T^k_{\hat{k}} Tk^k，再将粗匹配的结果基于该转换矩阵进一步对齐。
4）全局融合：Objects以非最大抑制（NMS）的方式进行合并，生成融合后的3D边界框，用于Ego车辆的下游任务。
为了找到来自不同局部坐标系的而属于同一object的3D边界框之间的对应关系，提出了两种匹配算法以将它们融合到一个统一的坐标系中：基于迭代最近点（Iterative Closest Point, ICP）的算法和基于最优传输（Optimal Transport, OT）的算法。
1.1 ICP-based framework
ICP是一个广泛使用的点云注册算法，通过迭代优化使局部对齐最优，找到source点和target点之间的刚性变换。
ICP算法基于接收到的边界框和Ego本地检测到的边界框的几何中心点的两个点集： P c 0 \mathcal{P}_c^0 Pc0和 Q c 0 \mathcal{Q}_c^0 Qc0来寻找属于相同object的中心点的集合 M ^ \mathcal{\hat{M}} M^，并将它们对齐：
- Step 1：Neighbour Selection
应用一个简单的相邻搜索算法，分别在 P c 0 \mathcal{P}_c^0 Pc0和 Q c 0 \mathcal{Q}_c^0 Qc0中筛选出可能表示相同object的两个点集合： M ^ 1 \mathcal{\hat{M}}_1 M^1和 M ^ 2 \mathcal{\hat{M}}_2 M^2。 - Step 2：Point Set Registration
所获得的两个可能点集（ M ^ 1 \mathcal{\hat{M}}_1 M^1和 M ^ 2 \mathcal{\hat{M}}_2 M^2）分别作为target点和source点，使用ICP算法来计算出发送车辆的real位置和noisey位置之间的相对变换矩阵 T k ^ k T^k_{\hat{k}} Tk^k。
变换矩阵 T k ^ k T^k_{\hat{k}} Tk^k将用于对粗匹配的3D边界框进行更精准的对齐。
但是，neighbour selection 步骤需要选择适当的参数 r r r（两个边界框中心点之间的距离的阈值），因为它将直接影响边界框中心点的匹配结果（本文设定r=2m）。如果 r r r 太小，可能找不到属于相同object的点集；而如果 r r r 太大，将会包含一些异常点（即那些没有对应关系的点）。
1.2 OT-based framework
同样，给定接收到的边界框和Ego本地检测到的边界框的几何中心点的两个点集： P c 0 \mathcal{P}_c^0 Pc0和 Q c 0 \mathcal{Q}_c^0 Qc0，基于OT的匹配算法的目的是，通过使source（ P c 0 \mathcal{P}_c^0 Pc0中的点）和 target（ Q c 0 \mathcal{Q}_c^0 Qc0中的点）之间的传输cost最小来找到属于相同object的边界框中心点的对应关系。
对于每个source点，要从target点集找到一个唯一与其相对应的点，需要满足两个约束条件：
1） P c 0 \mathcal{P}_c^0 Pc0中的target在 Q c 0 \mathcal{Q}_c^0 Qc0中最多只能有一个对应点；
2）有些target会因为视线不同、遮挡或检测错误而无法匹配。
- Step1：Optimal Transport
第1-3行：计算 P c 0 ( i ) \mathcal{P}^0_c(i) Pc0(i)与 Q c 0 ( j ) \mathcal{Q}^0_c(j) Qc0(j)之间的欧氏距离 C ( i , j ) C(i,j) C(i,j)，得到一个一个cost矩阵 C ∈ R M × N C∈\mathbb{R}^{M×N} C∈RM×N，作为Ego和发送车辆检测到的边界框之间关联的transportation cost。
第4-5行：通过添加新的行和列将 C C C进行增广 C ˉ \bar{C} Cˉ，增加的行和列用一个超参数 α ∈ R α∈\mathbb{R} α∈R填充，作为一个 "垃圾箱"以处理没有被匹配到的边界框中心点。
第6-8行：基于增广的transportation cost矩阵 C ˉ \bar{C} Cˉ，采用Sinkhorn算法通过解决optimal transport问题找到最佳分配矩阵 P ˉ ∈ R ( M + 1 ) × ( N + 1 ) \bar{P}∈\mathbb{R}^{(M+1)×(N+1)} Pˉ∈R(M+1)×(N+1)，其中 P ˉ ( i , j ) \bar{P}(i,j) Pˉ(i,j)表示 C ˉ ( i , j ) \bar{C}(i,j) Cˉ(i,j)上的分配概率；
最终的分配矩阵 P P P通过drop P ˉ \bar{P} Pˉ 的最后一行和一列来获得，如果 P ( i , j ) = a r g m a x P ( i , : ) = a r g m a x P ( : , j ) P(i, j) = argmaxP(i, :) = argmaxP(:, j) P(i,j)=argmaxP(i,:)=argmaxP(:,j)，其中 i ∈ [ 1 , M ] i∈[1, M] i∈[1,M]， j ∈ [ 1 , N ] j∈[1, N] j∈[1,N]，则点 P c 0 ( i ) \mathcal{P}^0_c(i) Pc0(i)和 Q c 0 ( j ) \mathcal{Q}^0_c(j) Qc0(j)被关联，得到所有可能匹配的点对集合 M ^ \mathcal{\hat{M}} M^ ( P c 0 ( i ) , Q c 0 ( j ) ) (\mathcal{P}^0_c(i), \mathcal{Q}^0_c(j)) (Pc0(i),Qc0(j))。 - Step 2: Random Sampling
由于存在异常值或带噪声的边界框信息，所得到的匹配点对集 M ^ \mathcal{\hat{M}} M^可能是不准确的。因此，采用随机抽样方法来进一步提高鲁棒性，通过进行多次抽样来找到使匹配率 η η η最大的最佳变换矩阵。
对于每次抽样，
1）随机选择一个子集 M ^ s ⊆ M ^ \mathcal{\hat{M}}_s⊆\mathcal{\hat{M}} M^s⊆M^，并在子集上用奇异值分解法（SVD）来估计变换矩阵 T k , s k ^ T^{\hat{k}}_{k,s} Tk,sk^；
2）在 M ^ \mathcal{\hat{M}} M^中的所有点对 ( o i p , o j q ) (o^p_i, o^q_j ) (oip,ojq)上基于变换矩阵 T k , s k ^ T^{\hat{k}}_{k,s} Tk,sk^进行对齐，如果 ∣ ∣ o i p − T k , s k ^ o j q ∣ ∣ 2 ≤ τ ||o^p_i- T^{\hat{k}}_{k,s} o^q_j ||_2\leqτ ∣∣oip−Tk,sk^ojq∣∣2≤τ，则认为这一对边界框是准确对齐的；（本文设定 τ = 0.25 τ=0.25 τ=0.25m）
3）计算本次抽样的对齐率 η s η_s ηs。 - Step 3：Optimal Transformation Selection
选择具有最大匹配率 η η η的那次抽样计算得到的变换矩阵 T k , s ∗ k ^ T^{\hat{k}}_{k,s^∗} Tk,s∗k^作为最终的变换矩阵 T k k ^ = T k , s ∗ k ^ T^{\hat{k}}_k = T^{\hat{k}}_{k,s^∗} Tkk^=Tk,s∗k^。
变换矩阵 T k ^ k T^k_{\hat{k}} Tk^k将用于对粗匹配的3D边界框进行更精准的对齐。
2 实验
在OPV2V数据集上对所提出的两个算法和baselines进行了评估。
为模拟pose error，在CAVs的真实位置上添加高斯噪声： δ p ∼ N ( 0 , σ p ) , σ p ∈ [ 0 , 1 ] δ_p∼N(0,σ_p), σ_p∈[0,1] δp∼N(0,σp),σp∈[0,1]；在偏航角上添加高斯噪声： δ φ ∼ N ( 0 , σ φ ) , σ φ ∈ [ 0 , 2.5 ] δ_φ∼N(0, σ_φ), σ_φ∈[0,2.5] δφ∼N(0,σφ),σφ∈[0,2.5]。其中，噪声的标准差通过均匀采样获得。平均精度（AP）作为目标检测的评估指标。
3 总结
通过使用基于Iterative Closest Point（ICP）和Optimal Transport（OT）的融合算法，来增强协同感知框架对pose error的鲁棒性。
将接收到的边界框转换到Ego车辆的坐标系下后，将Ego和发送车辆检测到的3D边界框的几何中心分别视为Source点和target点：
- 基于ICP的匹配算法通过迭代优化找到Source点和target点之间的相对变换；但是，该方法对那些没有对应关系的点比较敏感，并且需要选择一个合适的neighbour search参数，直接影响到点云注册结果。
- 基于OT的方法将相对变换计算视为一个Optimal Transport问题，以使Source和target之间的运输成本最小来得到最佳的pose变换矩阵。
基于ICP和OT的方法的性能都依赖于共同FOV内目标的数量。