1. 图像投影模型与3D重建理论
1.1. SLAM的概念
- SLAM（Simultaneous Localization and Mapping，即同步定位与地图构建）。
- 通过估计每个相机的位置和场景的三维点，实现对场景的重建。
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 齐次坐标与非齐次坐标
齐次坐标（Homogeneous Coordinates）：
- 定义：在原有坐标后增加一个维度（通常为 1），方便表示投影和变换。
- 表示：对于二维点 m = (m _X, m _Y)^\top，其齐次坐标为 \bar{m} = (m _X, m _Y, 1)^\top
作用：齐次坐标方便矩阵运算，尤其是在投影和变换过程中。
- 非齐次坐标（Inhomogeneous Coordinates）：
- 标准的笛卡尔坐标表示法，不包含额外的维度。
1.4 摄像机的线性校准
1.4.1 从归一化焦平面到图像平面
-
图像平面：
- 坐标系：像素坐标系，通常以图像左上角为原点，向右为 X 轴（列索引），向下为 Y 轴。
- 目的：将归一化焦平面上的点映射到实际的图像像素坐标上。
-
线性变换：
-
变换公式：
-
焦距 f
-
m _X, m _Y 是归一化焦平面上的点
-
光学中心在图像平面中的坐标 (U _0, V _0)
1.4.2 摄像机内参矩阵
- 将上述线性变换表示为矩阵形式
- 矩阵映射关系：
其中，\underline{P} 是图像平面中的点的齐次坐标
1.4.3 逆向过程
- 从图像平面到归一化焦平面：
1.4.4 可视锥
- 表示相机能够看到的空间范围。通过将图像的四个角点转换到归一化焦平面，然后连接光学中心，形成视锥。
1.5 畸变建模与校正
1.5.1 相机畸变的来源
- 实际相机镜头的光学缺陷，尤其在广角镜头中，导致图像出现畸变，直线变曲，图像边缘出现拉伸或压缩。
1.5.2 畸变模型
-
畸变函数：
- 将归一化焦平面上的理想点经过畸变函数（从理想图像到畸变图像），得到畸变后的点，此点为 2D 实际畸变聚焦平面
其中，$k$ 是畸变参数
- 举个例子: 多项式径向畸变模型
其中: \|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{P} _{\text{real}} 进行插值（由于坐标可能为非整数，可能要用双线性插值）
-
生成校正后的图像
2. 二维刚性变换和单应性
2.1 二维刚性变换
二维刚性变换包括平移和旋转
2.1.1 旋转
-
从一个参考系中选取一个向量然后转换到另一个坐标系中
-
\mathbf{R} _{wc} 是一个正交矩阵
2.1.2 平移
2.1.3 刚性变换公式
注意
证明：
2.1.4 齐次坐标
2.1.5 反变换
2.1.6 变换的组合性
2.2 单应性
2.2.1 平面场景假设
这个方程的意思就是， \mathbf{U} _i ^ A 这个点可以由 \underline{\mathbf{m}} _{Ai} 来表示，怎么表示呢？\Rightarrow 乘它的深度即可（因为 \underline{\mathbf{m}} _{Ai} 是单位深度）。
2.2.2 寻找 \underline{\mathbf{m}} _{Ai} 和 \underline{\mathbf{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 代表：向量 P 在参考系 A 中的法向量
通过这个平面方程的向量形式，我们得到了一个带有法向量的一个很重要的公式。
-
利用变量代换得到深度表达式
将 \mathbf{U} _i^A = z _i^A \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} 进行刚性变换即可
其中 \Pi(\cdot) 是投影函数
将上公式左右两边都乘 -\dfrac{\mathbf{n} _A^\top \cdot \underline{\mathbf{m}} _{A,i}}{d}
也就得到了各自归一化平面上 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} 之间的对应关系
我们已知：
\underline{\mathbf{P}} _{B,i} = K _B \cdot \underline{\mathbf{m}} _{B,i} \Rightarrow 将上面得到的 \underline{\mathbf{m}} _{B,i} 带入
回顾性质：
利用此性质，可得：
2.2.4 得到单应性矩阵 \mathbf{H} _{AB}
假设
\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.
\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}
\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}
\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)
\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}
\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.
\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}
\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
\mathbf{m}2^\top \cdot \left[ \mathbf{t}{21} \right]{\times} \cdot \mathbf{R}{21} \cdot \underline{\mathbf{m}}_1 = 0
\mathbf{m}2^\top \cdot \left( \left[ \mathbf{t}{21} \right]{\times} \cdot \mathbf{R}{21} \right) \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.
\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}{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.
\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
\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
5.1.2 要估计的参数
摄像机的姿态以及三维点云数据集
5.1.3 损失函数
其中：
- 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 数据
每张图像中检测到的点为：
-
这些点在不同视角下的图像中可以形成轨迹（tracks）
-
第 m 个摄像机检测到的点，其中 N _m 是第 m 个摄像机检测到的点的数量
其中：
- \text{p2d-id} _m 是二维点在图像中的索引
- \text{p3d-id} _m 是对应的三维点在点云中的索引
- 它们的大小尺寸都是 C _m \times 1
5.2.2 要估计的参数
- 相机外参：
- 三维点的位置：
5.2.3 损失函数
代价函数扩展为对所有摄像机和所有检测到的点进行误差计算，将投影点与实际观测点之间的距离最小化：
- C _m 是第 m 台摄像机的观测数量
- \mathbf{U} _{\text{p3d-id} _m(c)}^{w} 是与观测对应的三维点
我们可以简单地将上述代价函数简化成：
- 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 \mathbf{J}_i(\delta_k) \in \mathbb{R}^{B \times D} d_k \in \mathbb{R}^D
-
-
其中雅可比矩阵为:
- 代表了在点 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
\quad \mathbf{J}_k = \begin{bmatrix} \quad J_1(x_k) \\\\ \quad J_2(x_k) \\\\ \quad J_3(x_k) \\\\ \vdots \\\\ \quad J_N(x_k) \end{bmatrix} \quad \mathbf{b}_k = \begin{bmatrix} \quad f_1(x_k) \\\\ \quad f_2(x_k) \\\\ \quad \vdots \\\\ \quad 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是阻尼因子
-
- **如果新的代价函数值降低了**（说明更新有效），则减小$\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是否超过最大值，若超过则停止迭代
- 如果L' < L_{\min}（代价函数降低）：
-
终止条件：
- 当\lambda超过预设的最大值，或者参数更新的幅度小于阈值时，停止迭代