定位是自主机器人至关重要的能力与关键使能技

CMRNet++：在 LiDAR 地图中与地图、相机均无关的单目视觉定位

摘要

定位是自主机器人至关重要的能力与关键使能技术。尽管深度学习在许多计算机视觉任务上取得了巨大进展，但在“度量级（metric）视觉定位”能力提升方面仍未产生显著影响。主要障碍之一，是现有基于 CNN 的位姿回归方法难以泛化到先前未见过的地点。

我们此前提出的 CMRNet 通过在 LiDAR 地图中实现与地图无关（map independent）的单目定位有效解决了这一限制。本文在此基础上进一步提出 CMRNet++：一个显著更鲁棒的模型，不仅能有效泛化到新地点，而且还不依赖相机参数（camera parameters）。

我们通过将深度学习与几何技术结合，并把度量推理（metric reasoning）移出学习过程来实现该能力。这样网络的权重就不再与某一特定相机绑定。我们在三个具有挑战性的自动驾驶数据集（KITTI、Argoverse、Lyft5）上进行大量评估，结果表明 CMRNet++ 相比 CMRNet 以及其他基线方法显著更好。更重要的是，我们首次展示：一个深度学习方法可以在完全新的环境中、且无需任何重训练或微调、并且不依赖相机参数地实现准确定位。

I. 引言

自动移动机器人（如无人车）需要精确定位以确保安全导航。虽然 GNSS 能提供全球定位，但其精度与可靠性不足以满足机器人导航需求。例如在城市环境中，建筑物会遮挡或反射卫星信号，导致非视距（NLOS）和多径效应问题。

为缓解该问题，通常使用机器人自身传感器来提升定位精度与鲁棒性。已有大量方法利用不同车载传感器实现定位。LiDAR 方法通常能达到足够精确的定位，但因为成本高而阻碍普及。相较之下，相机方案更有潜力广泛部署，因为成本显著更低。

虽然传统上相机方案性能不如 LiDAR，但近年来计算机视觉与机器学习进展显著缩小了差距。一些方法使用 CNN 或随机森林直接从单张图像回归相机位姿。它们在室内环境表现突出，但在大规模室外环境中明显受限；并且只能在模型训练过的地点使用。

过去十年，地图提供商开始构建面向汽车的下一代 HD 地图，这类地图包含道路场景的精确几何重建（点云形式），通常由 LiDAR 生成。这促使研究者开发“在 LiDAR 地图中定位相机”的方法。通常可以通过：从相机重建场景 3D 几何再与地图匹配，或在图像平面进行匹配。

本文提出 CMRNet++，用于相机到 LiDAR 地图配准。我们基于此前 CMRNet（受相机-激光位姿标定技术启发）。CMRNet 能在不学习地图的前提下做定位；本文进一步让它也不依赖相机内参。与 PoseNet 等 SOTA CNN 位姿回归不同，CMRNet 不学习地图内容，而是学习“把图像与已有地图匹配”。因此只要有 LiDAR 地图，CMRNet 就能用于任何环境。

但 CMRNet 输出的是度量尺度的 6DoF 刚体变换（相对于初始位姿的增量），因此网络权重会与采集训练数据时的相机内参绑定。本文通过解耦定位流程来解决：先做像素到 3D 点匹配，再做位姿回归。这样网络与相机内参无关（第二步仍需用到内参）。我们在 KITTI、Argoverse、Lyft5 上评估，证明方法在地图与相机参数上都具备“不可知/无关（agnostic）”能力。

II. 技术方法

我们将定位拆成两步：
1）像素到 3D 点匹配；2）位姿估计（pose regression/求解）。

第一步 CNN 只在像素层面学习匹配，而不是直接做度量位姿回归，因此网络不依赖相机内参。相机内参被放到第二步，用传统几何方法根据匹配结果估计位姿。训练完成后，CMRNet++ 可用于不同相机与不同地图（相比训练时）。

（论文图 1：RGB 图像 + LiDAR-image 输入网络，输出像素位移场，再用 PnP+RANSAC 定位。）

