SLAM文献之-MASt3R-SLAM: Real-Time Dense SLAM with 3D Reconstruction Priors

1、系统简介

MASt3R-SLAM 是一种基于 MASt3R（Matching and Stereo 3D Reconstruction）的实时单目稠密 SLAM 系统。该系统利用 MASt3R 提供的双视图 3D 重建先验，实现了在未知相机模型下的稳健定位与稠密重建。系统在标准 GPU 上可达到每秒 15 帧的实时性能，适用于动态环境中的视频序列处理。

2、算法背景

传统的 SLAM 系统通常依赖于手工设计的特征匹配和几何约束，难以处理复杂场景中的稠密重建。近年来，基于深度学习的 3D 重建先验（如 MASt3R）为 SLAM 提供了新的思路。这些先验通过学习图像对之间的几何关系，提供了更准确的匹配和重建信息。

3、论文贡献（创新点）

1. 首次将 MASt3R 先验应用于实时 SLAM 系统：将双视图 3D 重建先验引入 SLAM，实现了在未知相机模型下的实时定位与重建。

2. 高效的点图匹配与相机追踪方法：提出了基于 MASt3R 的点图匹配和相机追踪算法，提高了系统的鲁棒性和精度。

3. 全局一致性的图优化与闭环检测：引入了二阶全局优化和闭环检测机制，确保了大规模场景中的一致性和精度。

4. 无需已知相机模型的鲁棒性：系统在无需已知相机内参的情况下，仍能实现高质量的稠密重建，适用于多种相机配置。

4、需解决的关键问题

• 动态环境中的鲁棒性：如何在动态场景中有效区分静态和动态物体，以避免重建误差。

• 实时性能与计算资源的平衡：在保证高精度的同时，如何优化算法以适应实时处理的需求。

• 闭环检测与图优化的效率：如何在大规模场景中实现高效的闭环检测和图优化，以保持全局一致性。

5、论文提出的算法流程

1. 输入处理：接收连续的视频帧，提取关键帧进行处理。

2. 点图匹配：利用 MASt3R 先验进行双视图点图匹配，获取初步的 3D 重建信息。

3. 相机追踪：通过匹配结果进行相机位姿估计，跟踪相机运动。

4. 局部融合：将新获取的 3D 点云与现有地图进行融合，更新局部地图。

5. 图构建与闭环检测：构建位姿图，进行闭环检测，识别回环并进行优化。

6. 全局优化：进行二阶全局优化，确保全局一致性。

7. 输出结果：输出相机轨迹和稠密点云地图。

6、核心方法细节

6.1、MASt3R 先验的应用

• MASt3R 先验的应用：MASt3R 提供了双视图之间的几何匹配信息，作为 SLAM 系统的先验知识，提高了匹配的准确性。

1️、输入与输出

• 输入：DUSt3R 接收一对图像

$$I_i,I_j\in {\mathbb{R}}^{H\times W\times 3}$$

• 输出：对应的点图（pointmaps）及置信度

$$X_i^i,X_i^j\in {\mathbb{R}}^{H\times W\times 3},C_i^i,C_i^j\in {\mathbb{R}}^{H\times W\times 1}$$

• 记号说明：$X_j^i$ 表示图像 $i$ 的点图在相机 $j$ 坐标系下的表示。

2️、特征预测

• MASt3R 为匹配任务额外预测每像素的 $d$ 维特征及其置信度：

$$D_i^i,D_i^j\in {\mathbb{R}}^{H\times W\times d},Q_i^i,Q_i^j\in {\mathbb{R}}^{H\times W\times 1}$$

• 定义 $FM(I_i,I_j)$ 为 MASt3R 的前向计算（forward pass），直接输出点图、特征及置信度。

3️、尺度与位姿表示

• 虽然训练数据具有度量尺度（metric scale），但尺度差异是预测不一致的主要来源。
• 位姿定义为仿射尺度变换：

$$T\in \text{Sim}(3)$$

• 使用李代数（Lie algebra） $\tau \in \text{sim}(3)$ 和**左加算子（left-plus operator）**更新位姿：

$$T=\left[\begin{array}{ccc}s0& R& t\end{array}\right],T\leftarrow \tau \oplus T\triangleq \text{Exp}(\tau )\circ T\text{\hspace{1em}(1)}$$

• 其中 $R\in SO(3)$、$t\in {\mathbb{R}}^3$，尺度 $s\in \mathbb{R}$。

4️、相机模型假设

• 仅假设通用中心相机（generic central camera） [35]，即所有光线通过唯一相机中心。
• 定义函数 $\psi (X_i^i)$ 将点图 $X_i^i$ 归一化为单位长度光线，每个点图定义自己的相机模型。
• 该方法可以统一处理时间变化的相机模型（如变焦）和**畸变（distortion）**问题。

