NOTE
本篇文章属于一篇个人的学习笔记，包含很多数学推导与底层算法实现，为了能覆盖大多数会用到的知识点，许多地方也就一笔带过了。如果你想更深入学习这方面的知识，不妨查阅相关权威著作。
我们学校综设依赖 Matlab，很多算法全部封装好了，所以如果你只是单纯完成综设，这篇文章或许对你的帮助并不是很大，反而会觉得很痛苦（举个例子比如特征点检测，Matlab 直接一句 points = detectSURFFeatures(image);，但是本文考虑了怎样匹配，有什么优化方案等等）

一、数学基础#

多视角三维重建的目标是从二维图像恢复三维结构与相机运动，这一过程依赖精密的数学语言。本节会一并梳理后续各节反复使用的核心数学工具：射影几何与齐次坐标、SVD 与最小二乘、非线性最小二乘，以及李群与李代数。

NOTE
这一节比较抽象且枯燥，需要有一定的线性代数功底。可以将这部分作为留着，等到需要这部分数学知识再回来补（建议先把1.1看完再跳到下一节）

1.1 摄影几何与齐次坐标#

这部分简单提一下，本质是 CV 与 CG 的共通之处，CG 里的 MVP 矩阵变换与三维重建中的投影关系，都是射影变换（在齐次坐标下左乘一个可逆矩阵的变换）。

齐次坐标#

对于一个二维点 $(x,y)$ ，它的齐次坐标通常表示为 $\text{x}=(x,y,1)$ （或 $(kx,ky,k), k \ne 0$ ），三维点 $(X,Y,Z)$ 同理表示为 $(X,Y,Z,1)$ 。

任意齐次向量 $\mathbf{x} = (x_1,x_2,x_3)$ ，若 $x_3 \neq 0，$ 则对应欧氏点 $(x_1/x_3, x_2/x_3)$ ；若 $x_3 = 0，$ 则表示理想点（无穷远点），其方向由 $(x_1,x_2)$ 决定

射影变换#

二维射影变换就是将任意 $3\times3$ 的可逆矩阵 $\text{H}$ 左乘来得到的变换。这种变换包括欧式变换（刚体运动），相似变换（放大缩小）与仿射变换等等。

1.2 SVD 与最小二乘#

后面几乎每一节都会求解形如 $\text{Ax}=\text{b}$ 的超定方程组，而奇异值分解（SVD） 是数值上最稳定、几何含义最清晰的核心工具。

SVD分解#

任意实矩阵 $A \in \mathbb{R}^{m \times n}$ （ $m \geq n$ ）均可以进行奇异值分解（Singular Value Decomposition, SVD）：

A = U\Sigma V^T

其中：

$U \in \mathbb{R}^{m \times m}$ 和 $V \in \mathbb{R}^{n \times n}$ 为正交矩阵，U称为左奇异向量，V称为右奇异向量；
$\Sigma \in \mathbb{R}^{m \times n}$ 为矩形对角矩阵，其对角线上的元素：

\sigma_1 \geq \sigma_2 \geq \cdots \geq \sigma_n \geq 0

称为矩阵 $A$ 的奇异值（Singular Values）。

因此，SVD 将任意矩阵分解为三个部分：

$V^T$ 负责输入空间的旋转；
$\Sigma$ 负责沿各个方向进行缩放；
$U$ 负责输出空间的旋转。

奇异值 $\sigma_i$ 描述了矩阵在不同方向上的伸缩程度，也是衡量矩阵信息强弱的重要指标。

齐次最小二乘#

在多视图几何中（例如八点法求解基本矩阵、估计单应矩阵），我们经常需要求解 $Ax=b$ ，当方程数多于未知数时，通常没有精确解，这是我们需要寻找一个 $x$ ，使得误差的平方和最小：

\min_{\|x\|=1}\|Ax - b\|^2

这便是最小二乘解。回到我们的问题，我们的问题是齐次方程 $Ae=0$ ，显然 $e=0$ 是一个平凡解，但我们不想要它，因此我们加上约束 $\|e\|=1$ ，然后求解：

\boxed{\min_{\|x\|=1} \|Ae\|^2}

问题本质还是最小二乘思想：找一个单位向量 $e$ ，使得 $Ae$ 尽可能接近零向量

令 $e = V y$ ，因为 $V$ 是正交矩阵，所以：

\|e\| = \|y\| = 1

于是：

\|Ae\|^2 = \|U \Sigma V^T e\|^2 = \|U \Sigma V^T V y\|^2 = \|\Sigma y\|^2

因为 $\Sigma$ 的对角元素是降序排列的奇异值：

\sigma_1 \geq \sigma_2 \geq \cdots \geq \sigma_n

要使 $\|\Sigma y\|^2$ 最小，同时 $\|y\| = 1$ ，显然应该把能量全部放在最小奇异值对应的方向上：

y = \begin{bmatrix} 0 \\ 0 \\ \vdots \\ 0 \\ 1 \end{bmatrix}

所以：

e = V y

就是 $V$ 的最后一列。

因此：

e = V_\text{ 最后一列}

因此，齐次最小二乘问题的解为：

\boxed{x=V_{n}}

即矩阵 $V$ 的最后一列，也就是最小奇异值对应的右奇异向量。在双目重建（Two View Reconstruction）中，这一结论是从本质矩阵（Essential Matrix）恢复相机运动的核心步骤。

二、相机模型#

本节将研究如何将一个三维点唯一地映射到图像平面上。

2.1 针孔投影几何#

相关前置概念：

相机坐标系：坐标系原点在相机位置（该点也叫投影中心），Z 轴指向视线方向。
针孔投影（透视投影）：在相机坐标系中，想象一个相机模型，其小孔位于投影中心，成像平面位于Z轴负方向上，距离成像中心 $f$ （焦距），光线透过小孔落在成像平面上的过程就是透视投影。
主点：光轴与像平面的交点
像素焦距：对于相机屏幕，1mm对应的像素个数被称为像素焦距，记作 $f_x, f_y$