A. 匹配步骤（Matching Step）

我们把 LiDAR 地图（点云）投影到一个虚拟成像平面，得到一个合成深度图，称为 LiDAR-image。该虚拟相机位姿放在 (H_{init})（一个粗略位姿估计，例如来自 GNSS）。投影使用相机内参。为处理点云遮挡，用 z-buffer 加遮挡估计滤波（细节见 [18]）。

网络输入为 RGB 图像与 LiDAR-image。对 LiDAR-image 中每个 3D 点，CMRNet++ 预测 RGB 图像中表示同一世界点的像素位置。

网络结构基于 PWC-Net（用于光流）。不同点：

RGB 与 LiDAR-image 属于不同模态，因此两路特征金字塔提取器不共享权重；
LiDAR-image 是单通道深度，因此把第一层卷积输入通道从 3 改为 1。
输出是一个稠密特征图（分辨率为输入的 1/4），2 个通道，对应 LiDAR-image 每个像素到 RGB 中对应像素的位移 ((u,v))。

训练标签（位移真值）生成：
先把地图点从 map 坐标系变换到 (H_{init}) 坐标系（式 1），再用内参 (K) 投影到 LiDAR-image（式 2）。筛选有效点索引 (VI)：剔除投影到相机后方、超出图像、或被遮挡滤波判定为遮挡的点。用这些有效点生成稀疏深度图 (D)（式 3）。然后把地图点投影到真值位姿 (H_{GT}) 对应的成像平面（式 4）。最后位移真值由两次投影像素差计算（式 5）。

对没有 3D 点的像素，设 (D_{u,v}=0)，位移 (\Delta P_{u,v}=[0,0])。并构造 mask：若 (D_{u,v}>0) 则为 1，否则为 0。

**损失函数：**由回归项 (L_{reg}) 与平滑项 (L_{smooth}) 组成。

(L_{reg})：只在有效像素（mask=1）处惩罚预测位移与真值位移差异（式 6）。
(L_{smooth})：对无真值像素（mask=0），鼓励其位移与邻域位移相似（式 7、8），其中 (\rho) 为 generalized Charbonnier：(\rho(x)=(x^2+\epsilon2)^{\alpha)，(\epsilon=10}{-9},\alpha=0.25)。

B. 定位步骤（Localization Step）

推理时：我们已知地图 3D 点集合 (P)，以及它们在 LiDAR-image 中的像素位置，同时网络输出这些点在 RGB 图像中的匹配像素 (p)：

(P_i=[x_i,y_i,z_i], i\in VI)（式 9）
(p_i=[u^{init}_i,v{init}i]+\widehat{\Delta P}{u^{init}_i,v{init}_i})（式 10）

给定 2D-3D 对应与相机内参，求相机位姿属于 PnP 问题。论文使用 EPnP + RANSAC：最大迭代 1000 次，内点阈值 2 像素。

C. 迭代细化（Iterative Refinement）

类似 CMRNet，采用迭代细化：训练多个 CMRNet++ 实例，每个实例专注不同初始误差范围。推理时先用“能处理大误差”的网络预测一次并更新位姿，再按新位姿重投影生成新的 LiDAR-image，送入“更小误差范围”的网络继续细化。可重复多次。

D. 训练细节（Training Details）

每个 CMRNet++ 实例从零训练 300 epoch，batch size 40，2 张 NVIDIA Tesla P100。优化器 Adam，初始学习率 (1.5\times 10^{-4})，weight decay (5\times 10^{-6})。第 20、40 epoch 学习率减半。

III. 实验结果

A. 数据集

为了评估定位精度与泛化，使用三个不同国家、不同传感器、不同交通条件的数据集：

1）KITTI：Karlsruhe。训练：序列 03/05/06/07/08/09（11426 帧）；验证：序列 00（4541 帧）。使用左目。
2）Argoverse：Miami/Pittsburgh。使用中前相机，1920×1200@30fps。训练：train1/2/3（36347 帧）；验证：train4（2741 帧）。
3）Lyft5：Palo Alto。前向相机 1224×1024。选 10 个城市场景做验证（1250 帧）。用于测试“零训练迁移”：先在 KITTI+Argoverse 训练，再直接在 Lyft5 上测试，不做微调。

