原文链接：https://arxiv.org/abs/2211.07214

I. 引言

协同3D目标检测任务需要智能体之间交换信息，但不完美定位导致的位置估计误差会导致消息错位，影响协同性能。目前处理定位误差的方法可分为两类：引入监督使模型能意识到位姿误差，以及设计鲁棒的框架或模型结构。但大多数都需要真实位姿进行监督，导致实用性较低。

本文提出CoAlign，一种对未知位姿误差有鲁棒性的混合协同框架（中期协同+晚期协同）。解决方案依赖智能体-物体位姿图建模，以提高协同智能体的位姿一致性。此外，还使用多尺度特征融合策略，以在不同分辨率下聚合中间特征。与之前的方法不同，CoAlign在训练时无需真实位姿监督，也不对位姿误差进行明确假设。实验证明CoAlign可以极大降低相对位姿误差，并在存在位姿误差时达到sota性能。

III. 问题描述

设场景中有$N$个智能体，$O_i,B_i$分别为智能体$i$的观测和检测输出。标准的中期协同3D目标检测流程如下：
F i = f e n c o d e r ( O i ) M j → i = f t r a n s f o r m ( ξ i , ( F j , ξ j ) ) F i ′ = f f u s i o n ( { M j → i } j = 1 N ) B i = f d e c o d e r ( F i ′ ) F_i=f_{encoder}(O_i)\\ M_{j\rightarrow i}=f_{transform}(\xi_i,(F_j,\xi_j))\\ F'_i=f_{fusion}(\{M_{j\rightarrow i}\}_{j=1}^N)\\ B_i=f_{decoder}(F'_i) Fi​=fencoder​(Oi​)Mj→i​=ftransform​(ξi​,(Fj​,ξj​))Fi′​=ffusion​({Mj→i​}j=1N​)Bi​=fdecoder​(Fi′​)

其中$F_i$为特征，${\xi }_i$为6自由度位姿，$M_{j\to i}$为智能体$j$传输给智能体$i$的消息（$M_{i\to i}=F_i$），其坐标系与$F_i$通过位姿变换对齐，$F_i^{\prime }$为聚合特征。

本文的目标是在消息传输前，通过引入额外位姿修正，最小化位姿$\xi$的误差。

IV. 位姿鲁棒的协同3D检测

A. 总体结构