NOTE
上面的成像平面有一个数学理想化与物理事实的颠倒关系。一般情况下，其实为了方便公式推导，大家默认把成像平面放到相机的前方（即Z轴正方向上，物体和小孔之间），因为这样计算出来的图像是正的，公式不用带负号，而此时的成像平面也叫虚拟成像平面。但实际的物理情况是成像平面在小孔后，公式便会带负号。下面公式推导我们默认用虚拟成像平面

公式推导：画一个图，根据相似三角形，场景点 $X_c=(X_c, Y_c, Z_c)$ 在像平面的物理坐标为：

x=f\frac{X_c}{Z_c}, y=f\frac{Y_c}{Z_c}

该公式体现了近大远小的特征，这便是透视，跟 OpenGL 里的投影矩阵的数学本质几乎一致，都通过除以深度 z 产生透视效果。上面公式的所求 $x,y$ 是像平面坐标（物理坐标，单位通常mm），我们还需转换成像素坐标，上述公式实际一般写成：

u = f_x\frac{X}{Z} + c_x, \\ v = f_y\frac{Y}{Z} + c_y

这一步本质就是把相机坐标系中的点转换到像素坐标系（也就是我们看到的图像，原点一般在左上角），其中：

$f_x$ ：焦距（已换成像素）
$c_x$ ：主点在图片中的像素位置

NOTE
假设相机 $1 \text{mm} = 500 \text{pixel}$ ，如果计算得到的物理坐标 $x = 2 {mm}$ ，那么转换成像素坐标就是 $u = 2 \times 500 = f_xx=f_x$ ，这里 $f_x=f \times( \text{pixel/mm})$ 。由于图片的原点基本在左上角，所以还需加上主点坐标进行平移

2.2 内参矩阵与外参矩阵#

内参矩阵就是把相机成像平面上的物理坐标（mm）转换为图像上的像素坐标（pixel）的变换矩阵。把上一节最后的两个方程写成矩阵形式就是相机内参矩阵：

K= \begin{bmatrix} f_x & s & c_x\\ 0 & f_y & c_y\\ 0 & 0 & 1 \end{bmatrix}

通常情况下， $s=0$ 。为何叫内参，因为这些参数只跟相机本身有关（镜头焦距，传感器尺寸，像素大小，主点位置）。如果移动相机，发生了变化的是旋转矩阵R与平移向量t，它们统称为外参。至此我们可以得到 SFM 中最经典的投影公式：

\text{p}=K[R | t]\text{P}

其中：

$\text{P}$ ：世界空间中的三维点
$[R | t]$ ：将世界坐标变换到相机坐标（外参）
$K$ ：将相机坐标投影到像素坐标（内参）
$K[R|t]$ ：大名鼎鼎的投影矩阵 (3 x 4)
$p$ ：带尺度因子的像素坐标（不是真的像素坐标，必须还除以深度Z！不信你自己算 doge ）

2.3 镜头畸变#

镜头畸变（Lens Distortion）是 SFM 必须解决的第一个现实问题。产生原因：我们前面提到的公式都基于一个重要假设：所有光线都严格经过一个理想的小孔。但真实相机并不是小孔，而是由很多片镜片组成。因此，光线在进入相机时会发生折射，导致最终成像位置发生偏移，这种现象就叫 镜头畸变

径向畸变#

特点：偏离图像中心越远，畸变越严重。
原因：镜片边缘折射误差最大。

由于径向畸变关于光轴中心对称，只与半径 $r^2 = x^2 + y^2$ ，修正公式如下：

x_d=x(1 + k_1r^2 + k_2r^4 + k_3r^6)\\ y_d=y(1 + k_1r^2 + k_2r^4 + k_3r^6)

其中： $k_1, k_2, k_3$ 为径向畸变系数。根据具体畸变选择 $k_1$ 正负

切向畸变#

原因：镜头没有装正，稍微有点倾斜，导致不同方向上的放大倍率不同，数学表达如下：

\begin{align} x_d &= x + 2p_1xy + p_2(r^2 + 2x^2) \\ y_d &= y + p_1(r^2 + 2y^2) + 2p_2xy \end{align}

其中： $p_1,p_2$ 都是很小的切向畸变系数。OpenCV 默认采用Brown-Conrady Model（完整畸变模型，包含径向与切向畸变）。

IMPORTANT
这里详细讲解一下如何做标准棋盘格相机标定：
最重要的是要保证焦距恒定不变！, 物理相机可以很好地做到这一点，这里说说手机相机的情况。

用手机相机App的”专业模式/手动模式”(iPhone是”电影模式”里有手动对焦;安卓大部分自带相机有”专业”模式),在专业模式里选择 MF 模式，把对焦锁定在一个固定值。之后的标定照片和SFM数据全程都要用这个焦距，中途不要重新对焦。

标定照片与SFM照片必须分辨率一致！

关掉计算摄影类功能，比如人像模式，多帧HDR合成，夜间模式等等，这类非针孔图像处理会破坏针孔相机模型的假设

用程序生成一张标准棋盘格的图片，可以打印出来，也可以直接在电脑/平板上全屏打开（注意不要让系统做任何伸缩）

用一把物理尺子实测一个黑格子的边长(毫米),记下这个数字，后面标定会用到。

用锁定对焦/变焦的相机,对着屏幕上的棋盘格拍15~20张不同角度、不同距离,覆盖画面四角和中心（一定要尽可能让棋盘占满照片，四个角落都要拍到，因为我们需要边缘的镜头畸变样本）

最后的标定结果 RMS 重投影误差理想情况在 0.3 ~ 1.0 像素之间，1 ~ 1.5 基本可用。对于手机相机来说，1就已经可以了。

三、特征检测#

NOTE
第三章和第四章原理明白即可，不用深入数学底层，底层交给 OpenCV

这是三维重建的起点，它要求我们需要知道A图中的某个点对应B图中的哪个点，因此需要在图像中找一些容易识别、容易被再次找到的位置，这些位置就是特征

NOTE
下面提到的很多算法在 OpenCV 里面都已经包装好了，不用自己写。但是作为学习，还是有必要了解背后原理的。

3.1 好特征#

一个理想的特征点应该满足几个条件：

