Slide 1: 研究背景与动机 (Motivation)

传统Visual SLAM的困境

稀疏特征法 (ORB-SLAM)： 在低纹理、强光照变化场景下易跟踪失败。
纯视觉缺陷： 存在尺度模糊 (Scale Ambiguity)，无法感知绝对物理尺度。

深度学习SLAM (Deep SLAM)的兴起与不足

优势： 前馈模型 (如 DUSt3R, MASt3R) 能直接回归稠密3D结构，具有强大的几何先验。
不足： 往往忽略多传感器融合 (IMU/GNSS) 的概率优势；难以保证全局一致性。

本文核心目标

紧耦合 (Tightly-coupled)： 将前馈视觉模型的Sim(3)输出与IMU的SE(3)测量紧密融合。
数学统一： 提出同构群变换 (Isomorphic Group Transformation) 解决不同李群空间的融合问题。

Slide 2: 系统总览 (System Overview)

两大阶段

实时SLAM (Real-Time)： 前端利用MASt3R进行稠密跟踪，后端基于滑动窗口因子图融合IMU。
全局优化 (Global Optimization)： 融合回环检测与GNSS绝对定位。

关键模块

Visual Frontend： MASt3R Network → Pointmaps → Sim(3) Constraints.
Fusion Backend： Sim(3)-to-SE(3) Bridge → Factor Graph Optimization.

Slide 3: 视觉前端 - 稠密测量模型 (Visual Measurement)

输入与输出

输入： 图像对 $I_i,I_j$。
网络${\mathcal{F}}_{dec}$输出：
$$X_i^{ij},X_j^{ij},D_i^{ij},D_j^{ij}={\mathcal{F}}_{dec}(F_i,F_j)$$
X： 稠密3D点云图 (Pointmaps)，定义在局部坐标系，尺度未知。
D： 像素级描述子图。

Slide 4: 核心推导I - Sim(3)视觉残差构建

核心选择：为什么是Sim(3)?

原因： 网络输出缺乏绝对尺度，必须优化两帧间的相似变换 $S_j^i=\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]$。

残差函数定义 (重点公式)

$$r_{ij}(S_j^i)=\left[\begin{array}{c}{u}_j^i-\pi (S_j^i\circ X_j)\\ (X_i[u_j^i]{)}_z-(S_j^i\circ X_j{)}_z\end{array}\right]$$

$u_j^i$推导?

初次关联： 利用点云 $X_i^{ij}$ 和 $X_j^{ij}$ 的空间接近度。系统寻找 $i$ 帧中的哪个像素 $u$ 指向的 3D 方向与 $j$ 帧中预测的 3D 点方向一致。公式如下 ：
$${\hat{u}}_j^i=\arg \underset{u_j^i}{\min }\Vert \frac{X_i^{ij}[u_j^i]}{\Vert X_i^{ij}[u_j^i]\Vert }-\frac{X_j^{ij}}{\Vert X_j^{ij}\Vert }\Vert$$这里利用归一化坐标来消除尺度不确定性的干扰，锁定大致的像素对应位置 。
精细化关联： 在 ${\hat{u}}_j^i$ 的邻域内，利用输出的描述子 $D_i^{ij}$ 和 $D_j^{ij}$ 进行点积搜索，找到特征最相似的点，从而获得最终高精度的像素对应关系 $u_j^i$ 。

物理意义解析

2D重投影约束： 约束平面几何一致性，变换后的点投影回图像应与观测点重合。
3D深度约束： 约束深度一致性，变换后的深度应等于参考帧预测深度。
核心作用： 有效约束纯旋转 (Pure Rotation) 运动，防止模型退化。

Slide 5: 核心推导II - 信息的压缩与线性化

非线性最小二乘问题

目标函数： $$E=\sum \Vert r_{ij}{\Vert }_{Q_{ij}}^2$$
$E$ (误差总和)：这是系统需要最小化的目标，即所有帧间视觉匹配误差的总和 。
$r_{ij}$ (视觉残差)：由公式 (11) 定义，包含了 2D 重投影误差和 3D 深度残差 。它描述了当前位姿估计与神经网络预测的 3D 结构之间的不一致性 。
$Q_{ij}$ (权重矩阵)：用于调整不同观测值的可信度 。它综合考虑了神经网络输出的置信度图（Confidence Map）以及用于抑制离群点的 Huber 鲁棒核函数 。

一阶泰勒展开 (Linearization)