CoAlign的流程如下：
F i , B i = f d e t e c t i o n ( O i ) ξ j → i ′ = f c o r r e c t i o n ( { B j , ξ j } j = 1 N ) M j → i = f t r a n s f o r m ( ξ i , ( F j , ξ j → i ′ ) ) F i ′ = f f u s i o n ( F i , { M j → i } j = 1 N ) B i ′ = f d e c o d e r ( F i ′ ) F_i,B_i=f_{detection}(O_i)\\ \xi'_{j\rightarrow i}=f_{correction}(\{B_j,\xi_j\}_{j=1}^N)\\ M_{j\rightarrow i}=f_{transform}(\xi_i,(F_j,\xi'_{j\rightarrow i}))\\ F'_i=f_{fusion}(F_i,\{M_{j\rightarrow i}\}_{j=1}^N)\\ B'_i=f_{decoder}(F'_i) Fi​,Bi​=fdetection​(Oi​)ξj→i′​=fcorrection​({Bj​,ξj​}j=1N​)Mj→i​=ftransform​(ξi​,(Fj​,ξj→i′​))Fi′​=ffusion​(Fi​,{Mj→i​}j=1N​)Bi′​=fdecoder​(Fi′​)

其中$B_i,B_i^{\prime }$为协同前后的检测输出，${\xi }_{j\to i}^{\prime }$为修正的相对位姿。

B. 带有不确定性估计的单智能体检测

$f_{detection}$可利用现成的检测器，但额外估计不确定性，以避免错误检测对位姿修正带来负面影响。带有不确定性的边界框记为$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位置、3D尺寸、3D朝向角、中心位置方差和角度方差。

本文将边界框中心位置视作服从高斯分布的随机变量，损失函数为高斯分布$\mathcal{N}(\hat{x},{\sigma }_x^2)$与真实delta分布$\mathbb{P}(x)=\delta (x-x_0)$的KL散度：
$$L_x=\frac{(\hat{x}-x_0{)}^2}{2{\sigma }_x^2}+\log \frac{{\sigma }_x^2}{2}$$

其中$x_0$为真值。

因为角度具有周期性，本文视其服从冯·米塞斯分布，均值为$\hat{\theta }$，集中度为$1/{\sigma }_{\theta }^2$。本文最小化该分布与真实delta分布$\mathbb{P}(x)=\delta (\theta -{\theta }_0)$之间的KL散度：
$$L_{\theta }=\log I_0(\exp (-s))-\exp (-s)\Vert \cos (\hat{\theta }-{\theta }_0)\Vert$$

其中${\theta }_0$为真值，$I_0$为0阶修正的贝塞尔函数，$s=\log ({\sigma }_{\theta }^2)$。为了在$s$较大时促进梯度回传，可加一个正则化项$\lambda ELU(s-c)$，其中$ELU$为指数线性单元（ELU），$c$为控制ELU影响的超参数。

单智能体检测与不确定性估计的总损失为
$$L=L_{cls}+{\alpha }_{reg}{L}_{reg}+{\alpha }_{center}(L_x+L_y)+{\alpha }_{\theta }{L}_{\theta }$$

C. 智能体-物体位姿图优化

单智能体检测后，智能体$i$会共享三类消息：自身定位模块估计的位姿${\xi }_i$，自身的检测结果，以及特征图$F_i$。位姿修正只需要前两个消息，且由于边界框只含有偏航角，本文仅修正2D空间中的$(x_i,y_i,{\theta }_i)$。

思路为对齐对应同一物体在不同智能体检测下的边界框，利用智能体-物体位姿图来表达智能体与物体的关系，随后在图上进行优化，以实现位姿对齐。具体来说，智能体$i$从所有其它智能体接收消息，并建立其自己的姿态图$G({\mathcal{V}}^{agent},{\mathcal{V}}_{object},\mathcal{E})$，这是一个智能体与被检测物体的二分图。节点集合${\mathcal{V}}^{agent}$包含所有$N$个智能体，${\mathcal{V}}_{object}$则包含所有物体，通过空间聚类来自所有智能体的、相似的边界框得到。边集$\mathcal{E}$表示智能体与物体的检测关系，当一个智能体检测到某物体时，在两者的节点之间添加边。注意该图是在各智能体局部建立的，但对所有智能体来说是一样的。

姿态图中的每个节点都对应一个位姿。对于智能体$j$，其位姿为其定位模块估计的结果；对于物体$k$，其位姿${\chi }_k$由边界框簇中的多个边界框采样得到，即多个智能体检测到的同一物体。$\mathcal{E}$则反映了智能体与物体的相对位姿。令$z_{jk}=(\hat{x},\hat{y},\hat{\theta })$为物体$k$相对于智能体$j$的相对位姿（来自智能体$j$的检测结果），理想情况下，同一物体的位姿在不同智能体视角下是一致的，即位姿一致性误差$e_{jk}=z_{jk}^{-1}\circ ({\xi }_j^{-1}\circ {\chi }_k)\in {\mathbb{R}}^3$为0，其中$\circ$为运动补偿操作，等价于乘以相应的齐次变换矩阵；${\xi }^{-1}$为逆位姿，等价于齐次变换矩阵的逆。

为了促进位姿一致性，考虑下列优化问题：
{ ξ j ′ , χ k ′ } = arg min ⁡ { ξ j , χ k } E ( E ) = ∑ ( ξ j , χ k ) ∈ E e j k T Ω j k e j k \{\xi'_j,\chi'_k\}=\argmin_{\{\xi_j,\chi_k\}}E(\mathcal E)=\sum_{(\xi_j,\chi_k)\in\mathcal E}e^T_{jk}\Omega_{jk}e_{jk} {ξj′​,χk′​}={ξj​,χk​}argmin​E(E)=(ξj​,χk​)∈E∑​ejkT​Ωjk​ejk​

其中${\Omega }_{jk}=diag([1/{\sigma }_x^2,1/{\sigma }_y^2,1/{\sigma }_{\theta }^2])\in {\mathbb{R}}^{3\times 3}$，类似基于图的SLAM中的信息矩阵，衡量该边界框测量的置信度。

上式的优化与基于图的SLAM类似，通过高斯-牛顿或莱文贝格-马夸特算法求解。迭代中，固定自车位姿，并更新所有其它智能体和物体的位姿。收敛通常很快，因为初始解 { ξ j , χ k } \{\xi_j,\chi_k\} {ξj​,χk​}较好。最后，可求相对位姿${\xi }_{j\to i}^{\prime }={\xi }_i^{\prime -1}\circ {\xi }_j^{\prime }$，并依据其将智能体$j$的特征变换到自车位姿下，得到$M_{j\to i}$。

D. 多尺度特征融合

在相对位姿修正后，特征图之间仍可能存在错位。为了进一步减小位姿噪声影响，本文使用多尺度融合方法，在不同空间尺度上融合特征。精细尺度可提供细节的几何和语义信息，而粗糙尺度对位姿误差更不敏感。

令$F_{j\to i}^{(l)}$为尺度$l$上的协同特征，多尺度融合可表达如下：
F j → i ( 1 ) = M j → i F j → i ( l + 1 ) = g l ( F j → i ( l ) ) F i ( l ) = F u s e ( { F j → i ( l ) } j = 1 N ) F i ′ = C a t ( [ F i ( 1 ) , u 2 ( F i ( 2 ) ) , ⋯   , u L ( F i ( L ) ) ] ) F_{j\rightarrow i}^{(1)}=M_{j\rightarrow i}\\ F_{j\rightarrow i}^{(l+1)}=g_l(F_{j\rightarrow i}^{(l)})\\ F_i^{(l)}=Fuse(\{F_{j\rightarrow i}^{(l)}\}_{j=1}^N)\\ F'_i=Cat([F_i^{(1)},u_2(F_i^{(2)}),\cdots,u_L(F_i^{(L)})]) Fj→i(1)​=Mj→i​Fj→i(l+1)​=gl​(Fj→i(l)​)Fi(l)​=Fuse({Fj→i(l)​}j=1N​)Fi′​=Cat([Fi(1)​,u2​(Fi(2)​),⋯,uL​(Fi(L)​)])

其中$g_l$为带有2倍下采样的残差层，$Fuse$为智能体维度的注意力操作，$u_l$为上采样操作。

本文在训练时为$x,y$与$\theta$添加高斯噪声，以模拟位姿误差。