3D重建理论

我并不擅长计算机视觉方向，因此下面内容只涉及一些基础理论知识。由于我的立体几何、3D空间想象能力非常垃圾，这种课程一直学不明白，因此我对它的兴趣也不大，因此我也没投入过多时间和精力来进一步研究或详细解释。

原本每章内容后都有实例分析，包括代码和结果展示，但由于版权限制，我无法将这些内容直接搬运到这里。

1. 图像投影模型与3D重建理论

1.1. SLAM的概念

SLAM（同步定位与地图构建）。
通过估计每个相机的位置和场景的三维点，实现对场景的重建。

1.2. 逆向二维图像

逆向投影

从二维图像中提取三维场景的信息，即进行3D重建
图像是三维场景经过投影后的二维表示，要恢复三维信息，需要逆转这个投影过程。
建立一个数学模型，描述三维场景如何投影到二维图像中，然后尝试逆向求解。

1.3 针孔相机模型

1.3.1 模型概述

定义：针孔相机模型假设所有的光线都通过一个公共点，即光心（光学中心）。
优点：模型简单，易于逆向计算，在三维重建中广泛使用。

1.3.2 坐标系和符号约定

摄像机坐标系：
- 原点O_C：光学中心，坐标为(0, 0, 0)。
- 轴方向：建立右手坐标系， X_C 向右， Y_C 向下， Z_C 指向后方（场景深度方向）。
- 优势： Z 轴指向后方，物体深度为正，符合直觉。

1.3.3 三维点的投影到归一化焦平面

三维点表示：
- 点U：坐标为(U_X, U_Y, U_Z)，表示空间中的一个三维点。
归一化焦平面：
- 一个与光心O_C 距离为1 的平面 ( Z_C = 1 )，称为归一化聚焦平面。
- 将远处的三维点投影到此平面上。

1.3.4 齐次坐标与非齐次坐标

齐次坐标:

定义：在原有坐标后增加一个维度（通常为1），方便表示投影和变换。
表示：对于二维点m = (m_X, m_Y)^\top，其齐次坐标为\bar{m} = (m_X, m_Y, 1)^\top

作用：齐次坐标方便矩阵运算，尤其是在投影和变换过程中。

非齐次坐标:
- 标准的笛卡尔坐标表示法，不包含额外的维度。

1.4 摄像机的线性校准

1.4.1 从归一化焦平面到图像平面

图像平面：
- 坐标系：像素坐标系，通常以图像左上角为原点，向右为X 轴（列索引），向下为Y 轴。
- 目的：将归一化焦平面上的点映射到实际的图像像素坐标上。
线性变换：

\begin{cases} P _U = f \cdot m _X + U _0 \\ P _V = f \cdot m _Y + V _0 \end{cases}

焦距f
m_X,m_Y 是归一化焦平面上的点
光学中心在图像平面中的坐标(U_0, V_0)

1.4.2 摄像机内参矩阵

将上述线性变换表示为矩阵形式

K = \begin{pmatrix} f & 0 & U _0 \\ 0 & f & V _0 \\ 0 & 0 & 1 \end{pmatrix}

矩阵映射关系：

\underline{P} = K \cdot \underline{m}

其中， \underline{P} 是图像平面中的点的齐次坐标

1.4.3 逆向过程

从图像平面到归一化焦平面：

\underline{M} = K^{-1} \cdot \underline{P}

1.4.4 可视锥

表示相机能够看到的空间范围。通过将图像的四个角点转换到归一化焦平面，然后连接光学中心，形成视锥。

1.5 畸变建模与校正

1.5.1 相机畸变的来源

实际相机镜头的光学缺陷，尤其在广角镜头中，导致图像出现畸变，直线变曲，图像边缘出现拉伸或压缩。

1.5.2 畸变模型

畸变函数：
- 将归一化焦平面上的理想点经过畸变函数（从理想图像到畸变图像），得到畸变后的点，此点为 2D 实际畸变聚焦平面
  \underline{m} _d = d(\underline{m}, k)
  其中，k 是畸变参数
举个例子: 多项式径向畸变模型

M _d = \left(1 + k _1 \|m\| _2^2 + k _2 \|m\| _2^4 + \dots \right) m

其中: \|m\| 2^2 = m x^2 + m _y^2

1.6 畸变校正的实现

1.6.1 任务描述

目标：将畸变的实际图像校正为理想的无畸变图像

1.6.2 实现步骤

定义参数: 理想的摄像机内参矩阵K_{\text{ideal}} ; 畸变的摄像机内参矩阵K_{\text{real}}; 失真参数k 。

对于每个理想图像的像素坐标，执行以下步骤

将像素坐标转换到归一化焦平面：

\underline{m} _{\text{ideal}} = K _{\text{ideal}}^{-1} \cdot \underline{P} _{\text{ideal}}
应用畸变函数：
\underline{m} _d = d(\underline{m} _{\text{ideal}}, k)
映射回实际图像坐标系：

\underline{P} _{\text{real}} = K _{\text{real}} \cdot \underline{m} _d
插值：
插值：对\underline{P}_{\text{real}} 进行插值（由于坐标可能为非整数，可能要用双线性插值）
生成校正后的图像

2. 二维刚性变换和单应性

2.1 二维刚性变换

二维刚性变换包括平移和旋转

2.1.1 旋转

\mathbf{U}^c = \overrightarrow{O _c U}^c \quad \mathbf{U}^w = \overrightarrow{O _w U}^w