公式： $$r_{ij}(S\oplus \eta )\approx r_{ij}(S)+J_{ij}\eta$$
说明： $\eta \in {\mathbb{R}}^7$ 为Sim(3)李代数扰动。

海森矩阵提取 (Hessian Extraction)

核心问题： 像素数量$N$巨大 ($H\times W$)，直接优化计算量过高。
GPU加速策略： 在GPU上计算每个像素的$J^{T}J$，累加得到紧凑海森矩阵。
核心公式：
$$H_{ij}=\sum _{k=1}^N(J_k^T{Q}_k{J}_k)\in {\mathbb{R}}^{7\times 7}$$
$$v_{ij}=\sum _{k=1}^N-(J_k^T{Q}_k{r}_k)\in {\mathbb{R}}^{7\times 1}$$
优化策略： 仅将$H_{ij},v_{ij}$传回CPU进行后端优化，降低数据传输成本。

Slide 6: 核心推导III - 同构群变换 (Isomorphic Transformation)

核心问题 (Problem)

前端输出： Sim(3)约束$\eta$ (7自由度)。
后端状态： SE(3)姿态 + 独立尺度$s$ (6+1自由度)。
待解问题： 如何将$H_{Sim3}$转换到VIO因子图中？

核心同构关系

$$S\circ p=sRp+t\equiv T\circ (s\cdot p)=R(sp)+t$$

扰动映射 (Perturbation Mapping)

Sim(3)扰动： $\eta =[\omega ,\nu ,\sigma {]}^T$
SE(3)及尺度扰动： $\xi =[\theta ,\tau {]}^T$ + 尺度扰动$\delta s$

Slide 7: 核心推导IV - 雅可比矩阵A的推导

线性关系建立

核心公式：$$\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\omega }^{\wedge }+\sigma I& \nu \\ 0& 0\end{array}\right]\approx \left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\theta }^{\wedge }& \tau \\ 0& 0\end{array}\right]\left[\begin{array}{cc}s+\delta s& 0\\ 0& 1\end{array}\right]$$

逐项推导结论

旋转项： $\omega =\theta$（旋转独立，不受尺度影响）。
尺度项： $\sigma \approx s^{-1}\delta s$（对数微分与线性微分的近似关系）。
平移项 (关键)： $\nu =s\cdot \tau$；Sim(3)平移$\nu$定义在归一化空间，SE(3)平移$\tau$定义在物理空间，物理位移需缩放$s$倍。

转换矩阵A (核心)

$$A=\text{diag}(I_{3\times 3},s\cdot I_{3\times 3},s^{-1})$$

Slide 8: 实时因子图构建 (Factor Graph)

1. 状态向量的定义 (State Vector)

首先，系统维护一个滑动窗口内的状态集合 $\mathcal{X}$ ：
$${\mathcal{X}}_i=(T_i,s_i,v_i,b_i)$$
$T_i\in SE(3)$: 第 $i$ 个关键帧在世界坐标系下的位姿（旋转和平移） 。
$s_i\in \mathbb{R}$: 尺度参数，用于校正视觉点云的比例 。
$v_i\in {\mathbb{R}}^3$: 机体在世界系下的速度 。
$b_i=(b_i^g,b_i^a)$: IMU 的角速度零偏和加速度零偏 。
精度说明：为了防止大尺度场景下的数值漂移，所有状态量均使用 double precision (float64) 维护 。

2. 视觉因子的构造 (Visual Factor)

视觉因子采用了**二次型因子（Quadric-form factor）**的形式，它描述了两帧之间的几何约束 ：
$$E_v({\mathcal{X}}_i,{\mathcal{X}}_j)=\frac{1}{2}{l}_v(\mathcal{X}{)}^{\top }{H}_{v,ij}{l}_v(\mathcal{X})-l_v(\mathcal{X}{)}^{\top }{v}_{v,ij}$$
$E_v({\mathcal{X}}_i,{\mathcal{X}}_j)$：表示第 $i$ 关键帧与第 $j$ 关键帧之间的视觉误差能量项（或代价项） 。它衡量了当前状态估计与视觉观测约束之间的不一致程度。
$l_v(\mathcal{X})$：这是一个线性容器 (Linear container) 。它的作用是将当前的系统状态 $\mathcal{X}$ 转换为在特定线性化点（基于 $Sim(3)$ 的对齐残差公式）处的李代数 (Lie algebras) 表达 。简单来说，它将复杂的位姿状态映射为优化器可以处理的微小增量。
$H_{v,ij}$：这是经过转换后的海森信息矩阵 (Hessian information) 。它是将前端 GPU 计算出的原始 $Sim(3)$ 视觉约束矩阵（$H_{ij}$），通过局部到全局的雅可比变换（Jacobian $J_{(i,j)}^{ij}$）以及同构群变换后得到的 。它代表了视觉约束在全局坐标系下的“权重”或“确定性程度”。
$v_{v,ij}$：这是转换后的海森形式向量信息 。它是对原始视觉约束向量（$v_{ij}$）进行同样的坐标系与空间变换后得到的结果，代表了视觉观测指引的位姿优化方向 。