B. LiDAR 地图生成

将单帧 LiDAR 扫描按其真值位姿聚合成全局点云地图：Argoverse/Lyft5 使用数据集提供真值位姿；KITTI 使用 SLAM 系统生成位姿。用 Open3D 按 0.1m 体素下采样。为只保留静态物体，利用 Argoverse/Lyft5 的 3D 框去除动态物体；KITTI 里程计序列无 3D 框，无法移除动态物体，未来计划用语义分割/3D 检测去除。

C. 多数据集联合训练

联合 KITTI 与 Argoverse 训练会遇到：

数据量不均衡（11426 vs 36347），易偏向 Argoverse。做法：每个 epoch 开始随机采样 Argoverse 子集，使其样本数与 KITTI 相同；不同 epoch 采样不同子集，最终网络会见到全部 Argoverse 数据。
相机 FOV 与分辨率差异大。做法：让分辨率一致并避免 Argoverse 直接裁剪导致 FOV 过窄：先把 Argoverse 下采样到 960×600，再对两者都随机裁剪到 960×320。裁剪在训练时在线随机做，以增强多样性。LiDAR-image 与位移真值在原分辨率生成，再对应下采样/裁剪；下采样时位移也要减半。

D. 初始位姿采样与数据增强

采用三阶段迭代细化，因此训练 3 个网络，分别覆盖不同初始误差范围。通过在真值位姿 (H_{GT}) 上加均匀噪声模拟 (H_{init})：

第 1 次迭代：平移 ±2m，旋转 ±10°
第 2 次迭代：±1m，±2°
第 3 次迭代：±0.6m，±1°

数据增强：

颜色扰动（对比度/饱和度/亮度）
随机水平翻转（并相应修改相机标定）
随机旋转 [-5°, 5°]
同步变换 LiDAR 点云以匹配这些变化，再生成 LiDAR-image

表 I（结果：三数据集三次迭代的中位数误差）

初始位姿误差约：平移 1.97m，旋转 9.83°。
迭代后（中位数）：

KITTI：
- Iter1：0.55m / 1.46°，失败率 2.18%
- Iter2：0.22m / 0.77°
- Iter3：0.14m / 0.43°
Argoverse：
- Iter1：0.80m / 1.55°，失败率 6.24%
- Iter2：0.34m / 0.58°
- Iter3：0.25m / 0.45°
Lyft5（零训练迁移测试）：
- Iter1：1.32m / 2.13°，失败率 10.56%
- Iter2：0.79m / 1.28°
- Iter3：0.70m / 1.18°

（Lyft5 未参与训练，用于证明无需重训即可泛化。）

表 II（KITTI seq00 与 SOTA 对比）

Caselitz [14]：平移 mean 0.30m，旋转 mean 1.65°（该方法还用跟踪与时序滤波）
CMRNet [18]：平移 median 0.27m，旋转 median 1.07°
CMRNet++：平移 median 0.14m，旋转 median 0.43°（最好）

E. 推理（Inference）

推理时一次处理一张图。网络全卷积，因此不同数据集不需要强制 resize 到统一分辨率，直接输入整张图（不裁剪）。但 Argoverse 在推理时仍会下采样以保持与训练时类似的 FOV。得到 2D-3D 对应后用 PnP+RANSAC 求位姿，并用三阶段网络迭代细化。

运行时间（单次迭代，GTX 1080Ti）：

CNN：约 0.05s
PnP+RANSAC：约 1.25s
（三次迭代总计约 3.9s 级别，按论文设置）

F. 讨论

验证集地理上与训练集分离，确保测试地点未见过。Lyft5 更是不同城市与不同传感器，且不重训。结果表明 CMRNet++ 在三数据集均有良好泛化。