6.2、点图匹配（Pointmap Matching）

• 点图匹配算法：采用高效的点图匹配算法，减少了计算量，提高了实时性。

1️、概述

• 对应关系（correspondence）是 SLAM 的核心组成部分，对于**跟踪（tracking）和建图（mapping）**都非常重要。
• 给定 MASt3R 的点图（pointmaps）和特征（features），目标是找到两幅图像间的像素匹配集合：

$$m_{i,j}=M(X_i^i,X_i^j,D_i^i,D_i^j)$$

• 蛮力匹配（brute-force matching）复杂度为二次，因为需要在所有像素对上进行全局搜索。
• DUSt3R 使用 3D k-d 树加速，但构建难以并行化，并且点图误差会导致不准确匹配。
• MASt3R 通过网络预测额外高维特征，实现更大基线匹配，并提出粗到细（coarse-to-fine）策略。
• 尽管如此，稠密像素匹配仍需秒级，稀疏匹配速度慢于 k-d 树。

2️、方法思路

• 不依赖全局搜索的高效方法，而是借鉴优化局部搜索的思想。
• 灵感来源于密集 SLAM 的投影数据关联（projective data-association）。
• 不同于传统投影数据关联需要闭式投影的相机模型，MASt3R 仅假设每帧有唯一相机中心。
  

3️、迭代投影匹配

• 给定点图 $X_i^i,X_i^j$，构建图像 $I_i$ 的通用相机模型，光线记作 $\psi (X_i^i[p])$。
• 对每个点 $x\in X_i^j$ 独立优化参考帧像素坐标 $p$，最小化光线误差：

$$p^{\ast }=\arg \underset{p}{\min }\Vert \psi ([X_i^i{]}_p)-\psi (x){\Vert }^2\text{\hspace{1em}(2)}$$

• 欧氏距离最小化等价于归一化光线夹角最小化：

$$\Vert {\psi }_1-{\psi }_2{\Vert }^2=2(1-\cos \theta ),\cos \theta ={\psi }_1^T{\psi }_2\text{\hspace{1em}(3)}$$

• 使用非线性最小二乘（nonlinear least-squares）方法，解析雅可比（analytical Jacobians），通过 Levenberg-Marquardt 求解。
• 每个点独立计算，几乎所有像素在 10 次迭代内收敛。

4️、初始值与遮挡处理

• 无投影初值（如新关键帧或闭环匹配）时，像素初始化为恒等映射（identity mapping）。
• 跟踪过程中使用上一帧匹配作为初值，加速收敛。
• 剔除 3D 距离过大的匹配，处理遮挡（occlusion）和异常值（outliers）。
• GPU 并行化，实现增量 SLAM 的高效匹配。

5️、特征优化（Coarse-to-Fine）

• 几何匹配提供初值，但每像素高维特征进一步提升位姿估计精度。
• 在局部窗口内进行粗到细图像搜索，更新像素位置以获得最大特征相似度。

6️、CUDA 实现与性能

• 迭代投影与特征精炼实现为自定义 CUDA 内核（kernels），像素级并行计算。
• 跟踪仅需 2 毫秒，新增图边构建几毫秒即可完成，无需投影初值。
• 匹配不依赖位姿估计，完全依赖 MASt3R 输出，区别于传统投影数据关联。

小结

• MASt3R 的点图匹配结合了几何初值 + 高维特征优化 + 并行计算。
• 支持高效、稳健的稠密匹配，适合实时 SLAM 系统。

6.3、相机追踪与位姿估计

• 相机追踪与位姿估计：利用匹配结果进行相机位姿估计，采用鲁棒的追踪算法，确保在动态场景中的稳定性。

1️、核心任务

SLAM 的关键组成部分之一是低延迟跟踪当前帧相对于地图的位姿。

• 在关键帧（keyframe）系统中，我们需要估计当前帧 $I_f$ 与上一关键帧 $I_k$ 之间的相对变换 $T_{kf}$。
• 为了高效，我们希望只使用网络的一次前向计算来估计该变换。

假设我们已经得到了上一关键帧的点图估计 ${\tilde{X}}_k^k$，为了求解 $T_{kf}$，我们需要将当前帧 $I_f$ 的点映射到相应坐标系中，这可以通过

$$FM(I_f,I_k)$$

得到。

2️、基于 3D 点误差的位姿估计

一种直接的方法是最小化 3D 点误差：

$$E_p=\sum _{m,n\in m_{f,k}}\rho (\Vert {\tilde{X}}_{k,n}^k-T_{kf}{X}_{f,m}^f\Vert w(q_{m,n},{\sigma }_p^2))\text{\hspace{1em}(4)}$$

其中，匹配置信度权重为：

$$q_{m,n}=p(Q_{f,m}^f{Q}_{f,n}^k)$$

