SfM点云获取（SfM算法简析）

Overview

参考 Structure-from-Motion Revisited，在此简单阐述 SfM 算法的基本思路。

增量式 SfM 的处理过程主要包括：特征提取（Feature Extraction）、匹配（Matching）、几何验证（Geometric Verification）以及重建（Reconstruction）。经过前几个步骤，可以从匹配点中估计描述两台相机几何关系的本质矩阵（Essential Matrix）$E$（相机已标定）或基础矩阵（Fundamental Matrix）$F$（相机未标定）；随后逐步恢复相机位姿并三角测量出稀疏点云。

基础知识

射影几何

仿射几何是研究在仿射变换下保持不变的几何性质的学科。仿射变换包括平移、缩放、旋转和剪切等，这些变换保持直线的平行性和点的共线性，但不一定保持距离和角度。仿射几何是射影几何的一个子集：通过指定一条特殊的直线（二维情况）或平面（三维情况）作为无穷远直线或无穷远平面，可以从射影几何中得到仿射几何。

在射影平面中，一条过原点的三维射线对应一个二维射影点；一个过原点的三维平面对应一条二维射影直线。这样便能把普通点和无穷远点放到同一个代数框架中处理。

齐次坐标系

齐次坐标是射影几何中的一个重要工具，用于表示点和直线。在二维情况下，一个普通点 $(x,y)$ 可以写成齐次坐标

$$\mathbf{x}=(x,y,1)^\mathsf{T}.$$

更一般地，齐次点写成 $(x,y,w)^\mathsf{T}$。对于任意非零常数 $k$，

$$(x,y,w)^\mathsf{T}\sim(kx,ky,kw)^\mathsf{T},\qquad k\ne 0,$$

二者表示同一个点。当 $w\ne 0$ 时，对应的欧氏坐标为 $(x/w,y/w)$；当 $w=0$ 时，该点位于无穷远处。

一条直线可以写成

$$ax+by+c=0,$$

其齐次坐标为 $\mathbf{l}=(a,b,c)^\mathsf{T}$。同样，$\mathbf{l}$ 与 $k\mathbf{l}$ 表示同一条直线。点 $\mathbf{x}$ 位于直线 $\mathbf{l}$ 上的充要条件为

$$\mathbf{x}^\mathsf{T}\mathbf{l}=0.$$

因此，点和直线都只有两个自由度：点由 $x,y$ 确定；直线的三个参数 $a,b,c$ 只在比例意义下有效。

从射影几何进一步过渡到欧氏几何时，还需要指定无穷远处的特殊几何对象。在二维射影平面中，可以在无穷远直线上指定一对复值虚圆点，它们满足

$$x^2+y^2=0,\qquad w=0.$$

在三维射影空间中，对应的对象是位于无穷远平面上的绝对二次曲线，其点满足

$$X^2+Y^2+Z^2=0,\qquad T=0.$$

指定这些对象后，角度和长度比等欧氏性质才有明确意义；这些概念在仿射几何中并没有被保留。

摄像机模型

基本针孔模型

考虑图像平面（或者聚焦平面）$Z=f$。在针孔摄像机模型下，空间坐标为 $X=(X,Y,Z)^\mathsf{T}$ 的点被映射到图像平面上的一点。该像点是连接空间点 $X$ 与投影中心的直线同图像平面的交点。由相似三角形可得

$$(X,Y,Z)^\mathsf{T}\mapsto(fX/Z,fY/Z)^\mathsf{T}.$$

也就是说，物点、投影中心和像点共线；深度 $Z$ 越大，投影后的尺度越小。接下来用齐次坐标表示中心投影：

$$\begin{pmatrix} X\\Y\\Z\\1 \end{pmatrix} \mapsto \begin{pmatrix} fX\\fY\\Z \end{pmatrix} = \begin{bmatrix} f&0&0&0\\ 0&f&0&0\\ 0&0&1&0 \end{bmatrix} \begin{pmatrix} X\\Y\\Z\\1 \end{pmatrix}.$$

把三维点和二维像点都写成齐次坐标后，投影关系可以统一写成

$$\mathbf{x}\sim\mathbf{P}\mathbf{X},$$