可重复性：同一个三维点在不同图像、不同光照、不同视角下最好都能被检测出来
判别性：特征点周围的图像块应足够独特，容易与其他点区分开来
数量充足且分布均匀

直观上来说，平坦区域与边缘的点都不能很好地满足上面的要求（平坦区域往任何方向移动变化都不大，边缘则只在垂直方向移动才有变化），角点却能很好地满足。

3.2 角点检测：Harris思想#

核心思想：用一个方形小窗口在图像上滑动，观察窗口内的亮度变化，如果窗口在角点上，往任何方向移动都会引起明显亮度变化。

我们定义一个函数 $E(u,v)$ ，表示：窗口移动 $(u,v)$ 后，亮度与原来相比变化有多大，数学上有如下近似：

E(u, v) \approx \sum_{\text{窗口}} w(x, y) \left[ I(x + u, y + v) - I(x, y) \right]^2

其中：

$I(x,y)$ ：某个像素移动前亮度
$I(x+u, y+v)$ ：某个像素移动后亮度
$w(x, y)$ ：权重，因为窗口中心比边缘更重要

接着对于图像函数 $I(x,y)$ 移动一点 $(u,v)$ 可用泰勒展开近似： $I(x+u,y+v)=I(x,y)+uI_x+vI_y$ ，这里的 $I_x$ 是 $x$ 方向的梯度。所以上式可展开为：

E(u, v) \approx \sum w(u^2 I_x^2 + 2uv I_x I_y + v^2 I_y^2)

又可整理为二次型：

\begin{aligned} E(u, v) &= \begin{bmatrix} u & v \end{bmatrix} M \begin{bmatrix} u \\ v \end{bmatrix} \\ \text{其中} \qquad M &= \sum w \begin{bmatrix} I_x^2 & I_x I_y \\ I_x I_y & I_y^2 \end{bmatrix} \end{aligned}

$M$ 便是Harris矩阵，对于任意二次型，最大的变化方向就是最大特征值的方向，同样，最小变化方向就是最小特征值方向。因此矩阵的两个特征值 $\lambda_1, \lambda_2$ 就表示窗口沿两个主方向移动时，亮度变化有多剧烈（两个特征值都很小对应平坦区域，一大一小对应边缘，只有两者都很大才是我们要找的特征位置——角点）

3.3 Shi-Tomasi#

这是 OpenCV 默认经常使用的模型，比 Harris 更稳定。直接看矩阵 $M$ 的最小特征值，如果 $\lambda_{min}$ 足够大，就是角点。对应代码为 goodFeaturesToTrack()

3.4 FAST#

目的：极快地判断一个像素是不是角点
检测原理：对于像素 $p$ ，以它为圆心，取半径为 3 像素的圆上的 16 个离散像素，并设定一个亮度阈值 $t$ 。如果在这 16 个像素中，存在连续 N 个像素（通常 N=9 或 12）都比 $I_p+t$ 更亮或都比 $I_p-t$ 更暗，那么 $p$ 就是角点

NOTE
这个过程非常快，但为了进一步加速，FAST 使用一种决策树式的排除方法：先检查圆上第 1、5、9、13 号四个位置，如果这四个位置中有三个同时满足“都亮”或“都暗”，才进行完整 16 点的检查；否则直接否定该像素。这淘汰了绝大多数非角点。

3.5 SIFT#

目的：实现对图像尺度缩放、旋转、仿射变形和光照变化都足够鲁棒的特征点。

[!question] 什么是 Harris 解决不了的？
由于Harris只能在一个固定大小的窗口里找角点。假设拍一栋房子，当拍摄距离近的时候可以很清晰地找到角点，但是一旦离远拍摄，房子只剩下几个像素时，窗口比房子还大，角点消失。所以我们需要让窗口大小也跟着变化，这便是尺度（观察图像是所用的放大倍率）。

尺度空间极值检测#

SIFT 用高斯差分金字塔来模拟尺度维度的变化：

高斯金字塔：对图像重复进行高斯模糊（为什么要模糊？因为我们要模拟尺度变化，进行模糊相当于离远拍摄，细节丢失），这样一层层做模糊便得到的许多尺度，这便是高斯金字塔。
高斯差分, DoG：有了许多层尺度后，我们仍不知道哪一层最适合做特征检测，于是 SIFT 将相邻的两张高斯模糊图像相减，得到 DoG 图像
极值点搜索：现在每一层都有一张 DoG 图像。在 DoG 空间里，每个像素与它同一尺度的 8 个邻居，以及上下相邻尺度的 9+9=18 个邻居（共 26 个）进行比较。如果它是最大值或最小值，则作为候选关键点。这一步保证选出的点同时是空间极值和尺度极值。

关键点精确定位#

上述候选点的确切位置可能在两个像素或两个尺度层级之间，SIFT认为DoG是连续函数，于是在附近用三维二次函数拟合的方法，将候选点展开为泰勒级数，解出极值的精确位置（亚像素）。

分配方向#

为了让特征对图像旋转不变，SIFT先算附近所有梯度方向，再统计为直方图（通常 36 个 bin，每 10° 一个），并按照像素到关键点的距离进行高斯加权。直方图峰值即为关键点主方向，即如果45° 为峰值，我们把局部区域旋转 -45° 变成同一朝向，这样无论转多少度，最终都能变成同一方向。

局部图像描述子#

至此，每个关键点获得了精确的位置、尺度和方向。现在需要用一个向量来描述它周围像素的外观，以便后续我们始终认得这个点，而且这个向量要对光照变化、轻微视角变化都稳定。

将关键点周围 16×16 的区域旋转至主方向（消除旋转影响），然后分成 4×4 共 16 个子块。（这里16×16不是像素数量，描述子采样是在关键点对应的尺度 $\sigma$ 进行的，如果 $\sigma=1$ ，就是 16×16 像素）
每个子块内统计 8 个方向的梯度直方图，梯度幅值同样经高斯加权。
将 16 个 8 方向直方图依次拼接，得到 128 维向量
后面再经过两次不同的归一化，提升对光照变化的鲁棒性：第一次归一化让整体亮度影响消失；第二次限制 128维向量每个分量小于 0.2，再重新归一化以减少强光点的影响
计算两个图中对应两个点的 128 维向量距离，距离小则认为匹配（下面一节会细讲匹配）