这里最关键的步骤是 $H_{v,ij}$ 的变换推导 ：

$$\{\begin{array}{l}{H}_{v,ij}={{\Lambda }_{(i,j)}}^{\top }{J}_{(i,j)}^{ij}{H}_{ij}{J}_{(i,j)}^{ij}{\Lambda }_{(i,j)}\\ v_{v,ij}={{\Lambda }_{(i,j)}}^{\top }{J}_{(i,j)}^{ij}{v}_{ij}\end{array}$$
公式推导逻辑连贯性说明：
输入信息 ($H_{ij},v_{ij}$): 这是前端 GPU 在 float32 精度下算出来的基于 $Sim(3)$ 的海森矩阵和梯度 。
雅可比映射 $J_{(i,j)}^{ij}$: 将残差相对于局部 $Sim(3)$ 的雅可比，映射到全局 $Sim(3)$ 状态上 。
同构变换 ${\Lambda }_{(i,j)}$: 这是我们在 V. A 章节 推导的核心矩阵 $A$ 。它负责将 $Sim(3)$ 李代数扰动映射为 $SE(3)$ 姿态扰动和尺度扰动 $\delta s$ 。
结果: 经过这一系列左乘和右乘，前端的视觉信息被完美“翻译”成了后端因子图可以直接理解的语言 。

3. IMU 预积分因子 (IMU Pre-integration Factor)

系统使用经典的预积分技术来约束连续关键帧之间的运动 ：
$$r_b({\mathcal{X}}_k,{\mathcal{X}}_{k+1})=\left[\begin{array}{c}{R}_{b_k}^{w\top }(p_{b_{k+1}}^w-p_{b_k}^w+\frac{1}{2}{g}^w\Delta t_k^2-v_{b_k}^w\Delta t_k)-\Delta {\tilde{p}}_{b_{k+1}}^{b_k}\\ R_{b_k}^{w\top }(v_{b_{k+1}}^w+g^w\Delta t_k-v_{b_k}^w)-\Delta {\tilde{v}}_{b_{k+1}}^{b_k}\\ \text{Log}\left((R_{b_k}^w{)}^{-1}{R}_{b_{k+1}}^w(\Delta {\tilde{R}}_{b_{k+1}}^{b_k}{)}^{-1}\right)\\ b_{k+1}^a-b_k^a\\ b_{k+1}^g-b_k^g\end{array}\right]$$
该残差项包含了位置、速度、旋转以及零偏的随时间演变误差 。+1
IMU-Camera 外参融合：
由于 IMU 和相机不在同一个位置，推导中必须考虑外参 $T_c^b$ ：+1
$$T_{b_i}^w=T_i\circ T_c^{b^{-1}}$$
这确保了视觉观测到的相机运动与 IMU 感知到的机体运动在数学上是一致的 。

4. 系统总成本函数 (Total Cost Function)

最后，所有的因子被汇总到一个非线性优化问题中（公式 34） ：
$$\underset{\mathcal{X}}{\min }\left(\sum _{i\in \mathcal{W}}||r_b({\mathcal{X}}_i,{\mathcal{X}}_{i+1})|{|}^2+\sum _{(i,j)\in \mathcal{E}}{E}_v({\mathcal{X}}_i,{\mathcal{X}}_j)+E_m(\mathcal{X})\right)[cite:312]$$
第一项：IMU 约束，提供尺度和重力方向感 。+2
第二项：视觉约束，提供精确的局部几何结构 。+2
第三项 $E_m(\mathcal{X})$：边缘化因子（Marginalization factor），代表被移出窗口的旧帧所留下的信息先验 。+1

Slide 10: 实验结果 (Experiments)

$KITTI-360数据集上不同VIO方案的相对位姿误差。$

$KITTI-360数据集上不同全局SLAM(带闭环)的绝对平移误差(m)$