第 11 章运动恢复结构与 SLAM (Structure from Motion and SLAM)

核心结论

相机标定（§11.1）：内参 / 外参 / 畸变；消失点法标定；张正友标定法。
姿态估计（§11.2）：PnP 问题——已知 3D 点 + 2D 像素求相机姿态；DLT / EPnP / P3P / 迭代优化。
三角化（§11.3）：已知两视图 + 对应点 → 3D 点；DLT / 中点法。
SfM（§11.4）：从图像集合恢复相机姿态 + 3D 结构；增量 / 全局 / 分层方法。
SLAM（§11.5）：同时定位与地图构建；前端（特征 + 光流）+ 后端（BA / 图优化）+ 回环。
多视图几何（§11.6）：对极几何（两视图）/ 三焦张量（三视图）；基础矩阵 / 本质矩阵。

本章主旨

本章是"3D 重建章"——从图像恢复 3D 结构与相机姿态。SfM / SLAM 是 AR / 自动驾驶 / 机器人 / 街景的核心技术；理解后能看懂 ORB-SLAM / COLMAP / VINS-Mono 等开源框架。

一、核心概念

本章围绕 6 个核心概念展开：相机标定 → 姿态估计 → 三角化 → SfM → SLAM → 多视图几何。

概念	定义 + 重要性	实现提示
相机标定	内参 / 外参 / 畸变；消失点法 / 张正友法。	§11.1；SfM / SLAM 前置。
姿态估计 (PnP)	已知 3D + 2D 求相机姿态；DLT / EPnP / P3P / 迭代。	§11.2；AR / 机器人核心。
三角化	两视图 + 对应点 → 3D 点；DLT / 中点法。	§11.3；3D 重建基础。
SfM (Structure from Motion)	从图像集合恢复相机 + 3D；COLMAP / OpenMVG。	§11.4；离线 3D 重建。
SLAM	同时定位与地图构建；前端 + 后端 + 回环。	§11.5；ORB-SLAM / VINS-Mono。
多视图几何	对极几何 / 三焦张量；基础 / 本质矩阵。	§11.6；理解两视图 / 多视图约束。

概念

定义 + 重要性

实现提示

相机标定

内参 / 外参 / 畸变；消失点法 / 张正友法。

§11.1；SfM / SLAM 前置。

姿态估计 (PnP)

已知 3D + 2D 求相机姿态；DLT / EPnP / P3P / 迭代。

§11.2；AR / 机器人核心。

三角化

两视图 + 对应点 → 3D 点；DLT / 中点法。

§11.3；3D 重建基础。

SfM (Structure from Motion)

从图像集合恢复相机 + 3D；COLMAP / OpenMVG。

§11.4；离线 3D 重建。

SLAM

同时定位与地图构建；前端 + 后端 + 回环。

§11.5；ORB-SLAM / VINS-Mono。

多视图几何

对极几何 / 三焦张量；基础 / 本质矩阵。

§11.6；理解两视图 / 多视图约束。

二、详细笔记

2.1 相机标定 (Camera Calibration)

What：求相机内参（焦距 / 主点 / 畸变）+ 外参（旋转 / 平移）。

Why：SfM / SLAM / 3D 重建的前置；标定后才能用相机模型。

How：

标定方法（§11.1）：

消失点法：从场景中平行线消失点求焦距 / 主点。
张正友标定法（1999）：用棋盘格标定板；至少 3 张不同角度图像。
自标定：从自然图像（无标定板）求内参；难度大。

标定的工程经验

张正友法是工业标准；OpenCV cv2.calibrateCamera 实现。
棋盘格是关键——角点必须精确检测（亚像素）。
镜头畸变需标定（径向 + 切向）；用 cv2.undistort 校正。

When：SfM / SLAM；AR；3D 扫描；多视图几何。

Example：cv2.findChessboardCorners + cv2.calibrateCamera；Kalibr 工具标定 IMU-相机联合。

2.2 姿态估计 (Pose Estimation)

What：已知 3D 点 + 2D 像素对应，求相机姿态 (R, t)。