3.8 ORB#

它用 oFAST 检测角点，用 rBRIEF 计算描述子，整体速度比 SIFT 快两个数量级，同时具备旋转不变性和一定的尺度不变性

rBRIEF 描述子#

BRIEF (Binary Robust Independent Elementary Features) 是一种二值描述子，其基本思想是：在关键点周围的一块平滑图像区域中，随机挑选若干对像素点 $(x,y)$ ，直接比较他们的亮度：如果 $I(x)<I(y)$ 就输出1，否则0，较 256 次就得到一个 256 位的比特串。

四、特征匹配#

前面我们已经在图像中找到了特征点，并给每个特征点赋予了一个描述子向量。接下来的问题是：如何在这些向量间建立对应关系

为了使匹配质量高，效率快，我们还需解决两个问题：

如何快速找到最相似的两个描述子（最近邻搜索）
如何可靠地排除错误匹配（离群值剔除）

3.1 描述子距离#

匹配的基础是描述子间的距离。对于 SIFT 这样的浮点描述子，使用欧氏距离；对于 ORB 这样的二值描述子，使用汉明距离（即比特位不同的个数）。距离越小，描述子越相似。

3.2 快速最近邻搜索#

最直接的匹配是暴力搜索，但是复杂度是 $O(N \times M)$ ，一旦点很多，计算量不可接受。为了加速，我们通常采用空间划分数据结构来组织描述子，从而避免与所有候选者逐一比较。最经典的是KD-树

KD-树#

构建：我们对树的每一层进行空间划分，每次划分空间时，选择数据分布变化最大（方差最大） 的那个方向（x轴，y轴…）来切，用其中的中位数将数据分为两半，中位数为根节点，与左右形成二叉树。就这样将高维空间递归地进行划分构建，查询时只用从根节点出发查询，直到到达叶节点，然后计算出距离。但我们还需回溯，检测另一侧是否还可能有更近的点（通过超球面与划分超平面的距离判断，若半径到达另一侧，就需要回溯检测，否则跳过）。

KD-树高维会退化，由于维度越高，点越近，导致一个高维球的几乎所有区域都可能包含近邻，所以会不断回溯，最后退化成几乎遍历所有点

近似最近邻（ANN）#

我们通常愿意牺牲一点点精确性来换取速度提升，这就是近似最近邻思想。视觉几何中最流行的是 FLANN（快速近似最近邻库）和基于局部敏感哈希（LSH）的方法。

FLANN 自动选择最适合数据分布和维度的算法（如 KMeans 树、层次聚类树等），并通过调节搜索参数在速度与精度间折衷。
局部敏感哈希 (LSH) 专门为二值描述子设计。它通过多个随机超平面或随机位采样，将相似的点以高概率映射到同一个哈希桶中。查询时只需检查同桶内的候选点，匹配极快

3.3 过滤误匹配：比率检验与交叉验证#

最近邻搜索返回了一个最佳匹配，但由于很多因素，必然存在大量的误匹配（离群值），我们需要利用描述子本身的性质来过滤

Lowe 的比率检验#

对于某个特征，不仅找到它的最近邻（最佳匹配距离 $d_1$ ，还找次近邻（第二匹配距离 $d_2$ ）。如果是真正具备判别力的最佳匹配， $\frac{d_1}{d_2}$ 应该够小才对。所以我们在实际操作中涉及一个阈值 $\tau$ （通常 0.7 或 0.8），仅保留满足：

\frac{d_1}{d_2} < \tau

交叉验证（双向匹配）#

单方向匹配可能出现多对一的歧义，交叉验证要求匹配必须双向一致，即 A 的特征 a 匹配到 B 的特征 b，同时 B 的特征 b 反过来匹配到 A 的特征 a（且都是各自方向上的最近邻）

五、对极几何#

NOTE
这一部分是重中之重！考验数学功底的时候了。F / E、归一化八点法、RANSAC、相对位姿恢复、三角化、PnP、Track 管理、增量 SfM 等等最好自己实现一遍，因为这是SFM骨架，以后调试点云等等主要靠这些知识

现在我们已经有了两幅图像之间的一组特征匹配，接下来的问题是我们能否仅凭这些二维对应点，推断出这两台相机之间的相对位置和朝向。我们可以借助对极几何，这是后面双目重建和运动恢复的基础。

5.1 场景设定#

对极几何场景设定

关键元素：一个空间点 $X$ ，左相机中心 $C$ ，右相机中心 $C'$ 。这三个点确定了一个平面，称为极平面，这个极平面与两张图像平面相交，交线就是极线。左图上一点 $x$ 的所有可能对应点 $x'$ 一定落在右图的某一条极线上，反之亦然。连接两个相机中心的直线 $CC'$ 称为基线。基线与图像平面的交点称为极点，在左图上极点 $e$ 是右相机中心 $C'$ 的投影。

这种映射关系将所有可能的二维匹配搜索从整幅图像压缩到一条一维线上，极大降低了误匹配的可能，为恢复相机运动提供了强约束。

5.2 基本矩阵 $F$ 与本质矩阵 $E$ #

根据相机是否已经标定，对极几何的代数表达有两种路径。

基本矩阵 $F$ 的作用#

基本矩阵 $F$ 作用于像素坐标。可以把 $F$ 想象成一个转换器，因为我们上面知道了已知左图的一个 $x$ ，那么其对应的 $x'$ 一定在右图的某条极线 $l'$ 上，所以我们现在要求的就是 $l'$ 。 $F$ 的作用便在于此：我们输入一个坐标，对应输出一条线，也就是 $l'=Fx$ （本质： $F$ 把二维搜索范围变成沿极线一维搜索范围）

更近一步，如果 $x'$ 正好在那条极线上，那么满足这个条件:

