Structure from Motion (SfM) 是从多张二维图像重建三维场景的核心技术，广泛应用于文化遗产保护、机器人导航、增强现实等领域。本章将深入探讨SfM的数学原理、算法实现以及工程优化，帮助读者掌握从图像序列到稠密3D网格的完整重建流程。学习目标包括：理解多视图几何基础、掌握特征匹配与相机标定技术、实现稠密重建算法、优化大规模场景重建。
SfM的核心是从二维图像中恢复三维结构和相机运动。考虑一个3D点 $\mathbf{X} = [X, Y, Z, 1]^T$ 在相机 $i$ 中的投影：
\[\mathbf{x}_i = P_i \mathbf{X} = K_i [R_i | \mathbf{t}_i] \mathbf{X}\]| 其中 $P_i$ 是 $3 \times 4$ 的投影矩阵，$K_i$ 是内参矩阵，$[R_i | \mathbf{t}_i]$ 是外参矩阵。 |
两视图之间的几何关系由基础矩阵 $F$ 描述，满足对极约束：
\[\mathbf{x}_2^T F \mathbf{x}_1 = 0\]基础矩阵可以分解为： \(F = K_2^{-T} E K_1^{-1}\)
其中 $E$ 是本质矩阵，编码了相机间的相对位姿： \(E = [\mathbf{t}]_\times R\)
给定两个视图中的对应点 $\mathbf{x}_1, \mathbf{x}_2$ 和投影矩阵 $P_1, P_2$，可以通过求解以下线性方程组重建3D点：
\[\begin{bmatrix} \mathbf{x}_1 \times P_1 \\ \mathbf{x}_2 \times P_2 \end{bmatrix} \mathbf{X} = 0\]使用SVD分解可得最小二乘解。
典型的增量式SfM包含以下步骤：
视图1 ←→ 视图2
↓ ↓
[特征匹配]
↓
[E矩阵估计]
↓
[三角化]
↓
3D点云
↓
[加入视图3]
↓
[PnP+BA]
↓
扩展点云
增量式方法：
全局式方法：
SIFT (Scale-Invariant Feature Transform)： 通过高斯差分金字塔检测关键点，计算梯度方向直方图作为描述子：
\(L(x,y,\sigma) = G(x,y,\sigma) * I(x,y)\) \(D(x,y,\sigma) = L(x,y,k\sigma) - L(x,y,\sigma)\)
ORB (Oriented FAST and Rotated BRIEF)： 结合FAST角点检测和BRIEF二进制描述子，计算效率高：
SuperPoint： 使用全卷积网络同时预测关键点和描述子：
D2-Net： 统一检测和描述，使用单个CNN同时完成两个任务： \(\mathbf{d}_{ij} = \frac{\mathbf{F}_{ij}}{\|\mathbf{F}_{ij}\|_2}\)
其中 $\mathbf{F}_{ij}$ 是位置 $(i,j)$ 的特征向量。
最近邻匹配： \(m_{ij} = \arg\min_{j} \|\mathbf{d}_i^1 - \mathbf{d}_j^2\|_2\)
Lowe’s ratio test： \(\frac{\|\mathbf{d}_i - \mathbf{d}_{j_1}\|}{\|\mathbf{d}_i - \mathbf{d}_{j_2}\|} < \tau\)
其中 $j_1, j_2$ 是最近和次近邻，典型 $\tau = 0.7-0.8$。
使用RANSAC估计基础矩阵并剔除外点：
输入：匹配点对 {(x₁ᵢ, x₂ᵢ)}
1. 随机采样8个点对
2. 计算基础矩阵F（8点算法）
3. 计算内点：|x₂ᵢᵀFx₁ᵢ| < ε
4. 重复N次，选择内点最多的模型
5. 使用所有内点重新估计F
构建图 $G = (V, E)$，其中：
边权重可定义为： \(w_{ij} = \frac{|\text{inliers}_{ij}|}{\min(|f_i|, |f_j|)}\)
针孔相机模型的内参矩阵： \(K = \begin{bmatrix} f_x & s & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}\)
考虑畸变的完整投影： \(\mathbf{x}_d = \text{distort}(\mathbf{x}_u, k_1, k_2, p_1, p_2, k_3)\)
径向畸变： \(r^2 = x_u^2 + y_u^2\) \(x_d = x_u(1 + k_1r^2 + k_2r^4 + k_3r^6)\)
切向畸变： \(x_d = x_u + 2p_1x_uy_u + p_2(r^2 + 2x_u^2)\)
张正友标定法： 使用平面棋盘格，通过单应性矩阵约束求解： \(H = K[r_1 \; r_2 \; \mathbf{t}]\)
约束条件： \(r_1^T r_2 = 0, \quad \|r_1\| = \|r_2\| = 1\)
自标定： 利用场景约束（如消失点、平行线）或运动约束估计内参。
BA通过最小化重投影误差联合优化所有参数：
\[\min_{\{P_i\}, \{X_j\}} \sum_{i,j} \rho(\|\mathbf{x}_{ij} - \pi(P_i, X_j)\|^2)\]其中：
利用雅可比矩阵的稀疏结构： \(J = \begin{bmatrix} \frac{\partial r_{11}}{\partial P_1} & 0 & \cdots & \frac{\partial r_{11}}{\partial X_1} & 0 & \cdots \\ 0 & \frac{\partial r_{22}}{\partial P_2} & \cdots & 0 & \frac{\partial r_{22}}{\partial X_2} & \cdots \\ \vdots & \vdots & \ddots & \vdots & \vdots & \ddots \end{bmatrix}\)
Schur补加速求解正规方程： \(\begin{bmatrix} U & W \\ W^T & V \end{bmatrix} \begin{bmatrix} \delta_c \\ \delta_p \end{bmatrix} = \begin{bmatrix} \epsilon_c \\ \epsilon_p \end{bmatrix}\)
先消去点参数： \((U - WV^{-1}W^T)\delta_c = \epsilon_c - WV^{-1}\epsilon_p\)
对于校正后的立体图像对，视差 $d$ 与深度 $Z$ 的关系： \(Z = \frac{f \cdot b}{d}\)
其中 $f$ 是焦距，$b$ 是基线长度。
匹配代价函数：
| SAD：$\sum_{(i,j) \in W} | I_1(i,j) - I_2(i-d,j) | $ |
PatchMatch算法：
光度一致性度量： \(C(p, d, \mathbf{n}) = \sum_{i \in \mathcal{V}} w_i \cdot \text{NCC}(p, \text{warp}(p, d, \mathbf{n}, i))\)
几何一致性检查： \(|d_1(p) - d_2(\text{proj}_{1→2}(p, d_1))| < \tau \cdot d_1(p)\)
置信度加权融合： \(d_{\text{fused}} = \frac{\sum_i c_i \cdot d_i}{\sum_i c_i}\)
MVSNet架构：
代价体构建： \(\mathcal{C}(d) = \text{Var}(\{\mathbf{F}_i(\text{warp}(p, d))\}_{i=1}^N)\)
分块策略：
并行化：
Truncated Signed Distance Function表示： \(\text{TSDF}(\mathbf{x}) = \min(\max(\text{SDF}(\mathbf{x}), -\delta), \delta)\)
增量式融合： \(D_{\text{new}}(\mathbf{x}) = \frac{W(\mathbf{x}) \cdot D(\mathbf{x}) + w_i \cdot d_i(\mathbf{x})}{W(\mathbf{x}) + w_i}\) \(W_{\text{new}}(\mathbf{x}) = W(\mathbf{x}) + w_i\)
深度图转点云： \(\mathbf{X} = K^{-1} \cdot d(u,v) \cdot [u, v, 1]^T\)
外点剔除：
将表面重建转化为泊松方程求解： \(\Delta \chi = \nabla \cdot \vec{V}\)
其中 $\vec{V}$ 是从点云法线构建的向量场，$\chi$ 是指示函数。
离散化后的线性系统： \(L\mathbf{x} = \mathbf{b}\)
使用多重网格方法加速求解。
步骤：
Ball Pivoting算法：
1. 选择种子三角形
2. 滚动球找到新顶点
3. 扩展三角形边界
4. 重复直到无法扩展
简化： 使用QEM（Quadric Error Metrics）： \(Q = \sum_{\mathbf{p} \in \text{planes}} \mathbf{p}\mathbf{p}^T\) \(\text{error}(\mathbf{v}) = \mathbf{v}^T Q \mathbf{v}\)
平滑： Laplacian平滑： \(\mathbf{v}_{\text{new}} = \mathbf{v} + \lambda \sum_{i \in \mathcal{N}(v)} w_i(\mathbf{v}_i - \mathbf{v})\)
孔洞填充：
视觉SLAM系统架构：
实时性优化：
结合语义分割的3D重建： \(E = E_{\text{photo}} + \lambda_{\text{sem}} E_{\text{semantic}} + \lambda_{\text{smooth}} E_{\text{smooth}}\)
语义一致性： 跨视图的语义标签传播和融合。
运动分割：
非刚性重建：
分层表示：
分布式处理：
本章系统介绍了基于SfM的3D重建技术，涵盖了从多视图几何理论到工程实现的完整流程。核心要点包括：
| 投影方程：$\mathbf{x} = K[R | \mathbf{t}]\mathbf{X}$ |
练习6.1：给定两个相机的内参矩阵 $K_1 = K_2 = \begin{bmatrix} 500 & 0 & 320 \ 0 & 500 & 240 \ 0 & 0 & 1 \end{bmatrix}$，相对旋转 $R$ 为绕Y轴旋转30度，平移 $\mathbf{t} = [1, 0, 0]^T$，计算本质矩阵 $E$ 和基础矩阵 $F$。
Hint：先计算旋转矩阵，然后使用 $E = [\mathbf{t}]_\times R$ 和 $F = K_2^{-T} E K_1^{-1}$。
练习6.2：实现8点算法估计基础矩阵。给定8组对应点，构建线性方程组 $A\mathbf{f} = 0$，其中 $\mathbf{f}$ 是 $F$ 的向量化形式。描述如何施加秩2约束。
Hint：使用SVD求解，然后对 $F$ 进行SVD分解并将最小奇异值置零。
练习6.3：推导RANSAC估计基础矩阵时，给定容错率 $\epsilon$ 和置信度 $p$，需要的最小迭代次数 $N$。
Hint：使用概率公式 $1 - p = (1 - (1-\epsilon)^8)^N$。
练习6.4：描述Schur补在Bundle Adjustment中的作用，解释为什么可以大幅提升计算效率。
Hint：考虑雅可比矩阵的稀疏结构和变量数量差异。
练习6.5：设计一个自适应的关键帧选择策略，考虑以下因素：基线长度、共视特征数量、跟踪质量。给出评分函数和阈值设定方法。
Hint：考虑多个因素的加权组合，使用归一化处理不同量纲。
练习6.6：比较PatchMatch和传统块匹配在MVS中的优劣，设计一个混合策略结合两者优点。
Hint：考虑不同场景特性和计算资源。
练习6.7：推导并实现一个鲁棒的深度图融合算法，处理：1) 噪声深度值，2) 遮挡边界，3) 动态物体。给出置信度计算和滤波策略。
Hint：结合几何一致性、光度一致性和时间一致性。
练习6.8：设计一个面向大规模场景（城市级别）的分布式SfM系统架构。讨论数据划分、通信协议、全局一致性保证等关键问题。
Hint：考虑地理划分、重叠区域处理、分层优化。