其中 $\mathbf{P}$ 是 $3\times4$ 的投影矩阵，$\sim$ 表示等比例相等。

若摄像机主点存在偏置，且横、纵方向焦距可能不同，则相机标定矩阵可以写成

$$\mathbf{K}= \begin{bmatrix} f_x&s&p_x\\ 0&f_y&p_y\\ 0&0&1 \end{bmatrix},$$

其中 $f_x,f_y$ 是两个方向上的焦距，$(p_x,p_y)$ 是主点，$s$ 是坐标轴倾斜参数。在本文使用的简化模型中，可以令 $f_x=f_y=f$ 且 $s=0$。

世界坐标系中的点 $\overline{\mathbf{X}}$ 转换到相机坐标系时，有

$$\overline{\mathbf{X}}_{\mathrm{cam}} =\mathbf{R}\left(\overline{\mathbf{X}}-\overline{\mathbf{C}}\right),$$

其中 $\mathbf{R}$ 表示旋转，$\overline{\mathbf{C}}$ 表示相机中心在世界坐标系中的位置。写成齐次形式便是

$$\mathbf{X}_{\mathrm{cam}}= \begin{bmatrix} \mathbf{R}&-\mathbf{R}\overline{\mathbf{C}}\\ \mathbf{0}^\mathsf{T}&1 \end{bmatrix} \mathbf{X}.$$

结合针孔成像模型，可以得到完整的投影关系

$$\mathbf{x} \sim\mathbf{K}\mathbf{R} \begin{bmatrix} \mathbf{I}&-\overline{\mathbf{C}} \end{bmatrix} \mathbf{X} =\mathbf{K} \begin{bmatrix} \mathbf{R}&\mathbf{t} \end{bmatrix} \mathbf{X},$$

其中 $\mathbf{t}=-\mathbf{R}\overline{\mathbf{C}}$，因此

$$\mathbf{P}=\mathbf{K} \begin{bmatrix} \mathbf{R}&\mathbf{t} \end{bmatrix}.$$

由此便不难理解相机内参与外参的含义：

• 内参：包含在矩阵 $\mathbf{K}$ 中，描述焦距、主点和坐标轴倾斜等相机内部参数。
• 外参：包含旋转矩阵 $\mathbf{R}$ 和平移向量 $\mathbf{t}$（或相机中心 $\overline{\mathbf{C}}$），描述相机在世界坐标系中的位置和姿态。

在本文采用的简化参数化下，投影模型由 9 个自由度组成：相机内部参数 3 个、旋转 3 个、相机中心位置 3 个。

若仅考虑二维相似变换，可以写成

$$\begin{pmatrix} x'\\y'\\1 \end{pmatrix} = \begin{bmatrix} \varepsilon\cos\theta&-\varepsilon\sin\theta&t_x\\ \varepsilon\sin\theta&\varepsilon\cos\theta&t_y\\ 0&0&1 \end{bmatrix} \begin{pmatrix} x\\y\\1 \end{pmatrix},$$

其中 $\varepsilon$、$\theta$ 和 $(t_x,t_y)$ 分别表示统一尺度、旋转角和平移。

两视图几何

两视图几何是计算机视觉和摄影测量学中的一个核心概念，研究的是从两个不同视角（即两幅图像）观察同一场景时，图像之间的几何关系。这种几何关系可以帮助我们理解场景的三维结构、摄像机的运动以及图像中对应点的位置约束。

考虑到 SfM 的本质便是从不断运动的相机所获得的图像中反求出相机内参（主点偏移、焦距）与外参（相机旋转与平移变换），进而反求出感兴趣点的稀疏坐标，研究两幅图像观察同一场景时的几何关系便极为重要。本节主要介绍基础矩阵（Fundamental Matrix）。

本质上，两幅视图之间的对极几何是图像平面与以基线为轴的平面束相交所形成的几何。基线是连接两个摄像机中心 $C$ 和 $C'$ 的直线；任意空间点 $X$ 与两个相机中心共同确定一个对极平面。这个平面分别与两幅图像相交，得到一对极线 $\mathbf{l}$ 和 $\mathbf{l}'$。基线与两幅图像平面的交点称为极点 $\mathbf{e}$ 和 $\mathbf{e}'$。

基础矩阵 F 的定义