\boxed{x'^TFx=0}

在线代里，一条直线 $ax+by+c=0$ 可以写成 $l=[a,b ,c]^T$ ，一个点写成 $x=[u,v,1]^T$ （齐次坐标）。那么 $x^Tl=0$ 展开就是 $au+bv+c=0$ ，也就是点在线上。而上面的 $Fx=l'$ ，所以如果满足上式，说明 $x'$ 在 $F$ 根据左图点 $x$ 计算出的极线上，但这并不能单独证明 $x'$ 就是 $x$ 的真实匹配点，因为一条极线上有很多像素点，这也是前面提及的极线约束厉害的地方：把二维搜索范围变成沿极线一维搜索范围。

$F$ 的本质是 3x3 矩阵，秩为2，还有一个重要性质是 $e'^TF=0$ ，推理如下：
右极点在所有右极线的交点，数学上有 $Fx$ 都经过 $e'$ ，所以 $e'^T(Fx)=0$ ，整理可得 $(e'^TF)x=0$ ，由于对所有x都成立，只能 $e'^TF=0$

$F$ 推导过程#

为了简化，我们假设第一个相机坐标系就是世界坐标系，所以第一个相机外参是 $[I|0]$ 。空间一点 $P$ 在第一个相机坐标系下的坐标为 $X_1$ （即从第一个光心指向点 $P$ 的三维向量），那么同一空间点 $P$ 在第二个相机坐标系下的坐标:

X_2 = R \cdot X_1 + t

$RX_1$ 是把 $X_1$ 拧到第二个坐标系下，得到的方向向量
$t$ 是从二个相机光心指向第一个光心的向量

现在考虑极平面的法向量，由于在第二个相机坐标系中，这个平面由两条相交的向量张成： $t$ 和 $RX_1$ ，那么法向量 $n$ 满足：

n=t\times (RX_1)

叉积出现了，而矩阵 $[t]_\times$ 就是专门用来计算“左乘向量 $t$ 的叉积的线性工具，所以：

t \times (RX_1)=[t]_\times \cdot R \cdot X_1

因为空间点 $p$ 的坐标 $X_2$ 肯定落在这个极平面上，所以 $X_2$ 必然垂直于法向量，代入 $n$ 整理成矩阵相乘的形式：

\boxed{X_2^T \cdot [t]_\times \cdot R \cdot X_1 = 0}

这里得到的是本质矩阵 E 的式子，我们下面详谈，先记住

注意：下面是求 $F$ 才涉及的内容，也就是把归一化坐标转换成像素坐标——引出 K, 这里的 $X_1, X_2$ 是归一化相机坐标系下的三维齐次坐标，他们与像素坐标 $x_1, x_2$ 的关系是 $x_1 = K \cdot X_1$ ， $x_2$ 同理，代入这两个式子去替换 $X_1, X_2$ 得：

x_2^T \cdot K^{-T} \cdot [t]_\times \cdot R \cdot K^{-1} \cdot x_1 = 0, \text{其中} \boxed{F = K^{-T} \cdot [t]_\times \cdot R \cdot K^{-1}}

[!question] 假设第一张图里的 $x_1$ 已经确定，为什么 $Fx_1$ 得到的就一定是第二张图里的极线？
代入 $F$ 的表达式，得到 $l_2 = K^{-T} \cdot [t]_\times \cdot R \cdot K^{-1} \cdot x_1 = K^{-T} \cdot n$ ， $n$ 为与相机基线 $t$ 和第一条观测射线相关的垂直方向（可以简单理解为极平面法向量），左乘 $K^{-T}$ 是为了在像素坐标系下，维持“点在直线上”这个内积关系永远成立（因为点和线在坐标变换下的变换规则不一样，如果点变换 $x'=Hx$ ，那么为了保证 $l^Tx=0$ ，线必须按照 $l'=H^{-T}l$ 来变换），而整体这个式子表示的就是极线的系数向量。

本质矩阵 $E$ #

如果我们事先标定了相机，已知内参矩阵 $K$ 和 $K'$ ，就可以将像素坐标转换为归一化坐标。（经过特征匹配，我们手里有的是像素坐标，需要转换）
对比 E 与 F：E 是从纯三维几何推导得出，而 F 是在此基础上硬噻了一个内参 K 进去，所以 E 比 F 简单点（

正向推导 E#

上面我们已经得到 $X_2^T \cdot [t]_\times \cdot R \cdot X_1 = 0$ ，其中本质矩阵为：

\boxed{E=[t]_\times \cdot R}

E 的推导过程完全没有涉及到内参，作用坐标是归一化坐标，它只涉及到三维空间里的旋转与平移，描述的是纯刚体几何

总结一下：

F 是像素坐标系里的极几何；E 是去掉内参后的归一化坐标系里的极几何。转换到归一化坐标，是为了去掉相机内参 K 的影响，让矩阵只剩下真实的空间运动信息。

[!question] 为什么已知内参 K 后，我们更喜欢先求 E，再从 E 恢复 R,t，而不是直接从 F 里恢复 R,t？
相机的运动 R,t 本身就是在相机坐标系里描述的，而 F 中还混着两个相机的内参，所以从 F 到运动，必须先去掉内参。所以我们一般都是直接对 E 进行奇异值分解求运动

5.3 八点法求 E 与归一化#

目标：求 $E$
由于每一对匹配点都要满足 $x'^TEx=0$ ，假设左点与右点分别为

x = \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}， x' = \begin{bmatrix} u' \\ v' \\ 1 \end{bmatrix}

E：

E = \begin{bmatrix} f_{11} & f_{12} & f_{13} \\ f_{21} & f_{22} & f_{23} \\ f_{31} & f_{32} & f_{33} \end{bmatrix}

代入 $x^T E x = 0$ 展开得（ $f_{ij}$ 统统是未知数，有9个）：

u' u f_{11} + u' v f_{12} + u' f_{13} + v' u f_{21} + v' v f_{22} + v' f_{23} + u f_{31} + v f_{32} + f_{33} = 0

上面方程可以简写为 $a^Tf=0$ ，其中 $a$ 是一个由该匹配点坐标构成的 9 x 1 向量。由于E整体乘一个数字没意义，实际自由度为8，所以只需要八个这样独立的线性方程即可求解，这就是8点法。把这八个点对应的方程堆叠起来形成一个矩阵 $A(8\times 9)$ ：

A \cdot f=0

NOTE
下面需要最小二乘与 SVD 的数学知识，如果还不清楚，务必先回到第一节

具体操作：

构造矩阵 A：将8对匹配点的像素坐标代入，构造矩阵 A
对 A 做奇异值分解，求得最小二乘解 $V_n$ ，再将其重排列为 3 x 3 向量得到 E
强制秩为 2

为什么E的SVD分解秩为2，而且满足前两个值相等？
回忆 E 的构造： $E=[t]_\times \cdot R$ ，R 是满秩的，不改变矩阵秩， $[t]_\times$ 是平移向量 $t$ 的反对称矩阵，其存在一个非零的零空间（t 方向本身），所以其秩为 2
设

t = \begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix}