CMRNet++ 可能失败，尤其在第 1 次迭代：若超过一半匹配错误，PnP+RANSAC 会输出错误解。作者认为主要原因是地面平面像素匹配困难（道路外观均一，难以区分具体像素）。失败判定：若第 1 次迭代结果距离 (H_{init}) 超过 4m，则标记失败，不进入后续迭代。失败率如表 I。

IV. 结论

提出 CMRNet++：一种用于 LiDAR 地图中单目定位的 CNN 方法，设计目标是同时与地图无关、与相机内参无关。据作者所知，这是首个无需重训即可泛化到新环境的 DNN 位姿回归类方法。KITTI 上中位数误差可达 0.14m / 0.43°，并在 Lyft5 零训练迁移中也能有效定位。未来计划探索把 differentiable RANSAC 用于端到端训练，同时保持相机参数在学习步骤之外。

2) 论文创新点概括（面向技术贡献）

把“度量位姿回归”改为“像素匹配 + 几何求解”
- 核心是把 metric reasoning 从网络里拿出去：网络只预测像素位移场（类似跨模态光流），从而摆脱“网络权重绑定相机内参”的问题。
相机内参无关（camera-agnostic）的学习设计
- 通过预测 LiDAR-image 到 RGB 的像素对应，而不是直接输出 6DoF 增量，使得同一网络可换相机（不同内参）仍可用；内参只在 PnP 求解时使用。
跨模态（RGB-深度投影图）PWC-Net 式结构用于稠密匹配
- 两路特征不共享权重 + 深度单通道输入适配，使其更贴合“RGB vs LiDAR-image”的模态差异。
迭代细化 + 分误差区间专门训练
- 通过多阶段网络分别覆盖大误差到小误差，提高收敛与鲁棒性，是工程上可用的重要设计。
零重训迁移的实证（KITTI+Argoverse 训练，Lyft5 直接测）
- 在自动驾驶这种城市差异巨大、传感器不同的场景，展示“无需微调可用”的证据链，是论文最想强调的亮点。

3) 实车内存占用与运行时耗分析（含可行优化方向）

下面按单帧推理的主要模块拆解：
A) 生成 LiDAR-image（点云投影 + z-buffer/遮挡过滤）
B) CNN 前向（CMRNet++）
C) 建立 2D-3D 对应 + EPnP + RANSAC
D) 迭代三次（重复 A/B/C）

论文给出的瓶颈非常明确：PnP+RANSAC（1.25s/iter）远大于 CNN（0.05s/iter）。

3.1 车端内存占用（RAM / GPU 显存）主要来自什么？

1) 地图点云（常驻内存，最大头）

地图点云即使 0.1m 下采样，城市级地图仍可能是千万级点量级（取决于覆盖范围）。
单点至少需要：XYZ（3×float32=12B），可能还要强度/语义/索引等；实际工程常见 16–32B/点。
举例（只做量级估算）：
- 5,000,000 点 × 16B ≈ 80MB
- 20,000,000 点 × 16B ≈ 320MB
  这还没算加速结构（体素栅格、KD-tree、hash map）带来的额外占用。

结论：若做“全城点云常驻”，内存会很快上百 MB 到数 GB；实车通常需要“按车位姿切片加载/滑窗缓存”。

2) LiDAR-image / 中间张量（每次迭代都要）

LiDAR-image 本质是一张深度图（float32 或 float16）。
若输入分辨率约 960×320：
- 深度图 float32：960×320×4 ≈ 1.23MB
- 位移场（2 通道）float32 在 1/4 分辨率：240×80×2×4≈0.15MB
这些都不大；真正大的是 CNN 的中间特征图（显存）。

3) CMRNet++ 网络权重与激活（显存）

论文没给参数量。按 PWC-Net 类结构，权重规模通常是几十 MB 以内（float32）。推理可用 FP16/INT8 进一步减半/四分之一。
推理显存峰值常由激活特征图决定，取决于输入分辨率与金字塔层数。以 960×320 级别输入，现代轻量化部署下通常能压到几百 MB 以内；如果用 TensorRT FP16/INT8，几十到一两百 MB 也常见。

4) RANSAC 的对应点集合与临时数据（CPU 内存）

