InfiniTAM SLAM 详细流程

InfiniTAM 是基于 TSDF（截断签名距离函数） 的 体素（Voxel）级 SLAM 系统，适用于实时 3D 重建。整个系统主要包含 四个核心模块：传感器输入、位姿估计（Tracking）、映射更新（Mapping）、表面提取（Rendering）。以下是每个模块的详细解析：

---

1. 传感器输入（Sensor Input）

(1) 传感器类型

InfiniTAM 主要支持 RGB-D 传感器（如 Kinect、Intel RealSense、Azure Kinect），输入数据包括：

• 深度图（Depth Map）：表示场景中各点到相机的深度信息（单位：米）。
• 彩色图（RGB Image）：提供场景的纹理信息。

(2) 数据预处理

为了提高系统精度，输入数据需进行以下处理：

• 深度图滤波（如双边滤波，减少噪声）。
• RGB-D 对齐（确保彩色图和深度图像素一一对应）。
• 相机内参校正（应用相机内参矩阵，计算 3D 点云）。

(3) 3D 点云计算

根据深度图，将像素点投影到 3D 空间：
$$[P_{3D}=K^{-1}\cdot (u,v,d,1{)}^T]$$
其中：

• ( K ) 为相机内参矩阵。
• ( (u, v) ) 是像素坐标，( d ) 是深度值。
• $(P_{3D})$ 为计算出的三维坐标。

---

2. 位姿估计（Tracking）

位姿估计的目标是 计算当前帧相机相对于前一帧的运动，常用方法包括 ICP（迭代最近点） 和 RGB-D 直接法。

(1) 迭代最近点（ICP）方法

目标：最小化帧间对应点的欧氏距离：
$$[E(R,t)=\sum _i\Vert R{p}_i+t-q_i{\Vert }^2]$$
其中：

• $(p_i)$ 是当前帧点云的 3D 点。
• $(q_i)$ 是上一帧匹配的 3D 点。
• $(R,t)$ 是旋转和平移矩阵。

求解方法：

1. 寻找对应点：使用最近邻搜索找到每个点 ( p_i ) 在上一帧中的最近点 ( q_i )。
2. 计算变换矩阵：
   • 计算 质心对齐 后的协方差矩阵 ( H )：
     $$[H=\sum (p_i-\bar{p})(q_i-\bar{q}{)}^T]$$
   • 通过 SVD 分解 求解旋转矩阵 ( R )：
     $$[U,\Sigma ,V^T=\text{SVD}(H),R=V{U}^T]$$
   • 计算平移矩阵 ( t )：
     $$[t=\bar{q}-R\bar{p}]$$
3. 更新位姿，迭代收敛。

(2) RGB-D 直接法

利用图像梯度信息优化位姿：

$$[E(R,t)=\sum _i\Vert I_t(u_i)-I_{t-1}(R{u}_i+t){\Vert }^2]$$

• 适用于 纹理丰富的环境。
• 计算代价较高，但适用于无几何特征的场景。

(3) 运动约束

为了提高鲁棒性，可引入 IMU 数据 或 运动模型 作为额外约束，避免漂移。

---

3. 映射更新（Mapping）

映射模块的目标是构建和维护 3D 环境模型，InfiniTAM 采用 TSDF（截断签名距离函数） 进行建模。

(1) TSDF 体素表示

核心思想：使用体素网格（Voxel Grid）存储 3D 空间信息，每个体素包含：

• TSDF 值：表示体素到最近表面的有符号距离。
• 权重值：用于加权融合多个观测。