那么

[t]_\times = \begin{bmatrix} 0 & -t_z & t_y \\ t_z & 0 & -t_x \\ -t_y & t_x & 0 \end{bmatrix}

如果我们看：

[t]_{\times}^{T}[t]_{\times}

会得到：

[t]_{\times}^{T}[t]_{\times} = \|t\|^2 I - tt^T

这个矩阵特别漂亮。它对 $t$ 方向作用时：

(\|t\|^2 I - tt^T)t = 0

所以对应一个特征值：0，而对于任何 垂直于 $t$ 的向量 $v$ ，因为 $t^T v = 0$ ，所以：

(\|t\|^2 I - tt^T)v = \|t\|^2 v

也就是说，垂直于 $t$ 的二维平面上，所有方向对应的特征值都是 $\|t\|^2$ 。于是 $[t]_{\times}^{T}[t]_{\times}$ 的特征值就是：

\|t\|^2, \quad \|t\|^2, \quad 0

而奇异值就是特征值开平方： $\|t\|, \|t\|, 0$

我们还需要对 E 做一次 SVD，因为 E 在理论上前两个奇异值相等，第三个奇异值为0，但是由于噪声，实际解出的 E 的奇异值有偏差，做法便是对 E 重新SVD，强制奇异值为理想状态，然后再返回去得到更精确的本质矩阵

IMPORTANT
直接在原始像素坐标上做八点法，数值稳定性会很差。因为齐次坐标第三位为1，前面的x,y在像素坐标下动辄几千，而线性方程中大数字会支配运算，导致精度差，于是需要先对像素归一化：

先平移，将所有点减去中心让点云中心再原点，

再缩放，将两点的平均距离缩成 $\sqrt{2}$ 以上两步叫做坐标归一化，用变换矩阵 $T$ 表示。

归一化坐标后再进行 SVD 求解 + 强制秩为2 + 反归一化从而得到真实F，当然如果你标定了相机有内参矩阵，就相当于是求 E

5.4 从本质矩阵中恢复运动 R,t#

如何分解出 $R, t$ ？给定 E，标准做法是对他做 SVD（此时的 $\Sigma$ 已经是上面那步强制后的形式），用一个固定的辅助矩阵：

W=\begin{bmatrix} 0 &-1& 0 \\ 1 &0& 0 \\ 0 &0 &1 \end{bmatrix}

能够拼凑出两种候选旋转 $R_1=UWV^T$ ， $R_2=UW^TV^T$ ，而平移方向就是 $U$ 的第三列 $u_3$ （单位向量，正负不确定），从而有旋转2种 × 平移正负2种 = 4种组合,只有一种在物理上是对的。

为什么候选旋转有两种，平移有正负之分？
将 $t$ 取反， $[t]_\times$ 变号，如果同时保持 $R$ 不变，实际上 E 的约束 $x'^T[t]_\times Rx=0$ 并不会因为变号改变，所以平移方向有正负两种歧义
而对于旋转，上面这两个构造都能够满足本质矩阵的分解结构，并且在需要时通过检查行列式，把它们调整为合法旋转矩阵 $det(R) = 1$ 。这就是旋转矩阵解的双重歧义。
所以最后有 2 x 2=4 组候选
几个细节：

如何取得 $t$ ？ t 跟 U 的第三列平行（E 的零空间对应 V 的第三列，E 的左零空间对应 U 的第三列；而平移方向 t 正好是 E 的左零空间，所以取U第三列）
工程实现需要检查 $det(R) = 1$ ，如果等于 -1 要调整为 1

Cheirality Check(正深度检验)#

拿几个点,用4组候选(R,t)分别做一次三角化,看哪一组算出来的3D点,在两台相机坐标系下的深度(Z坐标)都是正的，四组里,正深度点数最多的那组就是正确答案。

恢复 R, t 的代码实现#

实现 recoverPoseFromEssential()

1
bool recoverPoseFromEssential(
2
    const Eigen::Matrix3f& E,
3
    const std::vector<Eigen::Vector2f>& pts1,
4
    const std::vector<Eigen::Vector2f>& pts2,
5
    const Intrinsics& K,
6
    Eigen::Matrix3f& R,
7
    Eigen::Vector3f& t)
8
{
9
    // 1. SVD(E)
10
    // 2. 构造 W
11
    // 3. 生成两个 R 候选
12
    // 4. 生成 ±t
13
    // 5. 得到 4 组 R,t
14
    // 6. 对每组进行三角化
15
    // 7. 检查正深度数量
16
    // 8. 选择最优解
17
    return true;
18
}

5.5 三角化#

为什么两个相机就能实现2D点变3D点？
相机投影公式 $x∼PX$ （x<二维点>，X：三维点 $(X_1, X_2, X_3, X_4)$ ，P：相机投影矩阵。这里 ∼ 表示齐次等价，即 $x=\lambda PX$ ）。
第一个相机一般是 $P_1=[I|0]$ ，第二个 $P_2=[R|t]$ ，我们知道 $x_1,x_2$ ，现在就是求 $X$ 。齐次等价关系就是平行关系，所以 $x=\lambda PX$ 可以通过叉乘为0（ $x \times (PX)=0$ ）来抵消这个未知尺度，把它变成关于 $X$ 的线性约束，两个相机各自提供两个约束，展开可堆叠成 4 x 4 矩阵（ $AX=0$ ），然后通过 SVD 找到 $\|AX\|=0$ 找到最小二乘解，最后再除以 $X_4$ 得到普通 3D 坐标