\mathbf{R} _{wc} \underline{\mathbf{U}}^c = \mathbf{R} _{wc} \cdot \overrightarrow{O _c U}^c = \overrightarrow{O _w U}^w

从一个参考系中选取一个向量然后转换到另一个坐标系中

\mathbf{R} _{wc} 是一个正交矩阵

2.1.2 平移

\mathbf{T} _{wc} = \overrightarrow{O _w O _c}^{w}

2.1.3 刚性变换公式

\mathbf{U}^w = \mathbf{R} _{wc} \cdot \mathbf{U}^c + \mathbf{T} _{wc}

证明

\mathbf{R} _{wc} \cdot \mathbf{U}^c + \mathbf{T} _{wc} = \mathbf{R} _{wc} \cdot \overrightarrow{O _c U}^c + \overrightarrow{O _w O _c}^w = \overrightarrow{O _c U}^w + \overrightarrow{O _w O _c}^w = \overrightarrow{O _w U}^w = \mathbf{U}^w

2.1.4 齐次坐标

\underline{\mathbf{U}}^w = \begin{bmatrix} \mathbf{U}^w \\ 1 \end{bmatrix}

\mathbf{M} _{wc} = \begin{bmatrix} \mathbf{R} _{wc} & \mathbf{T} _{wc} \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} r _{11} & r _{12} & r _{13} & t _{x} \\ r _{21} & r _{22} & r _{23} & t _{y} \\ r _{31} & r _{32} & r _{33} & t _{z} \\ 0 & 0 & 0 & 1 \end{bmatrix}

2.1.5 反变换

\mathbf{M} _{cw} = \mathbf{M} _{wc}^{-1}

2.1.6 变换的组合性

\mathbf{M} _{ab} \cdot \mathbf{M} _{bc} = \mathbf{M} _{ac}

2.2 单应性

2.2.1 平面场景假设

\mathbf{U} _i^A = z _i^A \cdot \underline{\mathbf{m}} _{Ai}

这个方程的意思就是， \mathbf{U}i^A 这个点可以由\underline{\mathbf{m}}_{Ai} 来表示，怎么表示呢？ \Rightarrow 乘它的深度即可（因为\underline{\mathbf{m}}_{Ai} 是单位深度）。

2.2.2 寻找 m_Ai 和 m_Bi 之间的对应关系

光有这个方程，我们怎么找到\underline{\mathbf{m}}{Ai} 和\underline{\mathbf{m}}{Bi} 之间的对应关系呢，通俗来讲，怎么进行坐标对应变换呢？

法线关键公式我们需要先回顾一个性质，来得到一个法线和平面间的关键公式在参考系A 中，平面P 的方程为： ax + by + cz + d = 0 其中a, b, c 是平面法向量分量， d 是常数项，代表平面P 和原点O_A 相对距离在向量形式中，平面方程可以化简为：

\mathbf{n} _A^\top \mathbf{U} _i^A + d = 0

\mathbf{n} _A^\top 代表：向量 P 在参考系 A 中的法向量
通过这个平面方程的向量形式，我们得到了一个带有法向量的一个很重要的公式。
利用变量代换得到深度表达式
将 \mathbf{U} i^A = z i^A \cdot \underline{\mathbf{m}} _{A,i} 带入上式中

\mathbf{n} _A^\top \cdot z _i^A \cdot \underline{\mathbf{m}} _{A,i} + d = 0 \quad \Rightarrow \quad z _i^A = -\dfrac{d}{\mathbf{n} _A^\top \cdot \underline{\mathbf{m}} _{A,i}}
这样，我们就把\underline{\mathbf{m}}_{Ai} 给引进来了，其中z_i^A = -\dfrac{d}{\mathbf{n}_A^\top \cdot \underline{\mathbf{m}}_{A,i}} 代表了深度。换句话说，我们利用\mathbf{U}_i^A 的两个方程，将\mathbf{U}_i^A 替换掉了，这样就得到z_i^A 深度，可是仍然解决不了问题\Rightarrow 也就是说光有关于\underline{\mathbf{m}}_{Ai} 的方程是不够的，还需要从\underline{\mathbf{m}}_{Bi} 入手

接下来我们找B 坐标系下的点\underline{\mathbf{m}}{Bi}

我们从刚性变换公式入手\mathbf{U}^w = \mathbf{R}{wc} \cdot \mathbf{U}^c + \mathbf{T}{wc} 可见从c 投影到w 只需要对\mathbf{U}^c 进行变换即可，也就是说，为了得到\underline{\mathbf{m}}{Bi} 只需要对\underline{\mathbf{m}}_{Ai} 进行刚性变换即可

\underline{\mathbf{m}} _{B,i} = \Pi \left( \mathbf{R} _{BA} \mathbf{U} _i^A + \mathbf{t} _{BA} \right)

其中 \Pi(\cdot) 是投影函数

\underline{\mathbf{m}} _{B,i}= \Pi \left( \mathbf{R} _{BA} \left( -\dfrac{d}{\mathbf{n} _A^\top \cdot \underline{\mathbf{m}} _{A,i}} \right) \cdot \underline{\mathbf{m}} _{A,i} + \mathbf{t} _{BA} \right)

将上公式左右两边都乘 -\dfrac{\mathbf{n} A^\top \cdot \underline{\mathbf{m}} {A,i}}{d}