Why：AR（已知 3D 模型求相机姿态）；机器人（已知物体求抓取姿态）。

How：

PnP 算法（§11.2）：

DLT（直接线性变换）：线性化求解；外点敏感。
P3P（3 点解）：闭式解；4 个解（需第 4 点消歧）。
EPnP（2009）：高效 PnP；O(n) 复杂度。
迭代法（Levenberg-Marquardt）：精度最高；需初值。

PnP 的最小点数

3 点给 4 个解（可能歧义）。
4+ 点消歧；6+ 点适合迭代优化。
RANSAC + PnP 处理外点。

When：AR（SLAM / ARKit / ARCore）；机器人；SfM 增量。

Example：cv2.solvePnPRansac(obj_pts, img_pts, K, dist)；OpenCV EPnP 实现。

2.3 三角化 (Triangulation)

What：已知两视图姿态 + 对应点，求 3D 点坐标。

Why：SfM / SLAM 初始化 3D 地图。

How：

三角化方法（§11.3）：

中点法：两射线最近点（公垂线中点）；代数最简。
DLT：齐次线性求解；闭式。
迭代法：最小化重投影误差；精度最高。

三角化的精度依赖基线

基线（两相机距离）越大，三角化精度越高。
基线过短 → 深度不确定（"针孔模型退化"）。
远点 / 小基线 → 深度误差大。

When：SfM；SLAM 初始化；立体匹配。

Example：cv2.triangulatePoints(P1, P2, pts1, pts2)；OpenCV + COLMAP 三角化。

2.4 SfM (Structure from Motion)

What：从无序图像集合恢复相机姿态 + 3D 场景结构。

Why：3D 重建 / 街景 / 文物数字化 / 电影特效。

How：

SfM 流程（§11.4）：

特征提取 + 匹配：SIFT / ORB + 跨图匹配。
两视图几何：基础 / 本质矩阵 + RANSAC。
增量 SfM：添加新相机 + 三角化 + BA（COLMAP / OpenMVG）。
全局 SfM：所有相机同时估计（更鲁棒）。
分层 SfM：先聚类图像组，再合并。

SfM 的开源工具

COLMAP：SOTA；C++ 实现；GUI + 命令行。
OpenMVG：开源 C++ 库；学术常用。
AliceVision（Meshroom）：集成 SfM + MVS + 网格。

When：3D 重建；街景；文物数字化；AR 地图。

Example*：colmap automatic_reconstructor --workspace_path <dir>；手机 3D 扫描 APP（Polycam / Scaniverse）。

2.5 SLAM (Simultaneous Localization and Mapping)

What：实时估计相机姿态 + 增量构建 3D 地图；机器人和 AR 核心。

Why：自动驾驶 / AR / 机器人 / 无人机需要实时定位与建图。

How：

SLAM 框架（§11.5）：

前端：特征提取 + 匹配 + 姿态估计（视觉里程计 VO）。
后端：BA / 图优化（g2o / Ceres / GTSAM）。
回环：识别回到曾到过的位置；消除累积误差。
建图：稀疏 / 半稠密 / 稠密地图。
IMU 融合（VINS-Mono / ORB-SLAM3）：视觉 + IMU 提高鲁棒性。

经典 SLAM 系统：

Mono-SLAM（2007）：第一个实时单目 SLAM。
ORB-SLAM2 / 3（2017/2020）：特征点 SLAM SOTA；支持单 / 双 / RGB-D + IMU。
VINS-Mono（2017）：视觉 + IMU 紧耦合。
LSD-SLAM（2014）：半稠密直接法。
NeRF-SLAM（2023+）：神经隐式表示 SLAM。

视觉 SLAM vs 激光 SLAM

视觉 SLAM：便宜（单目相机）；纹理丰富；动态环境敏感。
激光 SLAM（LOAM / Cartographer）：精度高；不受光照影响；昂贵。
现代：多传感器融合（视觉 + 激光 + IMU + GPS）。

When：自动驾驶；AR；机器人；无人机；室内导航。

Example*：ORB_SLAM3 仓库；iPhone ARKit；自动驾驶 Apollo。

2.6 多视图几何 (Multi-View Geometry)