• 该加权方法来源于 MASt3R-SfM [10]。
• 为了鲁棒性，除了 Huber 范数 $\Vert \cdot {\Vert }_{\rho }$，每个匹配还使用权重函数：

$$w(q,{\sigma }^2)=\{\begin{array}{ll}{\sigma }^2/q& q>q_{\text{min}}\\ \infty & \text{otherwise}\end{array}\text{\hspace{1em}(5)}$$

• 虽然也可以将 $X_f^k$ 对齐到 $X_k^k$（无需显式匹配，因为像素已对齐），但实验发现显式匹配 $X_f^f$ 对大基线场景下精度更高。

注意：3D 点误差虽然可用，但容易被点图预测误差影响，因为深度预测的不一致性较为常见。跟踪误差会降低关键帧点图质量，而这些点图随后还会用于后端优化。

3️、基于光线的误差（Ray Error）

• 利用点图可以在中心相机假设下转换为光线（rays），可以计算方向光线误差（directional ray error），对深度预测错误更不敏感。
• 计算方法：将 Eq. (4) 的点归一化为单位光线：

$$E_r=\sum _{m,n\in m_{f,k}}\rho (\Vert \psi ({\tilde{X}}_{k,n}^k)-\psi (T_{kf}{X}_{f,m}^f)\Vert w(q_{m,n},{\sigma }_r^2))\text{\hspace{1em}(6)}$$

• 得到类似于 Eq. (3) 中的角度误差（见图 2）。

• 我们现在有许多已知匹配点，希望找到一个位姿，使参考光线与当前帧预测光线之间的角度误差总和最小。

• 由于角度误差有界，光线误差对异常值（outliers）更鲁棒 [30]。

• 另外，添加一个小权重的相机中心距离误差项，以避免纯旋转下系统退化，同时减少深度误差带来的偏差。

4️、位姿更新求解

• 使用 Gauss-Newton 在迭代加权最小二乘（IRLS）框架下高效求解位姿更新。
• 对相对位姿 $T_{kf}$ 的扰动 $\tau$ 计算光线误差和距离误差的解析雅可比（analytical Jacobians）。
• 将残差、雅可比和权重分别堆叠为矩阵 $r,J,W$，迭代求解线性系统并更新位姿。

在迭代加权最小二乘（IRLS）框架下，位姿更新通过线性系统求解：

$$J^{T}WJ\tau =-J^{T}Wr,T_{kf}\leftarrow \tau \oplus T_{kf}\text{\hspace{1em}(7)}$$

• 由于每个点图可能提供有价值的新信息，我们不仅在几何估计上进行滤波，同时也在相机模型上进行滤波，因为相机模型由光线（rays）定义。
• 求解相对位姿后，可以使用变换 $T_{kf}$ 更新标准点图（canonical pointmap） ${\tilde{X}}_k^k$，采用递推加权平均滤波（running weighted average filter）：

$${\tilde{X}}_k^k\leftarrow \frac{{\tilde{C}}_k^k{\tilde{X}}_k^k+C_f^k(T_{kf}{X}_f^k)}{{\tilde{C}}_k^k+C_f^k},{\tilde{C}}_k^k\leftarrow {\tilde{C}}_k^k+C_f^k\text{\hspace{1em}(8)}$$

• 初始点图由于仅使用小基线帧，误差较大且置信度较低，但滤波会将来自多个视角的信息融合起来。
• 实验表明，不同的更新方法中，加权平均最能在保持一致性的同时滤除噪声。
• 与 MASt3R-SfM [10] 中的标准点图相比，我们采用增量计算（incremental computation），需要对点进行变换，因为额外预测 $X_k^k$ 会降低跟踪速度。

滤波在 SLAM 中有着丰富的应用历史，其优点在于：可以利用所有帧的信息，而无需在后端显式优化所有相机位姿，也无需存储解码器输出的所有预测点图。

6.4、闭环检测与图优化

• 闭环检测与图优化：引入闭环检测机制，识别回环并进行图优化，消除漂移，保持全局一致性。

7、算法优势

• 高精度的稠密重建：利用 MASt3R 先验，实现了高质量的 3D 重建。

• 无需已知相机模型：系统在无需已知相机内参的情况下，仍能实现高精度的定位与重建。

• 实时性能：在标准 GPU 上可达到每秒 15 帧的实时性能，适用于动态环境中的视频序列处理。

• 全局一致性：通过闭环检测和图优化，确保了大规模场景中的一致性和精度。

8、参考文献

• Murai, R., Dexheimer, E., & Davison, A. J. (2024). MASt3R-SLAM: Real-Time Dense SLAM with 3D Reconstruction Priors. arXiv preprint arXiv:2412.12392.

• Murai, R., Dexheimer, E., & Davison, A. J. (2025). MASt3R-SLAM: Real-Time Dense SLAM with 3D Reconstruction Priors. CVPR 2025.