\underline{\mathbf{m}} _{B,i}= \Pi \left( \mathbf{R} _{BA} \cdot \underline{\mathbf{m}} _{A,i} - \dfrac{\mathbf{n} _A^\top \cdot \underline{\mathbf{m}} _{A,i}}{d} \cdot \mathbf{t} _{BA} \right)

\underline{\mathbf{m}} _{B,i} = \Pi \left( \left( \mathbf{R} _{BA} - \dfrac{\mathbf{t} _{BA} \cdot \mathbf{n} _A^\top}{d} \right) \cdot \underline{\mathbf{m}} _{A,i} \right)

也就得到了各自归一化平面上 A 点到 B 点的对应关系

问题：上述公式中左右两边都乘了-\dfrac{\mathbf{n}_A^\top \cdot \underline{\mathbf{m}}_{A,i}}{d}，为什么保持不变？投影函数\Pi(\cdot) 的特点是它是一个比例不变的操作（即只看方向和相对位置，不看绝对尺度）。因此，即使我们在右边乘上-\dfrac{\mathbf{n}_A^\top \cdot \underline{\mathbf{m}}_{A,i}}{d}，也不会影响等式成立的条件，因为投影结果相同

2.2.3 寻找 \underline{\mathbf{P}} {A,i} 和 \underline{\mathbf{P}} {B,i} 之间的对应关系

我们已知：