NOTE
试着手推一遍三角化的矩阵 A
提示：一个相机提供两个约束

[!question] 现在有两个方法：

直接用刚推出来的 $AX=0$ + SVD 三角化

直接调整 3D 点 $X$ ，让它在两张照片上的重投影误差最小

为什么方法1虽然很常用，但不一定得到重投影误差最小的3D点？

方法 1 实际最小化的是 $AX$ 这种代数残差，受到深度、齐次尺度影响。方法 2 实际优化的就是重投影误差，直接最小化两个投影点在图像平面的距离，即优化 $\Sigma (\text{像素误差})^2$

IMPORTANT
如果匹配点质量不佳，会导致后续哪些部分的操作有误差？（把前面串起来） $\text{2D匹配不好} \rightarrow F \rightarrow E \rightarrow R,t \rightarrow P_2(\text{第二个相机的投影矩阵}) \rightarrow A \rightarrow X_{3D}$
这是后面debug时候的重要思路

5.6 用 RANSAC 同时估计模型和剔除误匹配#

我们从未期望所有初始匹配都是正确的。八点法对离群值非常敏感，一个错误的匹配就能把 $F,E$ 带偏，所以我们采用 RANSAC（随机采样一致性），目的：从一堆包含错误的数据中，找出那个能被最多正确匹配支持的模型。

实际流程：

从所有匹配中随机抽取8个点。
用这8个点算出一个候选模型 $F$ （如果已经提前对像素坐标进行了归一化就是求 $E$ ）。
用该 E 所有匹配点对：如果 Sampson 距离小于一个阈值（比如说 1 或 2 个像素），则认为这对匹配是内点。
sampson 距离：

d = \frac{\left( \mathbf{x}_2^T F \mathbf{x}_1 \right)^2}{\left( F \mathbf{x}_1 \right)_x^2 + \left( F \mathbf{x}_1 \right)_y^2 + \left( F^T \mathbf{x}_2 \right)_x^2 + \left( F^T \mathbf{x}_2 \right)_y^2}

分子：当前匹配违反极几何约束有多严重；
分母：考虑了两边坐标变化对这个约束的敏感程度，相当于做了一个局部尺度修正

[!question] 为什么不直接计算点到极线的普通垂直距离，而要使用 Sampson distance？
我们想衡量“这个匹配点偏离极几何约束多少”，但直接做精确的几何距离优化比较麻烦，所以用一个计算便宜的一阶近似。

记录内点数量。重复上述过程 $N$ 次，保留内点最多的那个模型。确定 N 大小：

N \geq \frac{\log(1-p)}{\log(1 - w^8)}

$w$ ：内点比例
$(1-w^8)^N$ ，N次采样全失败概率
p，我们希望成功的概率

最后，利用所有 RANSAC 内点重新估计一个更稳定的 F/E。

总结一下：RANSAC 的本质：假设真正的匹配对占比 80%，随机抽到的8个点全是内点的概率为 $0.8^8 \approx 16.8%$ ，也就是说，虽然一次可能抽到垃圾，但抽很多次之后，很大概率会出现一次：恰好抽到一组全是正确匹配的最小样本。这一组点产生的 F 会让大量真正匹配满足极线约束，于是它自然拥有很多内点。

NOTE
选择合适的模型不一定内点数量越多越好，需要通过三个方面来选择：

内点阈值是否合适：内点阈值偏大，导致有些外点也被纳入了内点

误差大小：如果 500 个内点的模型，平均 sampson 误差为 9，而480个内点的误差为 0.9，那么选择 480个内点的模型

空间分布：如果内点全部集中在画面一角，舍弃

六、增量式 SFM#

6.1 PnP#

对于前两个照片来说，坐标关系是 2D-2D。我们此时并不知道这两个点对应空间的 3D 点是哪一个，所以只能借助同一个3D点，在两个相机的观察下必须满足极线约束。通过很多个这样的约束（2D-2D匹配 —— E矩阵 —— R,t —— 三角化 —— 点云）来呈现3D点云
现在来到第三张图，我们已经经过前两张照片得到了空间 3D 点，再次做 SIFT 时，用的是第一张照片中的 3D 点跟照片3的2D点匹配（3D-2D）。于是现在问题变成了已知3D点和2D点求相机的R,t，这里便是 PnP(Perspective-n-Point)问题，在Pnp内部也要实现 RANSAC

PnP 本质：已知 3D 点 → 已知空间几何 → 找一个 R,t，让这些 3D 点投影后尽可能贴近已知的 2D 观测。
这就是为什么这里不需要 F/E。F/E 是在两个相机都未知、3D 点也未知的情况下，用两张图之间的关系先恢复相对运动；而 PnP 已经有了 3D 世界坐标，相机运动直接由 3D→2D 投影关系约束。

[!question] 已知相机[I|0],经 PnP 求出来的 R,t 描述的是什么？按照opencv的规定，描述的是世界坐标系中的 3D 点，如何变换到当前相机坐标系，也就是 $X_{camera}=RX_{world} + t$ ，真正的相机位置（在世界坐标的位置）为 $C=-R^Tt$ （这个式子推导很简单，世界空间中的光心位置 C 就是那个在相机坐标系里投影为 (0,0,0)的点，所以把 $0=RC+t$ ）

6.2 链式增量式 SFM 的设计#

全局 track#

下面这个文本流程明了地展现了增量式 SFM 中 track 的工作原理：

1
第三张图提取特征 → 得到 feature_new
2
        ↓
3
与已有图像的特征进行匹配（描述子/光流/极线搜索）
4
        ↓
5
匹配到第二张图中的 feature_old
6
        ↓
7
查询 feature_old 所属的 track_id
8
        ↓
9
将 feature_new 也加入该 track
10
        ↓
11
检查该 track 是否已经三角化并分配了 pointId
12
        ├── 是 → 获得 pointId
13
        └── 否 → 没有 pointId（等待后续三角化）

核心数据结构是哈希表（key:(图片下标，关键点下标) 这是一个二元组（编码成一个64位整数，value是这个关键点对应的3D点云在点云数组的下标，每完成一次三角化,新3D点在两张参与三角化的图里的关键点下标都要登记进这张表

