CoAlign：基于Agent-Object Pose Graph的鲁棒协同感知（3D Object Detection）

标签：Pose Error；Intermediate Fusion；3D Object Detection；V2X； Hybrid Collaboration；Intermediate & Late collaboration；

论文标题：Robust Collaborative 3D Object Detection in Presence of Pose Errors
发表会议/期刊：
开源代码：CoAlign
数据集：OPV2V、V2X-Sim 2.0、DAIR-V2X

问题：在V2X协同感知中，为了相互分享有效的信息，每个agent的精确定位是多agent协作的基础，因为不准确的pose会造成协作中的空间信息错位，导致感知性能比单agent的感知更差。但是，
1. 每个agent的定位模块所估计的6 DoF pose 在实践中并不精确，严重的定位误差将引起很大的relative pose noises，造成空间信息错位，从而大大降低了协同3D目标检测的性能。
2. 许多现有的方法在训练阶段需要pose的ground-truth来监督训练，尽管训练数据中的pose噪声可以离线纠正，但这种pose标记过程可能是昂贵并且不准确的。

6 DoF：

1. 平移运动 刚体可以在3个自由度中平移：X轴平移（向前/后），Y轴平移（向上/下），Z轴平移（向左/右）。
2. 旋转运动 刚体在3个自由度中旋转：X轴旋转-纵摇(Pitch)、Y轴旋转-横摇(Roll)、Z轴旋转-垂摇(Yaw）。

本文方法

为了减轻 pose errors的不利影响，提出了一个混合协同框架CoAlign，它使多个agent共享中间特征和单agent检测结果。其核心思想是：

1. 利用所提出的agent-object pose graph 优化来调整场景中的agents和检测到的objects之间的relative pose relations，以促进pose一致性：一个object的pose从多个agent的角度来看应该是一致的。
2. 为了有效缓解pose errors的影响，还进一步考虑了多尺度的中间融合策略，以全面地聚合多个空间尺度的协作信息。

1 Problem Formulation

基于中间协作的3D目标检测

假设一个场景中有N个合作的agents，每个agent都有感知、通信和检测的能力，协同目标检测目的是通过分布式的相互协作达到每个agent的更好的3D检测能力。对于第i个agent，$O_i$表示第i个agent的感知观测值，$B_i$表示检测输出，${\xi }_i=(x_i,y_i,z_i,{\theta }_i,{\phi }_i,{\psi }_i)$表示其6 DoF pose（${\theta }_i,{\phi }_i,{\psi }_i$ 分别为偏航、俯仰和滚动角度），则基于中间协作策略的3D目标检测：

1. 特征提取：$F_i=f_{\mathrm{encoder}}(O_i)$；
2. 合作车辆（第$j$个agent）将其信息（包括特征$F_j$ 和 pose ${\xi }_j$）传送到第$i$个agent之后，通过pose变换将 $F_j$ 与 $F_i$ 坐标空间对齐：$M_{j\to i}=f_{\mathrm{transform}}({\xi }_i,(F_j,{\xi }_j))$；
3. 特征融合：$F_i^{\prime }=f_{\mathrm{fusion}}(F_i,\{M_{j\to i}{\}}_{j=1,2,...,N})$；
4. 目标检测：$B_i=f_{\mathrm{decoder}}(F_i^{\prime })$。

在实践中，每个6DoF pose ${\xi }_i$是由定位模块实时估计的，其往往具有噪声。因此，从不同agents接收到的特征在经过pose变换之后会有不同的坐标系，无法实现空间对其，导致特征融合错位，进一步使得目标检测不准确。

2 Pose-robust 协同3D目标检测

本文在pose变换之前引入了一个pose校正模块，将pose errors 的影响降到最低。提出了一个混合协作框架CoAlign，其结合了中间和后期协作方式，可同时传输中间特征和目标检测输出。这种混合协作方式与中间协作相比，可以利用检测输出（边界框）作为场景landmarks去纠正agents之间的相对位置。

CoAlign整体架构：

其中Bi和B0 i分别是协作前和协作后的检测输出，ξ0 j→i是第i个代理的视角对第j个代理的校正相对姿势（ξ0 i→i是身份）。

1）单agent检测，特征提取并进一步输出检测到的目标边界框 ：$F_i,B_i=f_{\mathrm{detection}}(O_i)$；
2）V2V通信：向附近合作agent广播单车检测结果、特征图和noisy pose；
3）Agent-object pose graph优化模块，根据 noisy pose 和从其他 agent 获得的检测到的边界框来修正相对pose：$\{{\xi }_{j\to i}^{\prime }{\}}_j=f_{\mathrm{correction}}(\{B_j,{\xi }_j{\}}_{j=1,2,...,N})$；
4）根据校正后的相对pose，将其他agent的特征同步到自车坐标系下：$M_{j\to i}=f_{\mathrm{transform}}(F_j,{\xi }_{j\to i}^{\prime })$；
5）为了进一步确保对pose噪声的鲁棒性，使用多尺度中间融合来更新特征：$F_i^{\prime }=f_{\mathrm{fusion}}(\{M_{j\to i}{\}}_{j=1,2,...,N})$，其中$M_{i\to i}=F_i$；
6）检测头，使用融合后的特征来获得最终的检测结果：$B_i^{\prime }=f_{\mathrm{decoder}}(F_i^{\prime })$。

2.1 具有不确定性估计的单agent检测

在单agent检测阶段，每个agent采用PointPillars模型生成中间特征和预测边界框。

预测边界框的估计不确定性

由于后续需要依靠预测边界框来纠正pose，messy检测可能导致误差更大的相对pose，而每个边界框的估计不确定性可以提供有益的置信度信息来排除不理想的检测。因此，对于每个预测边界框，也估计其不确定性。将每个边界框的不确定性参数化为$b=(\hat{x},\hat{y},\hat{z},\hat{l},\hat{w},\hat{h},\hat{\theta },{\sigma }_x^2,{\sigma }_y^2,{\sigma }_{\theta }^2)$，包括估计边界框的3D中心位置、长度、宽度、高度、偏航角、中心位置方差和角度方差。

1. 边界框中心位置建模
   为了将中心位置$x$（或$y$）建模为一个随机变量，考虑一个均值为$\hat{x}$、方差为${\sigma }_x^2$的高斯分布。 通过最小化 估计的高斯分布$\mathcal{N}(\hat{x},{\sigma }_x^2)$与ground-truth delta 分布$\mathbb{P}(x)=\delta (x-x_0)$之间的 KL divergence 来监督训练，获得预测边界框的不确定性。
   根据推导，由此产生的关于$x$的损失为：$L_x=(\hat{x}-x_0{)}^2/(2{\sigma }_x^2)+\log ({\sigma }_{\mathrm{x}}^2)/2$
2. 偏航角建模
   基于偏航角的周期性，考虑一个均值为 $\hat{\theta }$ 方差为$1/{\sigma }_{\theta }^2$的冯-米塞斯 von-Mises分布。通过最小化冯-米塞斯分布$\mathcal{M}(\hat{\theta },1/{\sigma }_{\theta }^2)$和ground-truth delta 分布$\mathbb{P}(\theta )=\delta (\theta -{\theta }_0)$之间的KL divergence 来监督训练。

将所有损失组合起来，与不确定性估计共同训练单agent检测。总损失为：
$L_{total}=L_{cls}+{\alpha }_{reg}{L}_{reg}+{\alpha }_{center}(L_x+L_y)+{\alpha }_{\theta }{L}_{\theta }$，
其中$L_{cls}$是目标分类的交叉熵损失，$L_{reg}$是边界框回归参数的平滑L1损失，${\alpha }_{reg},{\alpha }_{center},{\alpha }_{\theta }$为损失平衡的超参数。（在本文实验中， 设置 ${\alpha }_{reg}=2,{\alpha }_{center}=0.25,{\alpha }_{\theta }=0.125$ ）

2.2 Agent-Object Pose Graph优化

在单Agent检测之后，每个Agent将与其他合作Agent共享三种信息，包括：
i）由其自身定位模块估计的pose信息 ${\xi }_i$；
ii）自车检测的边界框；
iii）自身提取到的特征图$F_i$。
其中，pose信息和边界框将用于纠正相对pose，特征图将用于特征融合；另外，由于边界框检测的旋转分量只有偏航角，将每个pose简化到二维空间${\xi }_i=(x_i,y_i,{\theta }_i)$。

为了校正相对位置，需要对齐多个agent对于同一物体的相应边界框。首先，车辆接收来自所有其他agent的信息后，将建立其内部的Agent-Object Pose Graph 以模拟Agent和检测物体之间的关系，Graph由agent节点、Object节点和edge组成。
1）agent节点集：由所有的agent组成；
2）Object节点集：Object是通过对所有从agent接收收到的相似边界框进行空间聚类而得到的；
3）edge集：反映了Agent和Object之间的检测关系，当一个agent检测到一个Object的边界框时，就设置一条边来连接它们。

另外，每个节点都有一个相应的pose，每条edge也反映了一个agent和一个object之间的相对pose。每个Agent的pose ${\xi }_j$由其定位模块估计获得，而每个object的pose ${\chi }_k$从该object的多个预测边界框中采样得到。

优化

理想情况下，同一个object的pose是唯一的，即从多个agent的视角来看object的pose应该是一致的。所以，pose一致性误差 $e_{jk}=z_{jk}^{-1}◦({\xi }_j^{-1}◦{\chi }_k)\in {\mathbb{R}}^3$应该为0，为了促进pose一致性，考虑以下优化问题 ：

其中，${\Omega }_{jk}=diag([1/{\sigma }_x^2,1/{\sigma }_y^2,1/{\sigma }_{\theta }^2])\in {\mathbb{R}}^{3\times 3}$类似于 graph-based SLAM中的信息矩阵，其对角线元素为边界框的不确定性估计，用于衡量对pose估计的置信度。

该优化问题与 graph-based SLAM优化类似，但语义不同：Graph-based SLAM在多个时间戳中对齐同一object，而Agent-Object Pose Graph优化在同一时间戳中对齐由多个agent检测到的同一object。

在训练迭代时，将自车的pose固定下来，并更新所有其他agent和object的pose，最后计算出相对pose ${\xi }_{j\to i}^{\prime }={\xi }_i^{\prime -1}◦{\xi }_j^{\prime }$。有了校正后的相对pose ${\xi }_{j\to i}^{\prime }$，自车就可以将接收到的特征 $Fj$ 转换到自车坐标系下，使其与自车捕获的特征$F_i$具有相同的坐标系。

2.3 多尺度特征融合

在空间对齐后，每个agent需要将自身获得的信息与其他agent的协作信息聚合以获得一个具有更多信息量的融合特征。由于较小尺度的特征可以提供更详细的几何和语义信息，而较大尺度的特征对pose误差的敏感性较低。因此，为了进一步减轻pose噪声的影响，这里采用了多尺度融合方法来融合多个空间尺度的特征，以产生信息量大且具有鲁棒性的特征。

假设$F_{j\to i}^{(\ell )}$ 为第 $\ell$ 个空间尺度的协作特征，多尺度融合的工作原理为

其中$g_{\ell }$是下采样为2的第$\ell$个 residual layer，fuse(-)是沿 agent 维度的attention 加权，$u_{\ell }$ 是第$\ell$个尺度的反卷积上采样，concat(-)是沿特征通道维度的拼接。融合的特征$F_i^{\prime }$将被解码为最终的检测结果输出$B_i^{\prime }$。

3 实验

3.1 执行细节

1. 在训练阶段，为了模拟定位误差，在pose信息的二维中心$x、y$和偏航角$\theta$上添加高斯噪声。采用通用图优化算法（g2o）来优化Agent-Object Pose Graph，选择密集求解器用于增量方程，Levenberg-Marquartdt算法用于迭代优化，最大迭代次数为1000。对于多尺度中间融合，采用通道数分别为(128, 256) 的2个residual layer。
2. 为了验证在存在pose errors的情况下的3D目标检测性能，将CoAlign与现有的有/无pose errors设计的方法在在具有不同噪声水平的OPV2V、V2X-Sim 2.0和DAIR-V2X数据集上，比较它们在IoU阈值为0.5和0.7时的平均精度（AP）。实验表明，CoAlign在这三个数据集的各种噪声水平下都明显优于之前的方法，当噪声水平较高时，领先的差距更大。

4 总结

提出了一个对未知 pose errors 具有鲁棒性的混合协同3D检测框架CoAlign。采用中期和后期协作的混合策略，在agent-object pose graph modeling使用单车检测输出的边界框，来促进合作agent之间的 pose consistency来校正relative pose。采用多尺度的数据融合策略，在多个空间分辨率上聚合中间特征。与现有的其他方法相比，CoAlign在训练中不需要pose的ground-truth来监督学习，但是，在实验验证时仍然对pose 噪声做了假设，即在pose信息的二维中心和偏航角上添加了高斯噪声。