\left\{ \begin{aligned} \underline{\mathbf{m}} _{A,i} = K _A^{-1} \cdot \underline{\mathbf{P}} _{A,i}\\ \underline{\mathbf{m}} _{B,i} = K _B^{-1} \cdot \underline{\mathbf{P}} _{B,i} \end{aligned} \right.

\underline{\mathbf{P}}{B,i} = K_B \cdot \underline{\mathbf{m}}{B,i} \Rightarrow 将上面得到的\underline{\mathbf{m}}_{B,i} 带入

\underline{\mathbf{P}} _{B,i} = K _B \cdot \Pi \left( \left( \mathbf{R} _{BA} - \dfrac{\mathbf{t} _{BA} \cdot \mathbf{n} _A^\top}{d} \right) \cdot \underline{\mathbf{m}} _{A,i} \right)

\underline{\mathbf{P}} _{B,i} = K _B \cdot \Pi \left( \left( \mathbf{R} _{BA} - \dfrac{\mathbf{t} _{BA} \cdot \mathbf{n} _A^\top}{d} \right) \cdot K _A^{-1} \cdot \underline{\mathbf{P}} _{A,i} \right)

回顾性质:

K \cdot \Pi \left( \begin{bmatrix} a \\ b \\ c \end{bmatrix} \right) = \Pi \left( K \cdot \begin{bmatrix} a \\ b \\ c \end{bmatrix} \right)

利用此性质，可得：

\underline{\mathbf{P}} _{B,i} = \Pi \left( K _B \cdot \left( \mathbf{R} _{BA} - \dfrac{\mathbf{t} _{BA} \cdot \mathbf{n} _A^\top}{d} \right) \cdot K _A^{-1} \cdot \underline{\mathbf{P}} _{A,i}\right)

2.2.4 得到单应性矩阵\mathbf{H}_{AB}

假设 \mathbf{H}{AB} = K_B \cdot \left( \mathbf{R}{BA} - \dfrac{\mathbf{t}_{BA} \cdot \mathbf{n}_A^\top}{d} \right) \cdot K_A^{-1}

因此：

\left\{ \begin{aligned} &\underline{\mathbf{P}} _{B,i} = \Pi \left( \mathbf{H} _{BA} \cdot \underline{\mathbf{P}} _{A,i} \right) \quad \quad A \Rightarrow B\\ &\underline{\mathbf{P}} _{A,i} = \Pi \left( \mathbf{H} _{BA}^{-1} \cdot \underline{\mathbf{P}} _{B,i} \right) = \Pi \left( \mathbf{H} _{AB} \cdot \underline{\mathbf{P}} _{B,i} \right) \quad \quad B \Rightarrow A \\ \end{aligned} \right.

通过单应性矩阵我们可以将某点从一个相机图片坐标系变换到另一个相机图片坐标系，也就是点映射关系

2.2.5 单应性矩阵估计求解

\mathbf{H} _{AB} = \begin{bmatrix} h _1 & h _4 & h _7 \\ h _2 & h _5 & h _8 \\ h _3 & h _6 & h _9 \end{bmatrix}

这是一个齐次矩阵，它有9 个参数h_1 到h_9，齐次矩阵在尺度上具有冗余性，所以会导致自由度的丢失

简单的解法--参数化（要估计的参数 = 自由参数）

\mathbf{H} _{AB} = \begin{bmatrix} h _1 & h _4 & h _7 \\ h _2 & h _5 & h _8 \\ h _3 & h _6 & 1 \end{bmatrix}

\mathbf{h} = \begin{bmatrix} h _1 \\ \vdots \\ h _8 \end{bmatrix}

如何估计\mathbf{h}？

在这种情况下只要我们了解一个对应点就可以求得h_1 到h_8

\underline{\mathbf{P}} _{A,i} = \Pi \left( \begin{bmatrix} h _1 & h _4 & h _7 \\ h _2 & h _5 & h _8 \\ h _3 & h _6 & 1 \end{bmatrix} \cdot \underline{\mathbf{P}} _{B,i} \right)

由于\underline{\mathbf{P}}{A,i}， \underline{\mathbf{P}}{B,i} 是齐次坐标，我们将其展开：

\begin{bmatrix} P _{A,i,x} \\ P _{A,i,y} \\ 1 \end{bmatrix} = \Pi \left( \begin{bmatrix} h _1 & h _4 & h _7 \\ h _2 & h _5 & h _8 \\ h _3 & h _6 & 1 \end{bmatrix} \cdot \begin{bmatrix} P _{B,i,x} \\ P _{B,i,y} \\ 1 \end{bmatrix} \right)

\left\{ \begin{aligned} P _{A,i,x} = \dfrac{h _1 \cdot P _{B,i,x} + h _4 \cdot P _{B,i,y} + h _7}{h _3 \cdot P _{B,i,x} + h _6 \cdot P _{B,i,y} + 1} \\ P _{A,i,y} = \dfrac{h _2 \cdot P _{B,i,x} + h _5 \cdot P _{B,i,y} + h _8}{h _3 \cdot P _{B,i,x} + h _6 \cdot P _{B,i,y} + 1} \end{aligned} \right.

\left\{ \begin{aligned} P _{A,i,x} \cdot \left( h _3 \cdot P _{B,i,x} + h _6 \cdot P _{B,i,y} + 1 \right) = h _1 \cdot P _{B,i,x} + h _4 \cdot P _{B,i,y} + h _7 \\ P _{A,i,y} \cdot \left( h _3 \cdot P _{B,i,x} + h _6 \cdot P _{B,i,y} + 1 \right) = h _2 \cdot P _{B,i,x} + h _5 \cdot P _{B,i,y} + h _8 \end{aligned} \right.

\begin{bmatrix} P _{B,i,x} & 0 & -P _{A,i,x} \cdot P _{B,i,x} & P _{B,i,y} & 0 & -P _{A,i,x} \cdot P _{B,i,y} & 1 & 0 \\ 0 & P _{B,i,x} & -P _{A,i,y} \cdot P _{B,i,x} & 0 & P _{B,i,y} & -P _{A,i,y} \cdot P _{B,i,y} & 0 & 1 \end{bmatrix} \begin{bmatrix} h _1 \\ h _2 \\ h _3 \\ h _4 \\ h _5 \\ h _6 \\ h _7 \\ h _8 \end{bmatrix} = \begin{bmatrix} P _{A,i,x} \\ P _{A,i _y} \end{bmatrix}

因为有8 个未知数，需要八个独立的线性方程，而每对对应点可以提供两个对应方程，因此需要至少四对对应点。

即需要四个匹配\left( P_{A,i}, P_{B,i} \right) \quad i = 1, 2, 3, 4。 \Rightarrow \mathbf{h}^* = \arg\min_{\mathbf{h}} \sum_{i=1}^{4} \left\lVert M_i \mathbf{h} - P_{A,i} \right\rVert_2^2 \Rightarrow 线性最小二乘法

3. 使用 RANSAC 算法进行稳健的单应性估计

3.1 目标

图像对齐与拼接：通过估计两幅图像之间的单应性（Homography），实现图像的自动拼接。

3.2 自动建立对应关系---SIFT 算法

兴趣点检测

使用 SIFT 等算法在两幅图像中检测特征点（这段代码由老师提供），无需手动标记对应点，利用算法自动建立图像间的对应关系。
因此我们可以找到两幅图像中最相似的点对，但注意，点对并不一定正确对应。
也就是可能会出现错误匹配（离群点），这种情况下不可以直接用对应关系，我们将使用另一种算法叫做 RANSAC 来自动评估对应点之间的正确性，并得到最理想的 H 矩阵并输出。

3.3 RANSAC 算法进行稳健估计

算法思想：
- 随机抽样一致性（Random Sample Consensus） 是一种在存在离群点（错误点）的情况下估计模型参数（H）的稳健算法。
- 通过反复随机抽样，寻找最符合的模型。
RANSAC 流程：
重复 N 次（迭代次数根据经验或计算确定）

① 随机选取 4 对匹配点：

4 是估计单应性矩阵所需的最小匹配点数。
从所有的匹配点中随机选四个，不确定哪个对应关系正确，所以后续中有一个估计评判标准（欧几里得距离）。

② 估计单应性矩阵 H^k：

使用选取的 4 对匹配点，通过 DLT 算法（上个实验做过，其目的与作用是，在已知对应点的情况下，将一个相机视角转换到另一个相机视角）估计单应性矩阵。

③ 计算误差并评估模型：

对于所有匹配点（包括未选取的），将第二幅图像的点 P_{B i} 通过估计的 H^k 转换，即 H^k P_{B i}。
计算变换后的点（估计点）与第一幅图像实际点P_{A_i} 之间的欧氏距离。
定义代价函数：使用二值核（要么为0 要么为1）函数\phi_c(d)：
- 当距离d < \tau 时，认为匹配正确，代价为0。
- 当距离d \geq \tau 时，认为匹配错误，代价为1。
总代价L^k = \sum_{i} \phi_c(|P_{A_i} - H^k P_{B_i}|)

④ 更新最佳模型：

如果当前代价L^k 小于之前的最小代价L，则更新L 和对应的H。
最终输出：具有最小代价的单应性矩阵H。

3.4 阈值\tau 的选择：

\tau 是判断匹配是否为内点的距离阈值，通常根据图像分辨率和匹配精度选择，一般在0.5 到3 个像素之间。
选择过大会增加错误匹配，过小会忽略正确匹配。

3.5 为什么不用传统的二次代价函数

敏感性问题：
- 二次代价函数（如最小二乘法）对离群点非常敏感，如果某个点的误差很大，会导致代价函数值过大，这时即使其他点的误差很小也没有用。
稳健性：
- 二值核函数对那些特别大、离谱的点不敏感（都等于1），能够有效抑制离群点的影响，使得估计结果更稳健。
其他核函数：
- 除了二值核函数，还存在其他稳健核函数，如Huber 核、$Lorentzian$ 核等，可以在一定程度上兼顾误差大小和稳健性。

3.6 RANSAC算法的局限性

参数数量影响：
- 当模型参数数量增加时，所需的随机采样次数会指数增长，计算成本显著提高。
适用范围：
- RANSAC 适用于参数数量较少的情况，如直线拟合、基础矩阵和单应性估计等。

4. 立体视觉中的对极几何

到目前为止，我们已经研究了平面场景的情况，使用了单应性来描述两个视图之间的关系。然而，对于一般的三维场景，平面假设不再成立。为此，我们引入了对极几何。

4.1 对极几何

对极几何可以通过一个示意图很好地解释：

考虑两个相机，分别位于参考系1 和参考系2

相机1 的光心为O_1，相机2 的光心为O_2，空间中的一点U 投影到两个相机的图像平面上，得到点\underline{m}_1 和\underline{m}_2。

问题描述：

在一般情况下，我们无法对点U 做出任何假设（与之前的平面场景不同）。

我们需要找到一种方法，在不知道U 的情况下，建立\underline{m}_1 和\underline{m}_2 之间的关系。

4.2 对极平面和对极线

对极平面

U 和光心O_1、 O_2 定义了一个平面\Rightarrow 点m_1、 m_2、 O_1、 O_2 共面\Rightarrow 称为对极平面。

\text{Contrainte\ épipolaire} = \text{coplanarité}，即\underline{m}_1、 \underline{m}_2、 O_1、 O_2 共面。

在立体视觉中，基础矩阵F 和本质矩阵E 都依赖于共面性条件来计算。

对极线

对极平面与两个相机的图像平面相交，分别得到对极线l_1 和l_2。

m_2 是三维点U 在第二个图像平面的投影，但根据对极几何的约束， m_2 必须位于对极线l_2 上。

\Rightarrow 给定点m_1 的位置，可以通过基础矩阵F 确定对应的对极线l_2：

基础矩阵F 捕捉了两个相机之间的相对姿态和内在参数信息。这个公式表明，给定点m_1，可以计算出m_2 必须位于的对极线l_2

4.3 对极约束

目标：利用上述几何关系，形式化对极约束，建立m_1 和m_2 之间的数学关系。

定义向量：

\left\{ \begin{aligned} &\mathbf{\underline{m}_1} \text{ is the vector from the optical center } O_1 \text{ to the image point } \underline{m}_1 \quad \overrightarrow{O_1 m_1}^{1} \\ &\mathbf{\underline{m}_2} \text{ is the vector from the optical center } O_2 \text{ to the image point } \underline{m}_2 \quad \overrightarrow{O_2 m_2}^{2} \\ &\mathbf{t_{12}} = \overrightarrow{O_1 O_2}^{1} \text{ is the translation vector between the two camera optical centers} \end{aligned} \right.

定义对极平面的法向量：

\left\{ \begin{aligned} &\text{In reference frame } 1, \quad \overrightarrow{\mathbf{n}_1}^{1} = \underline{\mathbf{m}}_1 \times \mathbf{t}_{12} \\ &\text{In reference frame } 2, \quad \overrightarrow{\mathbf{n}_2}^{2} = \mathbf{R}_{21} \overrightarrow{\mathbf{n}_1}^{1}, \text{ where } \mathbf{R} \text{ is the rotation matrix between the cameras} \end{aligned} \right.

注意：

其中 \times 表示两个向量之间的叉积运算。叉积的结果是一个向量，它垂直于运算的两个向量，方向由右手定则决定，大小为这两个向量构成的平行四边形的面积。
法向量的坐标系变换不用考虑平移部分，因为单位法向量并不是坐标位置，方向向量在旋转过程中大小不变，不受平移的影响。总而言之，法向量只考虑旋转矩阵，而点则需要考虑旋转加平移。

\overrightarrow{\mathbf{n}_2}^{2} = \mathbf{R}_{21} \cdot \overrightarrow{\mathbf{n}_1}^{1} = \mathbf{R}_{21} \cdot \left( \underline{\mathbf{m}}_1 \times \mathbf{t}_{12} \right) = \mathbf{R}_{21} \cdot \underline{\mathbf{m}}_1 \times \mathbf{R}_{21} \cdot \mathbf{t}_{12}

由于之前我们已知 \mathbf{t} {21} = \mathbf{R} {21} \cdot \mathbf{t} _{12}，所以上式变为

\overrightarrow{\mathbf{n}_2}^{2} = \mathbf{t}_{21} \times \left( \mathbf{R}_{21} \cdot \underline{\mathbf{m}}_1 \right)

回顾叉积运算性质：

\mathbf{a} \times \mathbf{b} = \begin{bmatrix} a_x \\ a_y \\ a_z \end{bmatrix} \times \begin{bmatrix} b_x \\ b_y \\ b_z \end{bmatrix} = \begin{bmatrix} a_y b_z - a_z b_y \\ a_z b_x - a_x b_z \\ a_x b_y - a_y b_x \end{bmatrix}_{3 \times 1} \Rightarrow \left[\mathbf{a}\right]_{\times} = \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{bmatrix}

\mathbf{a} \times \mathbf{b} = \left[\mathbf{a}\right]_{\times} \mathbf{b} = \begin{bmatrix} 0 & -a_z & a_y \\ a_z & 0 & -a_x \\ -a_y & a_x & 0 \end{bmatrix} \begin{bmatrix} b_x \\ b_y \\ b_z \end{bmatrix}

利用上述性质，我们可以看出，叉积运算可以变成矩阵运算，因此我们利用以上性质得到：

\overrightarrow{\mathbf{n}_2}^{2} = \mathbf{t}_{21} \times \left( \mathbf{R}_{21} \cdot \underline{\mathbf{m}}_1 \right) = \left[ \mathbf{t}_{21} \right]_{\times} \cdot \mathbf{R}_{21} \cdot \underline{\mathbf{m}}_1

因为 \overrightarrow{\mathbf{n}_2}^{2} 是 \mathbf{m}_2 的法线 \Rightarrow \mathbf{m}_2^\top \cdot \overrightarrow{\mathbf{n}_2}^{2} = 0

\mathbf{m}_2^\top \cdot \left( \left[ \mathbf{t}_{21} \right]_{\times} \cdot \mathbf{R}_{21} \right) \cdot \underline{\mathbf{m}}_1 = 0

4.4 本质矩阵（matrice essentielle）

公式
假设
\mathbf{E} {21} = \left[ \mathbf{t} {21} \right] {\times} \cdot \mathbf{R} {21} \quad \Rightarrow \quad \text{matrice essentielle}
它包含了两个相机之间的相对旋转 \mathbf{R} 和平移 \mathbf{t} 的信息。
原式 = \underline{\mathbf{m}} 2^\top \cdot \mathbf{E} {21} \cdot \underline{\mathbf{m}} _1 = 0
自由度
5 \text{ degre de liberte} \\\\ \downarrow\\\\ 5 \text{ DDL} \left( \begin{array}{c} 3 \, \mathbf{R} _{21} \quad \text{rotation} \\\\ \quad 2 \, \mathbf{t} _{21} \quad \text{translation} \end{array} \right)\\\\ \downarrow\\\\ \quad \quad \| \mathbf{t} _{21} \|_2 \quad \text{ inconnue}

自由度：\left\{ \begin{aligned} &\text{The rotation matrix } \mathbf{R} \text{ has 3 degrees of freedom} \\ &\text{The translation vector } \mathbf{t} \text{ has 2 degrees of freedom (since the scale is unknown)} \\ &\text{Therefore, } \mathbf{E} \text{ has 5 degrees of freedom} \end{aligned} \right.
- 自由度（degree of freedom, DoF）是指描述本质矩阵所需的独立参数数量。在几何和线性代数中，自由度反映了系统在不受限制的情况下可以独立变化的方向或方式。
- 旋转矩阵具有 3 个自由度，描述了三维空间中的旋转。
- 平移向量理论上在三维空间中有 3 个自由度。但是本质矩阵中的平移向量一般只关注方向，对长度忽略（未知），所以平移向量只剩下 2 个有效的自由度，描述了平移的方向。

4.5 基础矩阵（Matrix Fundamental）

对上述公式继续变换：

\underline{\mathbf{m}}_2^\top \cdot \mathbf{E}_{21} \cdot \underline{\mathbf{m}}_1 = 0

已知

\left\{ \begin{aligned} \underline{\mathbf{m}}_2 = K^{-1} \cdot \underline{\mathbf{P}}_2 \\ \underline{\mathbf{m}}_1 = K^{-1} \cdot \underline{\mathbf{P}}_1 \end{aligned} \right.

\underline{\mathbf{P}}_2^\top \cdot (K^{-1})^\top \cdot \mathbf{E}_{21} \cdot K^{-1} \cdot \underline{\mathbf{P}}_1 = 0

当相机内参未知或未被考虑时，我们引入一个基础矩阵 \mathbf{F} 来覆盖 K。

假设 \mathbf{F} {21}= (K^{-1})^\top \cdot \mathbf{E} {21} \cdot K^{-1}

\mathbf{F}_{21} : \text{ matrice fondamentale} \quad \Rightarrow \quad 7 \text{ DDL}\quad \left\{ \begin{aligned} & \text{- matrice homogène} \\ & \text{- rang}(\mathbf{F}_{21}) = 2 \quad \Rightarrow \quad \det(\mathbf{F}_{21}) = 0 \end{aligned} \right.

性质：

齐次：基础矩阵 \mathbf{F} 是齐次矩阵，可以乘以任意非零标量而不改变其性质
秩约束： \mathbf{F} 的秩为 2

原式 = \underline{\mathbf{P}} 2^\top \cdot \mathbf{F} {21} \cdot \underline{\mathbf{P}} _1 = 0

\text{设：} \quad \mathbf{L}_2 = \mathbf{F}_{21} \cdot \underline{\mathbf{P}}_1 = \begin{bmatrix} a \\ b \\ c \end{bmatrix}

\underline{\mathbf{P}}_2^\top \cdot \mathbf{L}_2 = 0 \quad \Leftrightarrow \quad a P_{2,x} + b P_{2,y} + c = 0

这就是相机 2 的图像平面中的直线方程 \Rightarrow 对极线

4.6 本质和基础矩阵的估计

相机已经校准 \Rightarrow 本质矩阵 \mathbf{E} 的估计（5 个自由度） \Rightarrow 5 点对应算法
相机未校准 \Rightarrow 基础矩阵 \mathbf{F} 的估计（7 个自由度） \Rightarrow 7 点对应算法
求解 \Rightarrow 8 点对应算法 \Rightarrow 故意忽略约束条件 \det(\mathbf{F}) = 0

4.7 算法(8点对应算法)

步骤：

收集匹配点对：
\mathbf{F} 有 7 个自由度，但在算法中忽略了秩为 2 的约束，因此需要至少 8 对匹配点来估计 \mathbf{F}。
构建线性方程组
对于每一对匹配点 ( \mathbf{m} 1, \mathbf{m} 2)，构建方程：

\underline{\mathbf{m}}_2^\top \cdot \mathbf{E}_{21} \cdot \underline{\mathbf{m}}_1 = 0
\underline{\mathbf{P}}_2^\top \cdot \mathbf{F}_{21} \cdot \underline{\mathbf{P}}_1 = 0
求解：
将方程组表示为： \underline{\mathbf{P}} 2^\top \cdot \mathbf{L} 2 = 0
RANSAC 算法步骤
处理匹配点对中的离群点（错误匹配），稳健地估计 \mathbf{F}
- 随机采样：利用 8 点对应算法 来估计 \mathbf{F}
- 评估模型：利用估计得到的 \mathbf{F} 来计算所有匹配点对的对极约束误差，即点到对应对极线的距离
- 判断内点：根据设定的距离阈值，判断哪些匹配点是内点
- 迭代：重复上述过程，直到找到内点数量最多的模型

5. 束调整

束调整是一种同时优化摄像机参数（包括位置、姿态和内参）和场景中三维点位置的技术
其核心思想是通过最小化三维点在图像上的重投影误差，使得优化后的模型与实际观测更加吻合
记住五个字：最小化投影误差

5.1 两个摄像机的情况

5.1.1 数据

\left( P {A,i}, P {B,i} \right) _{i=1,\dots,N} \implies N \text{ correspondences}

5.1.2 要估计的参数

摄像机的姿态以及三维点云数据集

\mathbf{R} _{W1} \quad \mathbf{t} _{W1} \quad \mathbf{R} _{W2} \quad \mathbf{t} _{W2} \quad \left\{\mathbf{U}^w_i \right\}_{i=1,\dots,N}

5.1.3 损失函数

\mathcal{L} \left( \mathbf{R}_{w1}, \mathbf{t}_{w1}, \mathbf{R}_{w2}, \mathbf{t}_{w2}, \left\{ \mathbf{U}^w_i \right\}_{i=1,\dots,N} \right)= \sum_{i=1}^{N} \left( \left\lVert P_{1,i} - K_1 \Pi \left( \mathbf{R}_{w1}^\top \mathbf{U}_i^{w} - \mathbf{R}_{w1}^\top \mathbf{t}_{w1} \right) \right\rVert_2^2 +\left\lVert P_{2,i} - K_2 \Pi \left( \mathbf{R}_{w2}^\top \mathbf{U}_i^{w} - \mathbf{R}_{w2}^\top \mathbf{t}_{w2} \right) \right\rVert_2^2 \right)

其中：

K_A 和K_B 是摄像机A 和B 的内参矩阵。
\Pi(\cdot) 是投影函数，将三维点投影到二维平面上。
\mathbf{R}_{w1}^\top 和\mathbf{R}_{w2}^\top 等价于\mathbf{R}_{1w} 和\mathbf{R}_{2w}，即将点从世界坐标系转换到摄像机坐标系。
\mathbf{R}_{w1}^\top \mathbf{t}_{w1} 等价于\mathbf{t}_{1w}，表示平移向量。

\mathbf{U}_i^{1} = \mathbf{R}_{w1}^\top \cdot \mathbf{U}_i^{w} - \mathbf{R}_{w1}^\top \cdot \mathbf{t}_{w1}

也就是将\mathbf{U}_i^{w} 变换到\mathbf{U}_i^{1}，即从世界坐标系变换到相机坐标系。

做差：相机A 或B 中的图像坐标（实际）减去三维空间旋转变换得来的估计图像坐标，等于重投影误差。

5.2 多个摄像机的情况

5.2.1 数据

每张图像中检测到的点为：

\left\{ \left\{ P _{m,i} \right\} _{i=1,\dots,N _m} \right\} _{m=1,\dots,M}

这些点在不同视角下的图像中可以形成轨迹（tracks）
第 m 个摄像机检测到的点，其中 N _m 是第 m 个摄像机检测到的点的数量

\left\{ \text{p2d-id} _m, \ \text{p3d-id} _m \right\} _{m=1,\dots,M}

其中：

\text{p2d-id} _m 是二维点在图像中的索引
\text{p3d-id} _m 是对应的三维点在点云中的索引
它们的大小尺寸都是 C _m \times 1

5.2.2 要估计的参数

相机外参：

\left\{ \left( \mathbf{R} _{wm}, \mathbf{t} _{wm} \right) \right\} _{m=1,\dots,M}
三维点的位置：

\left\{ \mathbf{U} _i^{w} \right\} _{i=1,\dots,N}

5.2.3 损失函数

代价函数扩展为对所有摄像机和所有检测到的点进行误差计算，将投影点与实际观测点之间的距离最小化：

\mathcal{L}(x) = \sum_{m=1}^{M} \sum_{c=1}^{C_m} \left\| P_{m,\ \text{p2d-id}_m(c)} - K_m \Pi \left( \mathbf{R}_{wm}^\top \mathbf{U}_{\text{p3d-id}_m(c)}^{w} - \mathbf{R}_{wm}^\top \mathbf{t}_{wm} \right) \right\|_2^2

C _m 是第 m 台摄像机的观测数量
\mathbf{U} {\text{p3d-id} m(c)}^{w} 是与观测对应的三维点

我们可以简单地将上述代价函数简化成：

\mathcal{L}(x) = \sum_{i=1}^{N} \left\| f_i(x) \right\|_2^2 \quad \left\{ \begin{array}{l} x \in \mathbb{R}^D \\ f_i : \mathbb{R}^D \rightarrow \mathbb{R}^B \end{array} \right.

x 是所有待优化的参数（摄像机参数和三维点坐标）
f _i(x) 是第 i 个残差函数，表示第 i 个观测的重投影误差
我们的目标是找到 x，使得 \mathcal{L}(x) 最小化。这是一个非线性最小二乘问题，通常使用迭代的方法求解

5.3 高斯牛顿算法

用于非线性最小二乘问题的一种迭代优化算法\Rightarrow \text{ iteratif } \quad\delta_{k+1} = \delta_k + d_k

\text{Linearisation\ de\ } f_i: \quad f_i(x_k + d_k) \approx f_i(x_k) + \mathbf{J}_i(x_k)\cdot d_k

\delta x 是参数的增量，需要求解

对于每次迭代，我们在当前估计x_k 附近对f_i(x) 进行泰勒展开，并忽略高阶项

f_i(\delta_k + d_k) \in \mathbb{R}^B

f_i(\delta_k) \in \mathbb{R}^B \quad \mathbf{J}_i(\delta_k) \in \mathbb{R}^{B \times D} \quad d_k \in \mathbb{R}^D

其中雅可比矩阵为:

\mathbf{J}_i(x_k) = \frac{\partial f_i(x_k + d_k)}{\partial d_k} \bigg|_{d_k=0}

代表了在点 x_k 处函数 f_i 对于 d_k 的偏导数，并且此偏导数是在 d_k = 0 的条件下计算的
描述了在点 x_k 处函数 f_i 的线性变化率

线性最小二乘法
L_k(d_k) = \sum_{i=1}^{N} \left\| f_i(x_k) + \mathbf{J}_i(x_k) \cdot d_k \right\|_2^2
\mathbf{J}_k = \begin{bmatrix} J_1(x_k) \\ J_2(x_k) \\ J_3(x_k) \\ \vdots \\ J_N(x_k) \end{bmatrix} \mathbf{b}_k = \begin{bmatrix} f_1(x_k) \\ f_2(x_k) \\ \vdots \\ f_N(x_k) \end{bmatrix}
- \mathbf{b}_k是所有残差的组合
线性最小二乘问题变为：
L_k(d_k) = \lVert b_k + J_k \cdot d_k \rVert_2^2

通过最小化L_k(\delta x)，我们可以得到线性方程组：
J_k^T \cdot J_k \cdot d k = -J_k^T \cdot b_k \quad
- 其中 b_k ∝ \text{ gradient}
- 左边的矩阵 \mathbf{J}_k^T \mathbf{J}_k是海森矩阵的近似右边的向量 -\mathbf{J}_k^T \mathbf{b}_k是梯度的负值
- 求解这个线性系统，得到参数更新 d_x
Levenberg-Marquardt算法
在高斯-牛顿算法的基础上引入阻尼因子 \lambda，使得优化过程在接近解时具有高斯-牛顿的快速收敛特性，而在远离解时具有梯度下降的稳定性
- 常用于非线性最小二乘问题的迭代优化算法
- 目标函数:
  L_k(d_k) = \lVert b_k + J_k d_k \rVert_2^2 + \lambda \lVert d_k \rVert_2^2 \quad \quad \Rightarrow (J_k^T J_k + \lambda I_k)d_k = -J_k^T b_k
- \lambda是阻尼因子
  
  \begin{cases} \text{Si } \lambda = 0 & \Rightarrow \text{Gauss-Newton} \\ \text{Si } \lambda \rightarrow +\infty & \Rightarrow \lambda d_k \rightarrow -J_k^T b_k \quad \text{descente de gradient} \end{cases}
  - 如果新的代价函数值降低了（说明更新有效），则减小 \lambda，使算法更接近高斯-牛顿法，加快收敛
  - 如果代价函数值没有降低，则增大 \lambda，使算法更接近梯度下降法，保证稳定性

5.4 算法步骤总结

在实际应用中，Levenberg-Marquardt算法的步骤如下：

初始化：
- 设定初始参数x和阻尼因子 \lambda
- 计算初始代价函数 L_{\min}
迭代：
- 计算雅可比矩阵 \mathbf{J}和残差 \mathbf{b}
- 求解线性系统：
  (J^T J + \lambda I_d) d = -J^T b
- 更新参数：
  x′=x+d
- 计算新的代价函数L'
判断更新效果：
- 如果 L' < L_{\min}（代价函数降低）：
  - 接受更新： x = x'，L_{\min} = L'
  - 减小 \lambda：\lambda = \lambda / 2
  - 继续迭代
- 否则（代价函数未降低）：
  - 拒绝更新，不改变x
  - 增大 \lambda：\lambda = 2\lambda
  - 检查 \lambda 是否超过最大值，若超过则停止迭代
终止条件：
- 当 \lambda 超过预设的最大值，或者参数更新的幅度小于阈值时，停止迭代

Zehua