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
3
D
=
K
−
1
⋅
(
u
,
v
,
d
,
1
)
T
]
[ P_{3D} = K^{-1} \cdot (u, v, d, 1)^T ]
[P3D=K−1⋅(u,v,d,1)T]
其中：
- ( K ) 为相机内参矩阵。
- ( (u, v) ) 是像素坐标，( d ) 是深度值。
- ( P 3 D ) ( P_{3D} ) (P3D) 为计算出的三维坐标。
2. 位姿估计（Tracking）
位姿估计的目标是 计算当前帧相机相对于前一帧的运动，常用方法包括 ICP（迭代最近点） 和 RGB-D 直接法。
(1) 迭代最近点（ICP）方法
目标：最小化帧间对应点的欧氏距离：
[
E
(
R
,
t
)
=
∑
i
∥
R
p
i
+
t
−
q
i
∥
2
]
[ E(R, t) = \sum_{i} \| R p_i + t - q_i \|^2 ]
[E(R,t)=i∑∥Rpi+t−qi∥2]
其中：
- ( p i ) ( p_i ) (pi) 是当前帧点云的 3D 点。
- ( q i ) ( q_i ) (qi) 是上一帧匹配的 3D 点。
- ( R , t ) ( R, t ) (R,t) 是旋转和平移矩阵。
求解方法：
- 寻找对应点：使用最近邻搜索找到每个点 ( p_i ) 在上一帧中的最近点 ( q_i )。
- 计算变换矩阵：
- 计算 质心对齐 后的协方差矩阵 ( H )：
[ H = ∑ ( p i − p ˉ ) ( q i − q ˉ ) T ] [ H = \sum (p_i - \bar{p}) (q_i - \bar{q})^T ] [H=∑(pi−pˉ)(qi−qˉ)T] - 通过 SVD 分解 求解旋转矩阵 ( R )：
[ U , Σ , V T = SVD ( H ) , R = V U T ] [ U, \Sigma, V^T = \text{SVD}(H), \quad R = V U^T ] [U,Σ,VT=SVD(H),R=VUT] - 计算平移矩阵 ( t )：
[ t = q ˉ − R p ˉ ] [ t = \bar{q} - R \bar{p} ] [t=qˉ−Rpˉ]
- 计算 质心对齐 后的协方差矩阵 ( H )：
- 更新位姿，迭代收敛。
(2) RGB-D 直接法
利用图像梯度信息优化位姿：
[ E ( R , t ) = ∑ i ∥ I t ( u i ) − I t − 1 ( R u i + t ) ∥ 2 ] [ E(R, t) = \sum_{i} \| I_t(u_i) - I_{t-1}(R u_i + t) \|^2 ] [E(R,t)=i∑∥It(ui)−It−1(Rui+t)∥2]
- 适用于 纹理丰富的环境。
- 计算代价较高，但适用于无几何特征的场景。
(3) 运动约束
为了提高鲁棒性，可引入 IMU 数据 或 运动模型 作为额外约束，避免漂移。
3. 映射更新（Mapping）
映射模块的目标是构建和维护 3D 环境模型，InfiniTAM 采用 TSDF（截断签名距离函数） 进行建模。
(1) TSDF 体素表示
核心思想：使用体素网格（Voxel Grid）存储 3D 空间信息，每个体素包含：
- TSDF 值：表示体素到最近表面的有符号距离。
- 权重值：用于加权融合多个观测。
TSDF 计算公式：
[
Ψ
(
p
)
=
{
d
(
p
)
,
if
d
(
p
)
≤
δ
truncate
,
otherwise
]
[ \Psi(p) = \begin{cases} d(p), & \text{if } d(p) \leq \delta \\ \text{truncate}, & \text{otherwise} \end{cases} ]
[Ψ(p)={d(p),truncate,if d(p)≤δotherwise]
其中：
- ( d ( p ) ) ( d(p) ) (d(p)) 是点 ( p ) 到最近表面的距离。
- ( δ ) ( \delta ) (δ) 是截断范围（如 5cm）。
(2) 体素融合
当新帧进入时，需要更新 TSDF 体素：
[
Ψ
n
e
w
=
w
p
r
e
v
Ψ
p
r
e
v
+
w
n
e
w
Ψ
o
b
s
w
p
r
e
v
+
w
n
e
w
]
[ \Psi_{new} = \frac{w_{prev} \Psi_{prev} + w_{new} \Psi_{obs}}{w_{prev} + w_{new}} ]
[Ψnew=wprev+wnewwprevΨprev+wnewΨobs]
其中：
- ( Ψ p r e v ) ( \Psi_{prev} ) (Ψprev) 是之前存储的 TSDF 值。
- ( Ψ o b s ) ( \Psi_{obs} ) (Ψobs) 是新观测 TSDF 值。
- ( w p r e v , w n e w ) ( w_{prev}, w_{new} ) (wprev,wnew) 是对应的加权值。
(3) 体素存储优化
InfiniTAM 采用 多级体素存储（Hashing + Octree）：
- 稀疏体素存储（Voxel Hashing）：仅存储激活体素，减少内存消耗。
- 分层体素存储（Octree）：在不同分辨率下存储体素，提高计算效率。
4. 表面提取（Rendering）
为了可视化 3D 地图，InfiniTAM 采用 Marching Cubes 算法 从 TSDF 体素生成网格模型。
(1) Marching Cubes 网格提取
- 体素扫描：遍历 TSDF 体素网格，找到 TSDF 值过零的区域（表面区域）。
- 插值计算：在 TSDF 过零点进行线性插值，计算表面点位置：
[ v = p 1 ⋅ ∣ Ψ 2 ∣ + p 2 ⋅ ∣ Ψ 1 ∣ ∣ Ψ 1 ∣ + ∣ Ψ 2 ∣ ] [ v = \frac{p_1 \cdot |\Psi_2| + p_2 \cdot |\Psi_1|}{|\Psi_1| + |\Psi_2|} ] [v=∣Ψ1∣+∣Ψ2∣p1⋅∣Ψ2∣+p2⋅∣Ψ1∣] - 查表生成三角面片：根据查表法构建三角形网格。
(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 值和一个融合权重：
[
Ψ
(
p
)
=
{
d
(
p
)
,
if
d
(
p
)
≤
δ
truncate
,
otherwise
]
[ \Psi(p) = \begin{cases} d(p), & \text{if } d(p) \leq \delta \\ \text{truncate}, & \text{otherwise} \end{cases} ]
[Ψ(p)={d(p),truncate,if d(p)≤δotherwise]
其中：
- ( d ( p ) ) ( d(p) ) (d(p)) 是点 ( p ) 到最近表面的距离。
- ( δ ) ( \delta ) (δ) 是截断范围（一般取 5cm）。
融合策略：
当新帧进入时，更新 TSDF,同上描述
✅ 优化点：
- 新观测权重衰减（Weight Decay）：防止旧数据被快速遗忘，提升稳定性。
- 深度自适应加权：根据深度置信度调整融合权重，减少远距离误差。
2. 体素存储优化（Voxel Storage）
由于 TSDF 需要存储大量体素，InfiniTAM 采用 哈希体素存储（Voxel Hashing） 进行优化。
(1) 体素哈希存储
InfiniTAM 使用 哈希表 仅存储有效体素：
-
哈希索引计算：
[ h ( x , y , z ) = ( x ⋅ p 1 + y ⋅ p 2 + z ⋅ p 3 ) m o d N ] [ h(x, y, z) = (x \cdot p_1 + y \cdot p_2 + z \cdot p_3) \mod N ] [h(x,y,z)=(x⋅p1+y⋅p2+z⋅p3)modN]
其中 ( p_1, p_2, p_3 ) 为素数，( N ) 为哈希表大小。 -
仅存储 TSDF ≠ 0 的体素，减少内存占用。
✅ 优化点：
- 使用 GPU 共享内存缓存热点体素，减少全局显存访问。
- 体素哈希冲突处理：采用 链地址法 或 线性探测 解决哈希冲突。
3. 关键帧管理（Keyframe Selection）
在实时 SLAM 中，每帧数据都更新地图会导致 计算负担过大，因此 InfiniTAM 采用 关键帧策略 进行更新：
-
位姿变化检测：
[ ∥ T c u r r − T l a s t ∥ > θ t ] [ \| T_{curr} - T_{last} \| > \theta_t ] [∥Tcurr−Tlast∥>θt]- ( T_{curr} ) 为当前帧位姿，( T_{last} ) 为上次关键帧位姿。
- 设定阈值 ( θ t ) ( \theta_t ) (θt)（如 5cm 平移或 5° 旋转）。
-
信息增量判断：
- 计算新深度图与现有地图的 信息熵变化，如果变化较大则选取关键帧。
✅ 优化点：
- 自适应关键帧选取：在动态场景中增加关键帧密度，在静态区域减少关键帧数量。
4. 体素淘汰（Voxel Pruning）
为了减少存储和计算开销，InfiniTAM 采用 体素淘汰（Pruning） 机制移除无用体素：
- 基于权重衰减清理：
- 设定权重阈值 ( w_{min} )，若体素权重 ( w < w_{min} )，则移除该体素。
- 基于活跃度清理：
- 若体素长时间未被更新（如 50 帧），则将其删除。
✅ 优化点：
- GPU 并行清理无效体素，提高效率。
- 动态调整淘汰阈值，保证稳定性。
5. 局部地图管理（Submap Management）
InfiniTAM 采用 局部子地图（Submap）+ 全局哈希存储 方式管理大规模地图：
- 将地图划分为固定大小的子地图（如 8m × 8m × 8m）。
- 只在 GPU 内存中维护当前活跃子地图（可动态切换）。
- 历史子地图存储到 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。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