对应点数取决于有效深度像素数量：如果 LiDAR-image 稀疏但覆盖多，可能有几万到几十万对应。

存 3D 点（float32×3）+ 2D 点（float32×2）≈ 20B/对应（不含开销）
50k 对应 ≈ 1MB
500k 对应 ≈ 10MB
因此对应集合本身不是大头，但 RANSAC 迭代的计算是大头。

3.2 运行时耗（时延）分析：论文设置在车上为什么慢？

论文报告（GTX 1080Ti）：

CNN：0.05s/iter（约 20 FPS）
PnP+RANSAC：1.25s/iter
三次迭代：≈ 3×(0.05+1.25)=3.9s/帧（约 0.26 FPS）

这对实车闭环定位（通常期望 10–30Hz，或至少 1–5Hz）明显不够。主要原因：

RANSAC 迭代次数高（最多 1000）且内点评估代价大
- 每次假设要把大量 3D 点投影回图像计算重投影误差，点数越多越慢。
匹配在道路平面等“退化区域”错误率高
- 错配多 ⇒ 需要更多 RANSAC 迭代才能找到一致集，或直接失败。
三阶段迭代把几何求解重复了 3 次
- CNN 很快但几何很慢，重复三次使总时延线性上升。

3.3 实车可行的加速/降内存方案（不改变论文核心思想）

A) 让 PnP+RANSAC 从“秒级”到“毫秒级”的关键做法

强力下采样对应点（correspondence thinning）
- 从所有有效像素中随机/网格采样到例如 2k–10k 点；
- 或按深度分布、图像均匀性、边缘区域优先等策略采样。
  目标：把“每次评估要投影的点数”压到可控。
加入匹配置信度/一致性过滤（只喂高质量点给 PnP）
- 原论文输出只是位移，没有显式置信度；工程上可从 cost volume/特征相关强度派生置信度，或训练时额外预测一张置信度图。
- 只保留 top-K 或阈值过滤，显著降低外点比例 ⇒ RANSAC 迭代数可减少。
用更快的鲁棒求解：P3P+RANSAC + 局部优化 / 或 LO-RANSAC
- EPnP 常用，但在 RANSAC 框架里常见做法是用 P3P（最小集）产生假设更快；
- 然后对内点做一次非线性优化（Gauss-Newton/LM）精修。
- 同时启用 early termination（达到足够内点比例就停）。
把 RANSAC 放到 GPU / 或用并行化实现
- OpenCV 的 solvePnPRansac 通常在 CPU 上，1000 次迭代+大量点会很慢。
- 工业实现常把假设生成与内点统计并行化。
减少迭代次数：用运动模型/时序先验缩小初始误差
- 实车有 IMU/轮速/里程计，可把 (H_{init}) 误差从“±2m±10°”压到更小；
- 则可以只跑后两级网络、甚至单级网络即可满足精度，直接把 3 次迭代降到 1 次或 2 次。

B) 地图侧的内存/速度工程化

局部子图（submap）切片加载
- 只加载车辆附近例如 50m×50m 或 100m×100m 的点云块；
- 用空间索引（体素哈希/八叉树）快速裁剪，避免全图投影。
地图多分辨率（LOD）
- 第 1 级用更稀疏点云做粗定位；第 2/3 级用密点云精定位。
- 与迭代细化天然契合。

3.4 给一个“可落地”的资源预算参考（量级）

在做了“子图 + 对应点采样 + 更快 RANSAC”的情况下，常见可达到：

CNN 前向：仍在 20–50ms/iter（取决于 GPU/分辨率/量化）
投影生成 LiDAR-image：5–20ms（GPU实现更快；CPU实现视点数而定）
PnP+RANSAC（K=2k~10k 对应）：10–50ms（良好实现 + 早停）
总计：单次迭代 40–120ms；两次迭代 80–240ms（约 4–12.5Hz）

内存方面：

子图点云常驻：几十 MB 到一两百 MB（看覆盖半径与密度）
GPU 显存：模型+激活+输入，FP16 下通常几百 MB 内可控

这才更接近实车在线定位的可用范围。