编者按：最近，4D毫米波雷达越来越受到学术界和工业界的关注，该传感器可以在雨、雪、雾等恶劣环境或任何不利光照条件下鲁棒运行，测量环境的准确空间信息，同时直接获得场景中动态物体的描述。4D雷达的这些特性与相机形成了良好的互补。本文提出了一种基于深度学习的4D雷达视觉里程计方法，名为4DRVO-Net，它基于PWC网络架构，以从粗到精分层细化的方式实现4D雷达-视觉里程计。我们提出了一个适用于4D雷达点云的多尺度特征提取网络Radar-PointNet++，实现对稀疏点云的细粒度学习。采用自适应4D雷达相机融合的方法，以实现4D雷达点特征和图像特征自适应配准和充分交互。提出了速度引导的点置信度估计方法来处理环境中的动态物体和异常点对里程计的不利影响。值得注意的是，VoD数据集和In-house数据集上的实验证明了我们的方法以及所提出的模块的有效性。
《4DRVO-Net: Deep 4D Radar-Visual Odometry Using Multi-Modal and Multi-Scale Adaptive Fusion》
IEEE Transactions on Intelligent Vehicles, 2023.
Guirong Zhuo1, Shouyi Lu1, Lu Xiong∗1, Huanyu Zhou1, Lianqing Zheng1, Mingyu Zhou2
摘要：4D雷达-视觉里程计(4DR-VO)融合了4D雷达和摄像头的互补信息，是一种具有吸引力的解决方案，可实现准确、鲁棒的位姿估计。然而，4D雷达-视觉里程计系统可能会因(1)4D雷达点云的稀疏性(2)4D雷达和相机之间不准确的数据关联和不充分的特征交互和(3)环境中的动态物体对里程计估计造成干扰而遭受重大跟踪误差。在本文中，我们提出了一种名为4DRVO-Net的4D雷达-视觉里程计方法，其基于特征金字塔、位姿扭曲和成本量（PWC） 网络架构以从粗到精的方法估计和细化位姿。具体来说，在充分考虑丰富的4D雷达点信息的前提下，我们提出了一个适用于4D雷达点云的多尺度特征提取网络Radar-PointNet++来实现对稀疏4D雷达点云特征的细粒度学习。之后，为了有效融合两种模态，我们设计了一个自适应4D雷达-相机融合模块，该模块根据4D雷达点特征自动选择图像特征来实现多尺度的跨模态特征交互和自适应的多模态特征融合。此外，我们还设计了一个速度引导的点置信度估计模块来权衡局部运动模式，以减少动态物体和异常值的影响，其在位姿扭曲细化中也不断被更新。我们在VoD数据集和In-house数据集上证明了我们的方法的卓越性能和模型中每个模块设计的有效性。在VoD数据集的大多数序列上，我们的方法优于所有基于学习的方法和基于几何的方法。
关键词：4D雷达，多尺度特征提取，多模态特征融合
里程计估计是自动驾驶的关键技术之一，其可以在GPS失效的时候为智能车辆提供高精度定位。该任务使用连续的图像或点云 来获得两帧之间的相对位姿变换。大多数传统方法通过遵循特征提取和匹配以及运动估计和优化的pipeline来解决这个问题。尽管这些方法已经展现了优异的性能，但缺乏应对挑战性场景的能力。最近，深度学习技术在里程计任务中得到了广泛的应用，基于深度学习的里程计方法在处理稀疏特征和动态环境上展现了令人印象深刻的性能。在基于深度学习的方法中，许多研究使用单模态传感器，构建深度学习模型，实现里程计估计。也有少量研究采用多传感器融合技术，利用每个传感器之间的互补特征，通过数据驱动的方式实现该任务。本文旨在使用多传感器融合的方法来估计里程计。
单目相机是智能车辆上广泛使用的传感器之一，因为它体积小、成本低并且可以获得丰富的语义信息。但深度视觉里程计需要相对稳定的光照变化，通常是静态场景以及足够的纹理来提取特征进而估计相机运动。此外，若只使用单目图像，无法直接获得绝对尺度。最近，4D毫米波雷达越来越受到学术界和工业界的关注，因为与相机相比其有着独特的优势。首先，4D雷达可以直接获得场景中动态物体的描述，其可以削弱动态物体对里程计的不利影响。其次，4D雷达可以在雨、雪、雾等恶劣环境或任何不利光照条件下鲁棒运行。最后，4D雷达可以测量环境的准确空间信息，因此里程计估计具有绝对尺度。但4D雷达点云的稀疏性和嘈杂性是其在里程计任务中的主要挑战。
因此，4D雷达和单目相机具有很强的互补性。首先，相比于4D雷达点云，图像具有更丰富的语义信息和纹理信息。通过有效的4D雷达-视觉交互方法，可以赋予4D雷达点多个与其相关的语义特征，这样4D雷达点在融合其对应语义特征外还融合了其他空间位置的语义特征，这样使稀疏4D雷达点云中的每个点具有更丰富和更独特的特征表征的同时还潜在的解决了4D雷达点云稀疏性问题。其次，4D雷达点云对环境精准的测量可以解决单目视觉里程计的尺度不确定性问题。此外，4D雷达和相机具有低成本和小尺寸的优势，使其很容易在预算和装配空间有限的智能车辆上推广应用。因此，4D雷达视觉里程计的研究对于学术研究和工业应用具有重要价值。
对于有效的融合单目相机和4D雷达信息进行里程计估计，存在三个挑战：1）与激光雷达输出的点云相比，4D雷达采集的点云更稀疏、更嘈杂且分辨率更低。这种稀疏性和嘈杂性大大增加了鲁棒提取点云特征的难度；2）4D雷达点特征和图像像素特征进行精准关联和充分融合具有挑战性；3）4D雷达点云或图像中属于动态物体的点或像素不适合用于位姿估计。因为这些点或像素具有动态物体运动的不确定性。因为这些点或像素违反了静态环境假设。
为了应对以上挑战并充分利用丰富的4D雷达点云信息和图像的语义信息，在这项工作中，我们基于特征金字塔、位姿扭曲和成本量网络架构提出了一种基于深度学习的4D雷达视觉里程计方法，名为4DRVO-Net。首先，为充分利用4D雷达丰富的点云信息，基于PointNet++，提出了一个适用于4D雷达点云的多尺度特征提取网络Radar-PointNet++。该网络在不同尺度上通过对4D雷达点云的不同点云信息分开编码和多尺度融合的方式，有效的提取点云特征。其次，为了充分融合4D雷达和视觉信息，我们首先构建了图像金字塔和点云金字塔网络来提取图像和点云的多尺度特征。然后，我们基于deformable attention设计了一个4D雷达-相机自适应融合模块，来实现多尺度的跨模态特征交互和自适应的多模态特征融合。最后，我们基于4D雷达点的速度信息设计了一个速度引导的点置信度估计模块来减轻噪声和动态物体的干扰。我们工作的主要贡献总结如下：
基于特征金字塔、位姿扭曲和成本量网络架构，我们为4D雷达-视觉里程计任务构建了一个完全端到端的高效框架，名为4DRVO-Net。4DRVO-Net采用由粗到精的分层优化方式，实现对帧间位姿的迭代细化和精确估计。
在所提出的框架中，提出了一个适用于4D雷达点云的多尺度特征提取网络Radar-PointNet++，实现对稀疏点云的细粒度学习。并基于deformable attention设计了一个4D雷达-相机自适应融合模块，来实现多尺度的跨模态特征交互和自适应的多模态特征融合。
基于4D雷达点的速度信息，提出了速度引导的点置信度估计方法对点不确定性进行建模，来降低环境中的动态物体和噪声对位姿估计的不利影响。
所提出的方法在VoD数据集和In-house数据集上得到验证。大量实验证明了4DRVO-Net及其各种组件的有效性和稳健性。
图1 我们提出的4DRVO-Net通过自适应4D雷达相机融合模块融合4D雷达和相机信息。通过迭代位姿扭曲细化，逐层细化位姿、点置信度和嵌入特征。
A基于深度学习的视觉里程计
基于深度学习的视觉里程计方法在算法鲁棒性、信息理解和存储以及认知模式上较传统的视觉里程计方法有着巨大的优势[1]，因此受到研究人员的广泛关注。[2]是最早使用深度神经网络进行里程计估计的工作，其通过预测每张图像的速度和方向的方式，初步探索了深度学习在该领域中的应用。PoseNet设计了一个基于CNN的网络架构，用于从输入图像中提取特征，并预测相机的位置和姿态。GoogLeNet将PoseNet用于分类的线性回归和Softmax层更改为输出七维位姿向量的全连接层并将重投影误差作为网络训练损失来增加对位姿的有效监督。DeepVO使用深度循环神经网络来建模序列间的动态和关系进而估计自我运动。基于DeepVO，ESP-VO在统一的框架中推断位姿和不确定性。TartanVO设计了余弦相似度损失和归一化距离损失来提高VO模型的泛化性，并将相机内参合并到模型中。Xue等人设计了一个自适应记忆模块，其以神经模拟记忆的方式逐步自适应地保存从局部到全局的信息，使VO模型能够处理长期依赖性。为削弱环境中的动态物体对视觉里程计的有害影响，Liu等人通过测量关联图像中几何对应区域的相对相似度来生成输入图像像素的置信度值。DPVO使用基于稀疏补丁的匹配来代替密集流，获得了最佳的准确性和效率。Li等人提出了一种基于跨模态知识蒸馏的视觉里程计方法，其利用训练有素的视觉激光雷达里程计作为教师来指导VO网络的训练。CCVO基于两个级联CNN实现端到端位姿估计。第一个CNN检测可追踪的特征点并进行语义分割来剔除属于动态物体的特征点。第二个CNN将两个连续图像的静态特征点作为输入来预测两帧之间的位姿变换。
B基于深度学习的点云里程计
相比于传统毫米波雷达，4D雷达的输出不再是单一的点目标，而是更接近激光雷达的点云。基于激光雷达点云的深度里程计方法已经得到了充分研究[]。Nicolai等人首次将深度学习技术应用到点云里程计，其将3D点云投影到2D平面，然后使用基于图像的深度学习方法来实现深度点云里程计。DeepPCO采用全景深度视图来表示点云并设计了一个双分支架构来分别估计平移和旋转。LO-Net通过学习法线和动态区域掩码来提高里程计位姿估计的准确性。LodoNet将3D点云转移到图像空间后使用基于图像的特征提取和匹配的方法来进行里程计估计。DMLO将特征匹配方法适用于3D点云里程计任务，其将6-DoF姿态估计分解为基于学习的匹配网络和基于奇异值分解的位姿估计两部分。Xu等人开发了一个两阶段里程计估计网络，通过估计一组子区域变换并使用运动投票机制对它们进行平均来获得自我运动，并设计了一个3D点协方差估计模块，通过估计点3D协方差的方式来降低运动物体对里程计的干扰。PWCLO-Net为3D点云里程计任务构建了金字塔、扭曲和成本体积(PWC)结构，以从粗到精的方法分层细化估计的位姿，并提出了一个可训练嵌入掩码来权衡所有点的局部运动模式。EfficientLO-Net提出了一种3D点云的投影感知表示，将原始3D点云组织成有序数据形式，以提高点云里程计的效率。NeRF-LOAM将神经辐射场引入3D点云里程计系统，在应用于不同环境时表现出强大的泛化能力。STCLoc将绝对位姿回归和一种新颖的分类任务结合起来，该分类任务根据位置和方向对点云进行分类来规范对位姿的回归。基于4D雷达点云的深度里程计方法尚未得到充分的研究，只有少数工作将4D雷达点云里程计当作中间任务进行了估计。RaFlow是基于4D雷达点云进行场景流估计的工作，里程计作为中间任务进行了输出。CMFlow是一种通过跨模态学习进行基于4D雷达的场景流、运动分割和里程计估计的方法，其中使用了GPS/INS对里程计任务进行了有效的监督。
C视觉和点云融合的里程计系统
视觉和点云融合的里程计系统，在以前的研究中是指视觉-激光雷达里程计，该方法利用了两种传感器的模式，并实现了功能上的互补。DEMO[28]使用了LiDAR点云来赋予RGB图像深度信息。V-LOAM[29]使用高频的视觉里程计估计的位姿作为低频激光雷达里程计的运动先验来细化估计的运动。[30]通过将点和线特征引入位姿估计中来利用更多的环境结构信息，并使用LiDAR点云赋予点和线深度信息。LIMO[31]使用了LiDAR提供的深度信息来解决单目视觉里程计的尺度不确定性。SDV-LOAM结合了半直接视觉里程计和自适应sweep-to-map LiDAR里程计来解决视觉-LiADR里程计中3D-2D深度关联等问题。最近，有几种基于深度学习的视觉-激光雷达里程计方法。Yue等人提出了使用相机的高分辨率图像来丰富低成本16通道LiDAR的原始3D点云深度学习方法。最后，丰富的点云用于基于正态分布变换(NDT)执行LiDAR SLAM。H-LVO是一种混合的视觉-LiDAR里程计框架，其使用深度图、深度流图和深度LiDAR深度补全网络来完成2D特征匹配和3D关联，并使用基于几何的方法进行位姿求解。Self-VLO是一种自监督的视觉-LiDAR里程计框架，其将单目图像和3DLiDAR点投影的稀疏深度图作为输入，并设计了一个双路编码器来从视觉和深度图像中提取特征。MVL-SLAM基于RCNN网络架构，将RGB图像和从3D LiDAR点云生成的多通道深度图像作为输入，输出具有绝对比例的6D位姿。
然而，与激光雷达点云相比，4D雷达点云的噪声更大、更稀疏，提取可靠的边缘和平面特征具有挑战性。此外，4D雷达点云除了空间信息外还包含速度信息和RCS信息，激光雷达点云特征提取方法难以充分使用丰富的4D雷达点云信息。因此，直接使用以上视觉-激光里程计方法难以取得很好的效果。我们的方法针对4D雷达的点云特性，专门设计了一个Deep 4D Radar-Visual Odometry方法。
图2 提出的4DRVO-Net架构的细节。4DRVO-Net包括三个主要组件：(a)特征提取和融合模块由多尺度特征提取块和自适应4D雷达-相机融合块组成，分别用于在四个尺度上提取RGB图像特征和4D Radar点特征以及在每个尺度上自适应地融合在一起，(b)初始估计模块用于关联带有融合特征的两帧4D Radar点云并对点置信度和位姿进行初始估计，以及(c)迭代细化模块用于由粗到细地细化点置信度、嵌入特征和位姿。
在这项工作中，我们提出了4DRVO-Net，其以端到端的方式学习4D雷达-视觉里程计。图2说明了我们的4DRVO-Net的整体结构。网络的输入是两对同步的RGB图像-4D Radar点云对：、 和、 。两个RGB图像和4D Radar点云首先由图像特征金字塔和点特征金字塔编码，特征金字塔网络由四组conv层组成。每个set conv层对输入图像和点云进行下采样操作并提取下采样图像和点的局部特征。同一级别的set conv层共享相同的权重。然后，在每一尺度上，所提出的自适应4D Radar相机融合模块(A-RCFM)关联同步的4D Radar和相机帧以增强缺乏辨别细节的极其稀疏的4D Radar特征，以上内容将在3.1节介绍。接下来，使用成本量网络关联带有融合特征的4D Radar帧来生成嵌入特征，并进行初始点置信度估计来对嵌入特征进行加权后回归初始位姿,在3.3节中，我们提出了迭代细化模块，以由粗到精的方法细化嵌入特征、点置信度和位姿。最后，网络输出连续雷达帧之间的欧拉角和平移向量 。
3.1 多尺度特征提取和自适应特征融合
3.1.1 多尺度特征提取
多尺度特征提取由两个对称分支组成，名为图像分支和点分支，分别用于2D和5D数据。对于每一分支都构建了一个由多个set conv组成的孪生金字塔，用于编码和提取多尺度特征。对于图像分支，我们使用由Residual Block组成的图像金字塔网络来提取RGB图像的特征。在不同尺度上提取的RGB特征可以按照图2中标记的正向方向分别用 、、 和表示。
4D Radar点云通常是稀疏、嘈杂且分布不均匀的。虽然扩大感受野可以解决点云的稀疏问题，但在单尺度上提取特征仍无法解决点云分布不均匀的问题。受到PointNet++的“分层点集特征学习”部分的启发，我们提出Radar PointNet++(RPNet++)来构建4D Radar点云金字塔网络对4D Radar点云进行多尺度特征编码，如图3所示。在每个尺度上，输入是其中每个点具有5D点信息和在上一尺度上获得的。
图3 Radar PointNet++ structure. 首先使用最远点采样(FPS)和K Nearest Neighbors(KNN)对输入点云采样和分组。之后分别对每组中雷达点的空间信息、速度信息和强度信息进行编码，并在三个特征尺度上进行深度融合来得到聚合的点特征。
特征，输出是，其中和表示5D点信息和提取的局部特征。输出的所有采样点都是从输入点云中使用最远点采样(FPS)获得的。
对于每一个采样点，K Nearest Neighbors (KNN)被用来从输入点云中选择个邻居点，其中每一个邻居点有5D信息。接下来，新设计的特征聚合网络对个点进行特征提取。4D雷达点的空间信息、速度信息和强度信息在不同性质上表征了4D雷达点对环境的描述，我们认为若将4D雷达点所有信息映射到同一特征空间将造成特征混淆。因此，对于个邻居点，其空间、速度和强度信息分别用三个不共享权重的线性层、BN和ReLU学习，并在三个特征尺度上进行深度特征融合。最后，通过平均池化操作来得到聚合的点特征。我们将和连接起来，并经过一个线性层、BN和ReLU层后得到最终的局部特征。公式是：
此处，LBR是Linear，Batchnorm，ReLU层，Aggregation是聚合特征网络块。在本文中，我们从原始4D Radar点云中预采样个点，之后在4D雷达预采样点集中分层提取、、、个点及其对应的局部特征。按照图2标记的正方向，4D Radar点的坐标集在不同的尺度上分别由 、、和表示，同时在不同尺度上提取的相对应的局部特征分别由 、、和表示。
3.1.2 自适应特征融合
图4 自适应4D雷达相机融合模块。我们首先将4D雷达点投影到图像平面上。之后通过可学习的动态偏移在图像特征中以加权的方式聚合语义特征。最后，我们通过交叉注意模块融合聚合的语义特征和点特征，产生自适应融合特征。
将4D雷达点特征和图像特征进行精准关联和充分融合具有挑战性。在本节中，我们提出了一种新颖的融合RGB图像和4D雷达点云特征的方法。在不同的特征尺度上，使用4D雷达-相机的外参矩阵和相机内参矩阵 ，我们可以将在雷达坐标系下坐标为且局部特征为的4D雷达采样点投影到RGB图像的特征图上来获得坐标为的投影点和相应的语义特征。投影过程如下所示：
式中，表示4D雷达点的齐次坐标，在将和相乘后，我们取前三个坐标，使其可以与相乘。如果坐标不是整数，我们使用双线性插值进行检索。
需要注意的是，由于外参标定误差或车辆抖动使外参发生微小变化的原因，投影点和4D雷达点并不是准确的对应关系。此外，我们认为稀疏的4D雷达点应该在稠密的图像特征图上自适应的寻找多个感兴趣的语义特征进行融合来达到点特征和和图像特征充分交互的效果。为了解决以上问题，我们开发了基于deformable attention的空间交叉注意力，其中点特征查询自适应的在图像特征图上寻找感兴趣区域进行交互。如图4所示，我们首先将投影到。之后，对使用两个线性变换来得到采样点的动态偏移量和相应的权重。基于投影点将学习到的偏移量应用于图像特征可以得到与4D雷达点关联密切的一组图像特征。对加权来生成聚合图像特征。将投影到键和值。以上、和是可学习的线性映射。整个自适应4D雷达-相机融合过程表述为：
此处，MultiheadAttn()代表多头注意力块。最后，我们连接自适应融合特征和原始点特征以获得融合特征。
3.2 点云关联和速度引导的Point Confidence 估计
3.2.1 点云关联
接下来，使用[35]提出的Cost Volume层来关联两个点云。该成本量层以patch-to-patch的方式关联两帧点云，这有效缓解了4D雷达点云的稀疏性问题。成本量通过在自适应特征融合之后关联两个带有融合特征的点云来生成点嵌入特征,其描述了两个点云之间的点相关信息。
3.2.2 速度引导的Point Confidence 估计
使用嵌入特征E来生成两帧之间的全局一致位姿变换是一个新问题。在本节中，我们提出了一种新颖的速度引导的点置信度估计模块以从嵌入特征生成位姿变换。
4D雷达点云中属于动态物体的点、噪声点以及在其他帧被遮挡的点都对里程计估计产生不利的影响。需要过滤这些点，保留对里程计估计任务有价值的点。相比于其他传感器，4D雷达点独有的速度信息可以直接反应环境中动态物体，利用速度信息可以直接削弱点云中的动态点对里程计估计的不利影响。因此，我们提出了一种速度引导的点置信度估计模块，其将点云的速度信息和点特征输入到MLP层，然后进行Sigmoid操作以获得每个点的置信度估计。其中，在点的速度信息中，我们使用了点云中每个点的RRV的绝对值和每个点RRV的绝对值与点云中所有点的RRV的绝对值的中位数的差值。点特征的引入不仅增强了点置信度估计模块对动态点的敏感度还评估了其他因素（比如：点的空间位置）对里程计估计的影响。置信度估计的过程如下所示：
通过上述过程，我们对点云中的每个点赋予了一个0到1的权重，该权重代表点的置信度，若点的权重越小，则说明该点对里程计估计的贡献度越小，反之亦然。然后，使用估计的点置信度对点云的embedding feature 进行加权，之后通过两个独立的MLP层来回归旋转估计和平移估计。
式中，表示平均池化操作。
置信度估计在后续的pose-warp refinement模块中也不断被细化，如图2所示，置信度估计传播到点云的更密集层，并基于稀疏层的估计结果以coarse-to-fine的方式进行优化，使得最终的置信度估计和位姿变换的计算结果更加准确。我们称此过程为速度引导的hierarchical Point Confidence Optimization。
3.3 位姿扭曲细化模块
图5 在第层的位姿扭曲细化模块的细节。将在层的嵌入特征、置信度估计和位姿估计作为模块的输入，输出位姿扭曲细化后的第层的嵌入特征、置信度估计和位姿估计。
为了实现点云位姿的迭代细化，得到更精确的位姿估计，我们提出了位姿扭曲细化模块，如图5所示。该模块包含几个关键部分：KNN interpolation layer, pose warping，embedding feature、点置信度以及位姿细化。
3.3.1 KNN 插值层
为了以coarse-to-fine的方式细化embedding feature和点置信度，我们使用KNN interpolation layer将以上估计从稀疏层传播到密集层作为细化操作的初始值。该层的输入是层的嵌入特征和置信度估计，输出层的KNN插值的嵌入特征和KNN插值的置信度估计。具体来说，在该层中，层中的每个密集点都会从层的稀疏点中选择其KNN，并将密集点与被选择的稀疏点之间的距离作为权重，利用加权求和的方式将被选择的稀疏点的嵌入特征和置信度估计聚合起来，得到粗嵌入特征和粗置信度估计。
3.3.2 位姿扭曲层
是实现点云位姿迭代细化的重要操作，其将第层的欧拉角和平移向量应用于来生成。位姿扭曲的方程如下：
此处，是相对应的旋转矩阵。若和足够准确，和将无限接近，然而由于估计误差，只是比更接近。因此，位姿细化的过程即是估计和在第层上的残余运动，进而对和进行矫正来得到更加精确的位姿估计。
3.3.3 嵌入特征与点置信度精化
将和以及相关联的融合特征和输入到3.2.1节描述的cost volume层，第层的粗嵌入特征可以得到。之后，将生成的KNN插值的嵌入特征和粗嵌入特征连接起来并输入到MLP中以获得层的优化嵌入特征：
为了更新点置信度估计，与嵌入特征的细化一样，将第层的局部特征、点速度信息以及KNN插值的置信度估计连接起来并进行MLP和sigmoid操作后来获得层的优化点置信度估计：
3.3.4 姿态优化
与3.2节中的操作相同，使用估计的点置信度对embedding feature加权后。使用两个MLP层可分别得到和之间的残余变换和。使用和对和进行变换可得到在 尺度的细化位姿和：
3.4 训练损失
网络从四个尺度对连续帧之间的位姿变换进行了估计，对于每一尺度上的位姿估计，我们设计了旋转损失和平移损失 分别学习位姿变换的旋转分量和平移分量：
式中， 表示范数，和分别是由真实姿态变换矩阵生成的真实欧拉角和平移向量。
和 为网络在不同尺度下估计的欧拉角和平移向量。由于欧拉角和平移向量之间的尺度和单位不同，参考之前的深度里程计工作[27]，我们引入了两个可学习参数 和。对于每一尺度下的位姿训练损失函数为：
因此，在多尺度监督下，网络总的损失函数为：
式中，为网络的尺度数，表示尺度下的权重。
4.1 数据集
4.1.1 VoD数据集
The View-of-Delft (VoD) automotive dataset [36]是在2022年发布的一个公开数据集。该数据集在代尔夫特市的校园、郊区和老城区等场景中采集了包括相机、4D雷达和LiDAR等传感器的数据，以及提供了车辆的里程计信息和传感器之间的外参变换。数据集一共包括8682帧，根据帧间的连续性，我们将其分为24个序列，并根据[CMFLOW]中划分的训练集、验证集和测试集对网络进行训练、验证和测试。
4.1.2 In-house 数据集
我们使用装配有一个4D雷达、一个广角相机和一个高精度多源信息组合定位设备的大众ID4作为数据采集平台。4D雷达和相机以硬件触发的方式进行时间同步，以10Hz的频率对外输出数据，高精度多源信息组合定位设备输出数据帧率为100Hz。我们共采集了7个序列共20500帧数据。我们按训练集：验证集：测试集为6：2：2的比率选择05和06序列共4100帧数据作为测试集，选择04序列共4300帧数据作为验证集，其余数据作为训练集。
(a)序列05的数据采集轨迹 (b) 序列06的数据采集轨迹
图6 In-house数据集测试序列的数据采集路线
4.2 Baselines
相比于传统的3D雷达，4D雷达点的空间表达由二维变为三维，更接近于LiDAR。因此，我们的对比方法来自于经典的基于3D点的里程计方法和基于图像的里程计方法。其中包括四个基于经典的方法：ICP-point2point(ICP-po2po)[37]、ICP-point2plane(ICP-po2pl)[38]、GICP[39]以及NDT[40]；两个基于LiDAR的方法：A-LOAM w/o mapping[24]和LO-Net[27]；两个基于图像的方法：DeepVO[8]和TartanVO[10]。此外。我们还对比了两个基于4D雷达的里程计方法：RaFlow和CMFlow。RaFlow是基于4D雷达进行场景流估计的工作，里程计作为中间任务进行了输出。CMFlow是一种通过跨模态学习进行基于4D雷达的场景流、运动分割和里程计估计的方法，其中使用了GPS/INS对里程计任务进行了有效的监督。
4.3 评估指标
使用相对姿态误差（RPE）来量化估计姿态和地面实况姿态之间的差异，从而评估了所提出方法的性能。RPE通过测量估计姿态和地面真实姿态在一定间隔或距离上的姿态变化之间的差异来评估算法的准确性，其中考虑了旋转和平移误差。此外，在本研究中，我们使用了所有可能的子序列的平均平移均方根误差（RMSE）（m/m）和平均旋转均方根误差。
4.4 训练细节
由于VoD数据集和In-house数据集中使用的4D雷达设备不同，因此采集回的单帧4D雷达点数量差异很大，对于VoD数据集我们将输入4D雷达点云下采样到，对于In-house数据集我们将输入4D雷达点云下采样到。所有训练和评估实验均在带有Pytorch 1.8.0的单个NVIDIA RTX 3090 GPU上进行。采用Adam优化器来训练实验中的所有模型。学习率最初设置为0.001，每10个epoch以指数方式衰减0.1。Batchsize为8。3.4节中的可学习参数 和 的初始值分别设置为-2.5和0。网络损失总函数中，，，。
在本节中，演示了提出的4D雷达视觉里程计网络性能的定量和定性结果，以及为验证模型中每个模块有效性所进行的消融实验的结果。最后，对点置信度估计结果进行可视化和讨论。
5.1 性能评估
5.1.1 VoD 数据集测试效果
表1展示了提出的方法的性能在VoD数据集的测试集上与基线之间的定量比较结果。事实证明，我们的方法在VoD数据集上的相对平移误差和相对旋转误差指标都远远优于其他方法。具体来说，我们的方法在平均相对平移误差上比第二好的方法高27.5%，平均相对旋转误差比第二好的方法高69.3%。虽然基于LiDAR的里程计方法在密集的LiDAR点云上取得了卓越的性能，但由于4D雷达点云的稀疏性和嘈杂性，其在应用到4D雷达点云时并没有取得很好的定位效果。此外，由于稀疏的4D雷达点云，基于A-LOAM的方法在03序列上并不能完整的运行。VoD数据集的场景中包含大量动态物体，这对基于相机的里程计方法造成了严重的不利影响，因此，其在VoD数据集上运行并不鲁棒。提出的方法不仅充分发挥出4D雷达面对动态场景时的优势还结合了图像丰富的语义信息，并且可训练迭代细化使估计位姿在一个网络推理中多次细化。因此，在面对复杂场景时4D雷达视觉里程计取得了优异的性能。
此外，我们还对比了提出的4D雷达-视觉里程计方法与64线激光雷达运行不带有后端优化的A-LOAM的结果。如表2所示，使用低成本的4D雷达和单目相机的4DRVO-Net达到了和64线激光雷达里程计相当的定位效果。这取决于对提出的方法中每个模块的有效设计。
表1 VoD数据集上的4D雷达视觉里程计实验结果。最好的结果用红色加粗，次好的结果用蓝色加粗。
表2在vod数据集上的四维雷达-视觉测程和64波束激光测程实验结果。最好的结果用红色表示。
定性结果如图7和图8所示。图7展示了在VoD数据集03、04、09、17、22和24序列上提出的算法、ICP-po2po、GICP、TartanVO以及CMFlow的轨迹在XY平面上的投影结果。因为4D雷达点云在空间上分布不均匀且嘈杂，所以基于传统的里程计方法生成的轨迹与真值轨迹差异十分大。此外，基于相机的里程计方法无法直接获得绝对尺度，因此生成的轨迹和真值轨迹尺度不一致。4D雷达可以测量环境的准确空间信息，因此里程计估计具有绝对尺度且生成的轨迹几乎与真值轨迹重合。图7绘制了在所有测试序列上的平均段误差，从中可以看出，相比于其他方法，提出的方法的性能具有较大的优势，尤其是在旋转估计方面。
图7 在VoD数据集03、04、09、17、22和24序列上，所提出算法、ICP-po2po、GICP、TartanVO以及CMFlow的轨迹在XY平面上的投影结果。无论是算法的鲁棒性还是定位精度，我们的比其他算法好得多。
图8 对于路径长度为20的所有可能子序列，VoD测试序列的平均平移和旋转误差;20,40,…;160米。我们的方法性能最好。
5.1.2 In-house 数据集测试效果
在In-house数据集上的评估结果列于表3。可以看到提出的4D雷达视觉里程计方法在两个测试序列上均达到了最优的定位性能，尤其是在相对旋转误差上表现尤为优秀。具体来说，我们的方法在平均相对平移误差上比第二好的方法高35%，平均相对旋转误差比第二好的方法高76.2%。此外，我们还发现在In-house数据集上，基于传统方法的里程计方法（如NDT和GICP）的定位性能比在VoD数据集上的定位性能有了显著的提升。其背后的原因可能是我们数据集中使用的4D雷达设备一帧点云中4D雷达点个数是VoD数据集中使用的4D雷达设备一帧点云中4D雷达点个数的二倍。因此，基于点云配准的里程计方法的性能有所提高。
表3 In-house数据集上的4D雷达视觉里程计实验结果。最好的结果用红色加粗，次好的结果用蓝色加粗。
定性结果如图9和图10所示。图9展示了提出的方法和对比方法在测试序列05上的2D和3D轨迹。从图中可以看出，相比于其他算法，本文提出的方法在长距离的数据集上仍然展示出了令人印象深刻的定位性能，并且在X轴、Y轴和Z轴上都有着很低的轨迹漂移。图10展示了在两个测试序列长度20、40、...、160m的所有可能子序列上平均平移和旋转误差。从图中可以看出，在所有子序列上，提出的方法都取得了最佳的定位性能。
(a)序列05的二维轨迹图 (b) 序列05的三维轨迹图
图9 在In-house数据集测试序列05上，所提出算法、GICP、NDT、LOAM、TartanVO以及RaFlow的2D和3D轨迹。我们的方法性能最好
图10 对于路径长度为20的所有可能子序列，In-house测试序列的平均平移和旋转误差;20,40,…;160米。我们的方法性能最好.
5.2 消融实验
为了分析模型中提出的每个组件的有效性，我们删除或更改模型的组件，以在VoD数据集上进行消融研究。训练/测试细节与第4.4节中描述的相同。
Radar-PointNet++模块: 我们将Radar-PointNet++模块中的特征聚合方法修改为4D雷达点的空间信息、速度信息和强度信息输入到同一MLP中进行特征提取，这意味着4D雷达点的所有特征信息被映射到同一特征空间。结果，模型的定位性能下降很多，如表2(a)所示，这是因为提出的特征聚合方法对不同的特征信息进行了多尺度特征提取并在不同特征信息之间进行了深度融合，保证了对4D雷达点特征信息的充分使用。因此我们提出的特征聚合方法获得了最好的效果。
点置信度估计模块: 我们首先删除点置信度估计模块，这意味着场景中的动态物体和静态物体对里程计估计具有相同的贡献度。表2(b)中的结果表明，所提出的点置信度估计模块有助于获得更好的结果，这在动态物体较多的场景，如03和19序列，表现的更加明显。
自适应特征融合模块: 用其它融合方法替换论文中提出的自适应4D雷达相机融合模块。我们首先将4D雷达点投影在图像上索引得到的图像特征与4D雷达点特征连接作为融合特征，这意味着4D雷达点特征和图像特征没有做任何交互。然后我们使用Transformer Attention结构对4D雷达点特征和图像特征进行了简单的交互，这意味着4D雷达点没有自适应的去寻找感兴趣的融合目标。与我们提出的自适应融合方法相比，这两种融合方法的结果都会下降，如表2(c)所示，这是因为提出的融合方法可以自适应地寻找多个感兴趣的图像特征来达到4D点特征和图像特征深度融合的效果，因此我们提出的自适应4D雷达相机融合方法获得了最好的效果。
位姿扭曲优化的效果: 我们分别对比了没有位姿扭曲细化、有一层位姿扭曲细化、有两层位姿扭曲细化的网络的位姿估计结果。如表2(d)所示，从表中可以看出，随着位姿扭曲细化层数的增加，网络估计的位姿也更准确，在具有全部位姿扭曲细化层的完整网络结构上取得了最好的定位效果，定性评价如图8所示。这证明了从粗到细细化操作的重要性。
点置信度和嵌入特征细化的影响: 我们删除了点置信度和嵌入特征的优化，这意味着点置信度和嵌入特征在每个级别都是独立估计的。表2(e)中的结果表明，所提出的点置信度和嵌入特征分层优化有助于获得更好的结果。
采样点数量的影响: 在In-house dataset上我们对比了不同采样点数量对模型性能的影响。我们分别将输入雷达点采样到和个点。如表3所示，从表中可以看出随着采样点数量的增加，模型的定位性能不断增加。此外，即使采样点数量变少使模型性能变差，但定位效果仍是十分优异的，这证明了提出的模型对不同雷达点数量的鲁棒性。
(a)序列22的二维轨迹图 (b) 序列24的三维轨迹图
图11 在VoD数据集22和24序列上，有着完整pose warp-refinement的网络结构、有两层pose warp-refinement的网络结构、有一层pose warp-refinement的网络结构以及没有pose warp-refinement的网络结构生成的轨迹在XY平面上的投影结果。随着pose warp-refinement层数的增加，网络估计的位姿也更加准确。
表4 VoD数据集上的4D雷达视觉里程计网络结构的消融研究结果。
5.3 运行时间
4D雷达点云和图像随着时间的推移一一捕获，及时处理这些数据对算法的实时性至关重要。在VoD数据集中，4D雷达和相机以10Hz的频率进行了对齐，即每隔0.1秒获取数据。因此，这里的实时性是指每次获取数据的处理时间小于0.1秒。表3展示了在VoD数据集04序列上，提出的算法和对比算法的平均运行时间。因为每一帧中4D雷达点的数量十分少，所以基于传统的方运行时间十分短。我们的方法总共耗时39ms，约为26Hz。因此，本文所提出的方法可以实现26Hz的实时4D雷达-视觉里程计。
表5 Average runtime on the VoD dataset Seq.04
5.4点置信度可视化
我们可视化了最后一个位姿细化层中的点置信度估计结果，以显示每个点对位姿估计的贡献度。如图8所示，从静态物体（如建筑物和钢丝围墙）上采样的点比从动态物体（如移动的汽车）上采样的点具有更高的置信度。此外，随着距离的增加，点的权重不断降低，因为远处的点多为不稳定的杂点，近处的点则更加可靠。因此，点置信度估计可以有效降低动态物体和异常点对位姿变换的影响。
图12 点置信度的可视化。我们展示了4D雷达点云和相应的图像，并用不同颜色表示点的置信度大小。在以上两个示例中，建筑物以及钢丝围墙的权重较高，而行驶的汽车的权重较低。此外，点的置信度还会随着距离的增加而逐渐降低，因为我们更信任近处稳定的点。
据我们所知，我们的方法是第一个4D雷达-视觉里程计方法。它基于PWC网络架构，以从粗到精分层细化的方式实现4D雷达-视觉里程计。在我们的架构中，提出了一个适用于4D雷达点云的多尺度特征提取网络Radar-PointNet++，实现对稀疏点云的细粒度学习。提出了自适应4D雷达相机融合方法，以实现4D雷达点特征和图像特征自适应配准和充分交互。提出了速度引导的点置信度估计方法来处理环境中的动态物体和异常点对里程计的不利影响。值得注意的是，VoD数据集和In-house数据集上的实验证明了我们的方法以及所提出的模块的有效性。
在未来工作中，点云历史信息将减少里程计方法处理长序列时的累计误差。因此，我们计划将历史信息嵌入到我们的架构中，使系统能够以端到端的方式保留长期依赖关系。我们还将后端优化技术融入进我们的方法中来实现更精确的定位效果。具体来说，使用4D雷达、视觉和IMU三个低成本的传感器，前端使用基于深度学习的方法来获得各模态里程计结果，后端通过因子图的方式实现各传感器目标级的融合。此外，点云关联算法对里程计精度至关重要，我们将在当前all-to-all cost volume方法的基础上，从计算效率和充分利用4D雷达点云信息的角度出发，对适用于4D雷达的all-to-all cost volume方法展开研究。
参考文献
| 联系人：唐老师 |
| 电话：13917148827 |
| 邮箱：tangyanqin@tongji.edu.cn |
点“阅读原文”获取论文