• 对极几何：在两幅图像中，第一幅图像中的点 $\mathbf{x}$ 在第二幅图像中的对应点 $\mathbf{x}'$ 必须位于一条称为**极线（Epipolar Line）**的直线上。

给定第一幅图像中的点 $\mathbf{x}$，基础矩阵把它映射为第二幅图像中的极线：

$$\mathbf{l}'=\mathbf{F}\mathbf{x}.$$

由于对应点 $\mathbf{x}'$ 位于 $\mathbf{l}'$ 上，因此满足对极约束

$$\mathbf{x}'^\mathsf{T}\mathbf{F}\mathbf{x}=0.$$

为了理解 $\mathbf{F}$ 的构造，可以考虑一个不经过两个相机中心的平面 $\pi$。空间点 $X$ 在两幅图像中的投影分别为 $\mathbf{x}$ 和 $\mathbf{x}'$。如果先让 $\mathbf{x}$ 通过平面 $\pi$ 转移到第二幅图像中，可以得到

$$\mathbf{x}'=\mathbf{H}_{\pi}\mathbf{x},$$

其中 $\mathbf{H}_{\pi}$ 是由平面 $\pi$ 诱导的单应性矩阵。连接 $\mathbf{x}'$ 与第二幅图像的极点 $\mathbf{e}'$，便得到对应极线：

$$\mathbf{l}' =\mathbf{e}'\times\mathbf{x}' =[\mathbf{e}']_{\times}\mathbf{x}' =[\mathbf{e}']_{\times}\mathbf{H}_{\pi}\mathbf{x}.$$

因此基础矩阵可以表示为