What：两视图 / 三视图之间的几何约束（基础 / 本质 / 三焦张量）。

Why：SfM / SLAM 的数学基础；理解后才能改进算法。

How：

对极几何（§11.6）：

\[\mathbf{x}_2^T F \mathbf{x}_1 = 0, \quad F = K^{-T} E K^{-1}, \quad E = [t]_\times R \end{bmatrix>\]

F：基础矩阵（像素级）；E：本质矩阵（相机坐标级）。
[t]×：t 的反对称矩阵。

三焦张量：三视图的几何约束；更复杂但更鲁棒。

多视图几何的工程应用

对极几何：SfM 初始化 / 三维重建 / 立体匹配。
三焦张量：三角测量 / 多视图 SfM 优化。
Bundle Adjustment：所有视图同时优化（§8.3）。

When：SfM / SLAM 数学基础；立体匹配；3D 重建。

Example*：cv2.findFundamentalMat / cv2.findEssentialMat；OpenCV + 多视图几何库。

三、关键图表

视觉图表

Figure 1. 图 11-1：相机标定棋盘格

Figure 2. 图 11-2：对极几何

Figure 3. 图 11-3：三角化示意

Figure 4. 图 11-4：SLAM 框架

非可视化条目

非可视化条目（表 / 算法）

编号

内容摘要

表 11.1

PnP 算法对比（DLT / P3P / EPnP / 迭代）。

表 11.2

SLAM 系统对比（ORB-SLAM / VINS-Mono / NeRF-SLAM）。

式 11-1 至 11-20

标定 / 对极几何 / PnP / 三角化公式。

列表 11.1-11.x

OpenCV 标定 / SfM / SLAM 示例。

核心公式对照表

概念

公式

本质矩阵

\(E = [t\)_\times R]

基础矩阵

\(F = K^{-T} E K^{-1}\)

对极约束

\(\mathbf{x}_2^T F \mathbf{x}_1 = 0\)

三角化（中点法）

\(\mathbf{X} = \frac{1}{2}(d_1 \mathbf{c}_1 + d_2 R_2^{-1} \mathbf{c}_2)\)

PnP（线性）

\(\min_{R,t} \sum_i | \pi(R X_i + t) - x_i |^2\)

四、思维导图

mindmap
  root((第 11 章 SfM与SLAM))
    相机标定
      消失点
      张正友
      畸变
    姿态估计
      PnP
      P3P
      EPnP
    三角化
      中点法
      DLT
    SfM
      增量
      全局
      分层
    SLAM
      前端VO
      后端BA
      回环
    多视图几何
      对极
      三焦张量

五、重点与易错点

相机标定是 SfM / SLAM 前置：张正友法是工业标准；棋盘格必须精确。
PnP 至少 4 点消歧：3 点有 4 个解；用 RANSAC 处理外点。
三角化精度依赖基线：基线过短 → 深度不确定。
COLMAP 是 SfM 标配：开源 SOTA；支持增量 / 全局 / 分层。
ORB-SLAM 是视觉 SLAM SOTA：特征点法 + 回环 + BA。
VINS-Mono 是 VIO 标杆：视觉 + IMU 紧耦合。
BA 是 SLAM 后端核心：稀疏 BA 可高效求解大规模问题。
NeRF-SLAM 是新方向：神经隐式表示 SLAM。
跨章衔接：第 8 章 BA 是 SfM/SLAM 后端；第 7 章特征是前端；第 12 章深度估计可替代特征法。

第 11 章 运动恢复结构与 SLAM (Structure from Motion and SLAM)

核心结论

一、核心概念

二、详细笔记

2.1 相机标定 (Camera Calibration)

2.2 姿态估计 (Pose Estimation)

2.3 三角化 (Triangulation)

2.4 SfM (Structure from Motion)

2.5 SLAM (Simultaneous Localization and Mapping)

2.6 多视图几何 (Multi-View Geometry)

三、关键图表

视觉图表

非可视化条目

核心公式对照表

四、思维导图

五、重点与易错点

第 11 章运动恢复结构与 SLAM (Structure from Motion and SLAM)