TSDF 计算公式：
$$[\Psi (p)=\{\begin{array}{ll}d(p),& \text{if }d(p)\le \delta \\ \text{truncate},& \text{otherwise}\end{array}]$$
其中：

• $(d(p))$ 是点 ( p ) 到最近表面的距离。
• $(\delta )$ 是截断范围（如 5cm）。

(2) 体素融合

当新帧进入时，需要更新 TSDF 体素：

$$[{\Psi }_{new}=\frac{w_{prev}{\Psi }_{prev}+w_{new}{\Psi }_{obs}}{w_{prev}+w_{new}}]$$
其中：

• $({\Psi }_{prev})$ 是之前存储的 TSDF 值。
• $({\Psi }_{obs})$ 是新观测 TSDF 值。
• $(w_{prev},w_{new})$ 是对应的加权值。

(3) 体素存储优化

InfiniTAM 采用 多级体素存储（Hashing + Octree）：

• 稀疏体素存储（Voxel Hashing）：仅存储激活体素，减少内存消耗。
• 分层体素存储（Octree）：在不同分辨率下存储体素，提高计算效率。

---

4. 表面提取（Rendering）

为了可视化 3D 地图，InfiniTAM 采用 Marching Cubes 算法 从 TSDF 体素生成网格模型。

(1) Marching Cubes 网格提取

1. 体素扫描：遍历 TSDF 体素网格，找到 TSDF 值过零的区域（表面区域）。
3. 插值计算：在 TSDF 过零点进行线性插值，计算表面点位置：
   $$[v=\frac{p_1\cdot |{\Psi }_2|+p_2\cdot |{\Psi }_1|}{|{\Psi }_1|+|{\Psi }_2|}]$$
4. 查表生成三角面片：根据查表法构建三角形网格。

(2) GPU 渲染优化

• TSDF 体素存储在 GPU 纹理缓存中，加速查询。
• CUDA 并行计算 体素融合和三角网格提取。

---

5. CUDA 并行优化策略总结

任务	并行策略	优化方法
TSDF 体素更新	线程块更新单个体素	CUDA 共享内存 + 原子操作
ICP 位姿估计	并行计算点云匹配	Warp-level 归约 + cuSolver SVD
Marching Cubes	并行体素扫描 + 三角化	Thrust 前缀和 + 原子操作

---

6. 地图更新策略

1. TSDF 体素融合（Voxel Integration）

(1) TSDF 计算公式

对于每个体素，InfiniTAM 维护一个 TSDF 值和一个融合权重：
$$[\Psi (p)=\{\begin{array}{ll}d(p),& \text{if }d(p)\le \delta \\ \text{truncate},& \text{otherwise}\end{array}]$$
其中：

• $(d(p))$ 是点 ( p ) 到最近表面的距离。
• $(\delta )$ 是截断范围（一般取 5cm）。

融合策略：
当新帧进入时，更新 TSDF,同上描述

✅ 优化点：

• 新观测权重衰减（Weight Decay）：防止旧数据被快速遗忘，提升稳定性。
• 深度自适应加权：根据深度置信度调整融合权重，减少远距离误差。

---

2. 体素存储优化（Voxel Storage）

由于 TSDF 需要存储大量体素，InfiniTAM 采用 哈希体素存储（Voxel Hashing） 进行优化。

(1) 体素哈希存储

InfiniTAM 使用 哈希表 仅存储有效体素：

• 哈希索引计算：

  $$[h(x,y,z)=(x\cdot p_1+y\cdot p_2+z\cdot p_3)\mathrm{mod}N]$$
  其中 ( p_1, p_2, p_3 ) 为素数，( N ) 为哈希表大小。

• 仅存储 TSDF ≠ 0 的体素，减少内存占用。

✅ 优化点：

• 使用 GPU 共享内存缓存热点体素，减少全局显存访问。
• 体素哈希冲突处理：采用 链地址法 或 线性探测 解决哈希冲突。

---

3. 关键帧管理（Keyframe Selection）

在实时 SLAM 中，每帧数据都更新地图会导致 计算负担过大，因此 InfiniTAM 采用 关键帧策略 进行更新：

1. 位姿变化检测：
   $$[\Vert T_{curr}-T_{last}\Vert >{\theta }_t]$$

   • ( T_{curr} ) 为当前帧位姿，( T_{last} ) 为上次关键帧位姿。
   • 设定阈值 $({\theta }_t)$（如 5cm 平移或 5° 旋转）。

2. 信息增量判断：

   • 计算新深度图与现有地图的 信息熵变化，如果变化较大则选取关键帧。

✅ 优化点：

• 自适应关键帧选取：在动态场景中增加关键帧密度，在静态区域减少关键帧数量。

---

4. 体素淘汰（Voxel Pruning）

为了减少存储和计算开销，InfiniTAM 采用 体素淘汰（Pruning） 机制移除无用体素：

• 基于权重衰减清理：
  • 设定权重阈值 ( w_{min} )，若体素权重 ( w < w_{min} )，则移除该体素。
• 基于活跃度清理：
  • 若体素长时间未被更新（如 50 帧），则将其删除。

✅ 优化点：

• GPU 并行清理无效体素，提高效率。
• 动态调整淘汰阈值，保证稳定性。

---

5. 局部地图管理（Submap Management）

InfiniTAM 采用 局部子地图（Submap）+ 全局哈希存储 方式管理大规模地图：

1. 将地图划分为固定大小的子地图（如 8m × 8m × 8m）。
2. 只在 GPU 内存中维护当前活跃子地图（可动态切换）。
3. 历史子地图存储到 CPU 或磁盘，减少 GPU 占用。

✅ 优化点：

• 使用 GPU 动态内存分配（CUDA malloc/free） 管理活跃子地图。
• 启发式子地图切换策略，减少不必要的数据传输。

---

6. 多分辨率地图更新（Multi-Resolution Mapping）

为了在不同精度需求下高效更新地图，InfiniTAM 采用 多层级体素（Octree 或 Pyramidal TSDF） 进行更新：

• 低分辨率（如 4cm 体素）用于全局地图，加快远距离建图。
• 高分辨率（如 1cm 体素）用于局部细节，提高精度。

✅ 优化点：

• 局部高精度、全局低精度存储，减少计算量。
• 动态调整分辨率，根据传感器精度和计算资源优化存储。

---

7.总结

策略	优化目标	具体方法
TSDF 体素融合	平滑更新地图	权重衰减、自适应加权
哈希体素存储	减少内存占用	稀疏存储、GPU 共享内存
关键帧管理	降低计算量	位姿变化检测、信息熵计算
体素淘汰	清理无用数据	权重阈值 + 活跃度判断
局部地图管理	提高运行效率	GPU 维护活跃子地图
多分辨率更新	平衡精度和计算量	Octree / Pyramid TSDF

7. 适用场景

InfiniTAM 适用于：

• 实时 3D 重建
• AR/VR 交互
• 机器人导航

---

总结

模块	主要方法	优化策略
输入数据	RGB-D 传感器	预处理深度图、去噪
位姿估计	ICP、RGB-D 直接法	SVD 优化、IMU 约束
映射更新	TSDF 体素融合	稀疏存储、Octree
表面提取	Marching Cubes	GPU 并行优化

---

InfiniTAM 通过 TSDF+体素存储+并行优化 实现高效的 实时 3D SLAM。