$$\mathbf{F}=[\mathbf{e}']_{\times}\mathbf{H}_{\pi},$$

其中 $[\mathbf{e}']_{\times}$ 是由极点 $\mathbf{e}'$ 构造的反对称矩阵。基础矩阵 $\mathbf{F}$ 描述了一个点在第一幅图像中如何映射到第二幅图像中的极线。

对于已经标定的相机，可以在归一化相机坐标下使用本质矩阵 $\mathbf{E}$。它与基础矩阵之间的关系为

$$\mathbf{E}=\mathbf{K}'^\mathsf{T}\mathbf{F}\mathbf{K},$$

对应点仍满足

$$\mathbf{x}'^\mathsf{T}\mathbf{F}\mathbf{x}=0.$$

本质矩阵和基础矩阵都是 $3\times3$ 矩阵，秩为 2。二者的区别在于，本质矩阵作用于归一化坐标并依赖已知内参，而基础矩阵直接作用于像素齐次坐标。

为什么需要极线几何

单应性矩阵 $\mathbf{H}$ 与极线几何都能描述图像之间的点对应关系，但适用条件不同：

特性	单应性矩阵 $\mathbf{H}$	极线几何（$\mathbf{E}$ 或 $\mathbf{F}$）
适用场景	平面场景或摄像机纯旋转	任意三维场景
运动估计	可以估计旋转和平移，但需要额外约束	可以估计相对旋转和平移
点对应关系	仅适用于平面场景或纯旋转	适用于任意三维场景
计算复杂度	较低	较高
应用范围	图像拼接、平面场景重建	立体视觉、三维重建、摄像机运动估计

在非平面场景中，极线几何可以更好地描述点对应关系，因为它不依赖平面假设：

• 极线约束：通过极线约束描述点对应关系，适用于一般三维场景。
• 深度变化：可以处理点的深度变化，从而描述透视效应。
• 运动估计：可以估计摄像机的相对旋转和平移，适用于非平面场景。

特征提取过程

在特征提取阶段，SfM 系统会从每张图像中提取出一组局部特征。这些特征通常是图像中的关键点（如角点、边缘等），并且每个关键点都有一个描述符（Descriptor），用于表示该点的外观信息。描述符的设计需要具有对光照、旋转、尺度等变化的鲁棒性，以便在不同图像中能够识别出相同的特征点。

• 特征表示：对于每张图像 $I_i$，系统会检测出一组局部特征

  $$\mathcal{F}_i=\{(\mathbf{x}_j,\mathbf{f}_j)\},$$

  其中 $\mathbf{x}_j$ 是特征点的位置（二维坐标），$\mathbf{f}_j$ 是该特征点的描述符。

• 特征类型：常用的特征提取算法包括 SIFT（Scale-Invariant Feature Transform）及其变体，以及近年来基于学习的特征提取方法。SIFT 特征因其对光照和几何变化的鲁棒性而被广泛使用。

SIFT 算法

SIFT（Scale-Invariant Feature Transform，尺度不变特征变换）是一种用于图像处理和计算机视觉的特征检测算法。它由 David Lowe 在 1999 年提出，并在 2004 年进一步完善。SIFT 算法的主要优势在于其对图像的尺度、旋转、亮度变化等具有不变性，能够在不同的图像条件下稳定地检测和匹配特征点。

1. 尺度空间极值检测

   • SIFT 算法首先通过构建高斯金字塔来检测图像中的关键点。高斯金字塔是通过对图像进行不同程度的模糊（高斯滤波）和下采样得到的。
   • 在高斯金字塔的基础上，SIFT 算法进一步构建差分高斯金字塔（DoG，Difference of Gaussians），通过计算相邻高斯图像的差值来检测局部极值点。这些极值点是潜在的关键点。

2. 关键点定位

   • 在检测到极值点后，SIFT 算法通过拟合三维二次函数来精确定位关键点的位置和尺度。同时，算法会去除低对比度的点和不稳定的边缘响应点，以提高关键点的稳定性。

3. 方向分配

   • 为了使特征具有旋转不变性，SIFT 算法为每个关键点分配一个或多个方向。这是通过计算关键点周围区域的梯度方向直方图来实现的，直方图中的峰值对应关键点的主方向。

4. 关键点描述

   • 最后，SIFT 算法为每个关键点生成一个描述符。描述符基于关键点周围区域的梯度信息计算得到，通常是一个 128 维向量。这个描述符能够捕捉关键点周围的局部图像特征，并且对尺度、旋转和亮度变化具有较强的鲁棒性。

匹配过程

在匹配阶段，系统会通过比较图像的特征描述符，寻找不同图像之间的对应关系。具体来说，系统会尝试找到图像 $I_a$ 和图像 $I_b$ 之间的特征点匹配，即找到 $I_a$ 中某个特征点在 $I_b$ 中的对应点。

• 匹配策略：最简单的方法是暴力匹配（Brute-force Matching），即对每对图像的特征进行两两比较。然而，这种方法在大规模图像集合中计算复杂度很高，因此不适用于大型数据集。

• 高效匹配：为了提高效率，可以使用基于词汇树（Vocabulary Tree）的匹配、近似最近邻搜索（Approximate Nearest Neighbor，ANN）等方法。这些方法通过减少需要比较的图像对或描述符数量，加速特征匹配过程。

• 输出：匹配阶段的输出是一组潜在的图像对

  $$\mathcal{C}=\{(I_a,I_b)\mid I_a,I_b\in\mathcal{I},\ a<b\},$$

  以及每个图像对之间的特征点对应关系

  $$\mathcal{M}_{ab}\subseteq\mathcal{F}_a\times\mathcal{F}_b.$$

描述符相近只说明两个局部区域在外观上相似，并不能保证它们真的是同一个空间点。因此，匹配结果还需要经过几何验证。

几何验证

在几何验证阶段，系统会对匹配阶段得到的潜在图像对进行几何验证，以确保这些匹配点确实对应同一个三维场景点。由于匹配阶段仅基于外观信息，可能会出现误匹配，例如两个特征点虽然外观相似，但实际上对应不同的三维点。几何验证通过估计图像对之间的几何变换来过滤这些误匹配。

几何验证的核心是估计图像对之间的几何变换模型。根据估计的几何模型，将所有匹配点对分类为内点（Inliers）和外点（Outliers）。内点是符合几何模型的匹配点对，外点则是误匹配。

选择几何模型

根据图像对的几何关系，选择合适的几何变换模型来描述图像之间的对应关系。常见的几何模型包括：

• 单应性矩阵（Homography）：适用于摄像机纯旋转或拍摄平面场景的情况。
• 基础矩阵（Fundamental Matrix）：适用于一般场景中的运动相机。
• 本质矩阵（Essential Matrix）：适用于已经标定的相机。

估计几何变换

使用鲁棒估计方法（如 RANSAC）来估计图像对之间的几何变换模型。RANSAC（Random Sample Consensus）是一种迭代算法，能够从包含大量误匹配的数据中估计出正确的几何模型。

RANSAC 的基本步骤如下：

1. 随机采样：从匹配点对中随机选择最小数量的点。对于基础矩阵，常见的最小解法需要 7 个点，线性求解也常使用 8 点法。
2. 模型估计：使用这些点估计几何变换模型，如基础矩阵。
3. 内点检测：计算所有匹配点对在该模型下的误差，将误差小于阈值的点标记为内点。
4. 迭代选择：重复上述过程多次，选择内点数量最多、误差较小的模型作为最终估计。

几何验证的输出是一组经过验证的图像对 $\mathcal{C}$，以及它们之间的内点对应关系 $\mathcal{M}_{ab}$。这些内点对应关系构成了场景图（Scene Graph），其中图像是节点，经过验证的图像对是边。

重建

经过上述操作，我们可以得到相机几何模型。根据前面讲述的相机模型，世界点到相机坐标系的变换为

$$\mathbf{X}_{\mathrm{cam}}= \begin{bmatrix} \mathbf{R}&-\mathbf{R}\overline{\mathbf{C}}\\ \mathbf{0}^\mathsf{T}&1 \end{bmatrix} \mathbf{X}.$$

增量式 SfM 首先选择一对几何关系稳定、具有足够内点且基线合适的初始图像，恢复两台相机的相对位姿，并由匹配点初始化三维结构。随后不断注册新的图像、三角测量新的空间点，再对当前模型进行优化。

三角测量

给定两幅图像的相机位姿 $(\mathbf{R}_1,\mathbf{t}_1)$、$(\mathbf{R}_2,\mathbf{t}_2)$，以及匹配点 $\mathbf{x}_1$ 和 $\mathbf{x}_2$，可以通过三角测量计算对应三维点 $\mathbf{X}$：

$$\mathbf{x}_1\sim \mathbf{K}_1 \begin{bmatrix} \mathbf{R}_1&\mathbf{t}_1 \end{bmatrix} \mathbf{X}, \qquad \mathbf{x}_2\sim \mathbf{K}_2 \begin{bmatrix} \mathbf{R}_2&\mathbf{t}_2 \end{bmatrix} \mathbf{X}.$$

理想情况下，两条由相机中心和像点形成的反投影射线相交于同一个空间点；实际观测中存在噪声，因此通常把上式整理成齐次线性方程，再通过最小二乘方法求解 $\mathbf{X}$。

捆绑调整

捆绑调整（Bundle Adjustment）是一个非线性优化问题，目标是同时优化相机参数和三维点位置，使所有观测的重投影误差最小：

$$E=\sum_{i,j} \left\| \mathbf{x}_{ij}- \pi\!\left( \mathbf{K}_i \begin{bmatrix} \mathbf{R}_i&\mathbf{t}_i \end{bmatrix} \mathbf{X}_j \right) \right\|_2^2,$$

其中：

• $\mathbf{x}_{ij}$ 是第 $j$ 个三维点在第 $i$ 幅图像中的观测值。
• $\pi(\cdot)$ 是投影函数，把三维点投影到二维图像平面。
• $\mathbf{K}_i,\mathbf{R}_i,\mathbf{t}_i$ 是第 $i$ 台相机的内参与外参。
• $\mathbf{X}_j$ 是第 $j$ 个三维点。

该非线性最小二乘问题通常可以使用 Levenberg–Marquardt 等迭代方法求解。随着新图像不断加入，系统会交替进行图像注册、三角测量、异常观测过滤和局部或全局捆绑调整，逐步得到稳定的相机位姿与稀疏点云。

以上便是 SfM 算法的核心内容，其本质便是完成从连续变化的相机视图中，还原出现实世界欧氏空间下感兴趣物体的稀疏点云集合。

参考文献

1. Johannes L. Schönberger, Jan-Michael Frahm. Structure-from-Motion Revisited. CVPR, 2016.
2. David G. Lowe. Distinctive Image Features from Scale-Invariant Keypoints. International Journal of Computer Vision, 2004.