[!question] 为什么有些track还没有进行三角化？ 如果这些观测点所对应的相机的基线太小，会导致三角化退化，所以就先不进行三角化；同时可能之前的匹配质量并不是很好，三角化后重投影误差太大就会先暂时放弃这个track的三角化，等待更多可靠观测加入这个track

匹配策略<每张新图>,只和最近一次成功注册的那张图做匹配，这也很直观，不可能所有照片都跟第一张比较，离得越远，重叠越少，越容易匹配失败。匹配后，使用匹配中已有3D点的那部分，求解新图的位姿。在新图注册后，使用新图与旧图的位姿以及匹配得到的2D点三角化得到新 3D 点

七、光束法平差（BA, Bundle Adjustment）#

前面的增量SFM有一个问题：每一步都是基于前一步结果估计出来的，误差会不断累积。BA 的作用：把所有相机位置和所有3D点一起重新优化，让所有图片的投影误差整体最小。

7.1 BA 的优化对象#

假设第 i 个相机看到了第 j 个3D点，我们已经这道了这个点在图片中的真实观测数据 $u_{ij} = (u,v)$ ，当前估计的相机 $R_i, t_i$ ，以及当前估计的 3D 点 $X_j$ ，那么我们可以把这个3D 点投影回第 i 张照片：

\hat{u}_{ij}=\pi(R_iX_j + t_i), \text{其中} \pi(X_c) = (\frac{X{^x_c}}{X^z_c},\frac{X^y_c}{X^z_c})

这个观测产生的投影误差又名残差 $r_{ij}=u_{ij} - \hat{u}_{ij}$ ，从而得到BA 的目标优化函数：

\boxed{\min_{R, t, X} \sum_{i,j} \|u_{ij} - \pi(R_i X_j + t_i)\|^2}

[!question] 为什么这里要对所有观测的误差平方后再加起来，而不是直接把误差加起来？
直接把带正负号的误差相加会互相抵消

假设现在有7个相机于 3w 个 3D 点。每个相机有六个自由度（3个旋转，3个平移），7个相机一共42个参数。每个3E点有三个参数（x,y.z），所有点一共9w个参数。如果把他们全部放进一个向量大概就是10w维。如果真的构建一个 10w x 10w 的稠密矩阵，计算与内存都要命，所以 BA 的优化对象是利用这些参数之间特殊的稀疏关系来优化，而不是单纯去优化这 10w 个参数

稀疏结构#

BA 虽然有 10 万多个变量，但并不是这 10 万个变量之间都互相有关。比如相机 $C_1$ 看到了 $P_1, P_2$ ，相机 $C_2$ 看到了 $P_2, P_3$ ，那么 $C_1$ 的误差只跟 $C_1, P_1, P_2$ 有关，与 $P_3$ 无关，这就意味着 Jacobian 矩阵虽然可能非常大，但里面会有大量的 0。

7.2 Jacobian#

在此处语境下可以理解为每个变量稍微动一下，会让每个重投影误差变化多少？

7.2 高斯-牛顿方程#

高斯牛顿方程的计算目标：想找到一组参数 $x$ ，使得残差向量 $r(x)$ 的平方和最小。

J^TJ\Delta x = -J^Tr

其中：

$r$ ：残差向量
$J$ ：雅可比矩阵
$J^TJ$ ：近似海森矩阵
$\Delta x$ ：增量向量

我们令 $H=J^TJ$ ， $g=J^Tr$ ，于是可得 $H\Delta x=-g$ ，将变量拆分成：

\Delta x = \begin{bmatrix} \Delta c \\ \Delta p \end{bmatrix}

其中c是相机参数，p是所有3D点。那么 H 可以写成：

\begin{bmatrix} H_{cc} & H_{cp} \\ H_{pc} & H_{pp} \end{bmatrix} \begin{bmatrix} \Delta c \\ \Delta p \end{bmatrix} = - \begin{bmatrix} g_c \\ g_p \end{bmatrix}

四个块分别代表：

$H_{cc}$ : 相机和相机之间的关系
$H_{pp}$ : 点和点之间的关系
$H_{cp}$ : 相机和点之间的关系
$H_{pc}$ : 点和相机之间的关系

萘Naie_Char

一、数学基础#

1.1 摄影几何与齐次坐标#

齐次坐标#

射影变换#

1.2 SVD 与最小二乘#

SVD分解#

齐次最小二乘#

二、相机模型#

2.1 针孔投影几何#

2.2 内参矩阵与外参矩阵#

2.3 镜头畸变#

径向畸变#

切向畸变#

三、特征检测#

3.1 好特征#

3.2 角点检测：Harris思想#

3.3 Shi-Tomasi#

3.4 FAST#

3.5 SIFT#

尺度空间极值检测#

关键点精确定位#

分配方向#

局部图像描述子#

3.8 ORB#

rBRIEF 描述子#

四、特征匹配#

3.1 描述子距离#

3.2 快速最近邻搜索#

KD-树#

近似最近邻（ANN）#

3.3 过滤误匹配：比率检验与交叉验证#

Lowe 的比率检验#

交叉验证（双向匹配）#

五、对极几何#

5.1 场景设定#

5.2 基本矩阵 FFF 与 本质矩阵 EEE#

基本矩阵 FFF 的作用#

FFF 推导过程#

本质矩阵 EEE#

正向推导 E#

5.3 八点法求 E 与归一化#

5.4 从本质矩阵中恢复运动 R,t#

Cheirality Check(正深度检验)#

恢复 R, t 的代码实现#

5.5 三角化#

5.6 用 RANSAC 同时估计模型和剔除误匹配#

六、增量式 SFM#

6.1 PnP#

6.2 链式增量式 SFM 的设计#

全局 track#

七、光束法平差（BA, Bundle Adjustment）#

7.1 BA 的优化对象#

稀疏结构#

7.2 Jacobian#

7.2 高斯-牛顿方程#

目录

5.2 基本矩阵 $F$ 与本质矩阵 $E$ #

基本矩阵 $F$ 的作用#

$F$ 推导过程#

本质矩阵 $E$ #