计算机视觉之三维重建——深入浅出SFM系统与SLAM系统的核心算法

第一章：摄像机几何

1. 针孔模型&透镜

1.1 小孔成像原理

当将胶片直接放置在物体前方时，3D物体上的同一点会在胶片的多个位置产生成像；

因此，在物体和胶片之间放置一个带有针孔的隔板时，假设针孔大小只允许穿过一条光线，那么3D物体上的同一点只能有一条光线穿过小孔并在胶片上成像；

此处的小孔相当于就是光圈，光圈的尺寸越大，会有更多的光线穿过小孔，成像会越亮，但是同一个3D点会有多条光线穿过小孔并在胶片上的多个位置（相近）产生成像，导致物体成像模糊。光圈越小，物体成像越清晰，但是亮度也越暗；

1.2 针孔相机数学模型

$P$为空间3D点，$P^{{}^{\prime }}$为成像后的像平面的2D点

将上面的三维示意图投影到二维平面（$k-j$平面，垂直于$i$轴）后，会产生一对相似三角形:
$$\frac{y^{{}^{\prime }}}{f}=\frac{y}{z}$$
进而得到：
$$y^{{}^{\prime }}=f\frac{y}{z}$$

同理，若将将上面的三维示意图投影到二维平面（$k-i$平面，垂直于$j$轴）后，会产生另一对相似三角形:
$$\frac{x^{{}^{\prime }}}{f}=\frac{x}{z}$$
进而得到：
$$x^{{}^{\prime }}=f\frac{x}{z}$$

1.3 透镜

• 若是采用之前的隔板+小孔的方式，在成像清晰的的情况下，由于通过光线少，图片会非常暗。
• 当增加透镜之后，透镜会将从同一3D反射的多条光线聚焦到胶片上的同一点，增加了图片的亮度；

（1）焦距

• 所有平行于光轴的光线都会汇聚到焦点，焦点到透镜中心的距离成为焦距；
• 穿过中心的光线的方向不会发生改变；

（2）近轴折射模型（带透镜的小孔成像模型）

由于过光心的直线映射关系不变，加不加镜头小孔成像模型依然成立。此时，摄像机的焦距（小孔到成像平面的距离）变为：
$$z^{{}^{\prime }}=f+z_0$$
根据折射定律：
$$f=\frac{R}{2(n-1)}$$
其中，R为透镜球面半径，n为透镜折射系数。

（3）失焦

如上图所示：

• 3D点P在的光线穿过透镜刚好汇聚于胶片上的一点；
• 而比点P离透镜更近的一点的光线会汇聚于胶片后一点，则在胶片上就是发散的；
• 同理，比点P离透镜更远的一点的光线会汇聚于胶片前方一点，则在胶片上也是发散的；

景深：在一定景深范围内，能成清晰的相；离开景深的地方，可能形成虚像，如微距摄像：

（4）径向畸变

径向畸变：图像像素点以畸变中心为中心，沿着径向产生的位置偏差，从而导致图像中所成的像发生形变。其可以分为枕形畸变和桶形畸变两种。

2. 摄像机几何

2.1 摄像机坐标系 - 像素坐标系

（1）3D摄像机坐标系 - 2D CCD坐标系

$$\{\begin{array}{r}{x}^{{}^{\prime }}=f\frac{x}{z}\\ \\ y^{{}^{\prime }}=f\frac{y}{z}\end{array}$$

（2）2D CCD坐标系 - 2D像素坐标系

$$\{\begin{array}{r}u=fk\frac{x}{z}+c_x\\ \\ v=fl\frac{y}{z}+c_y\end{array}$$

CCD坐标系和像素坐标系存在两个不同之处：

• 坐标系原点不一致：CCD坐标系的远点在图像中心，像素坐标系的原点在左上角，$c_x$和$c_y$就是原点的平移量；
• 度量单位不一致：CCD坐标系的单位为$m$，像素坐标系的单位为pixel，$k,l$ 分别为 $x,y$ 方向的度量单位变换量，单位为 $pixel/m$

将 $fk$ 和 $fl$ 表示为 $\alpha 和\beta$可得：

$$\{\begin{array}{r}u=\alpha \frac{x}{z}+c_x\\ \\ v=\beta \frac{y}{z}+c_y\end{array}$$

（3）齐次坐标变换

$$P^{{}^{\prime }}=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}\alpha x+c_{x}z\\ \beta y+c_{y}z\\ z\end{array}\right]=\left[\begin{array}{cccc}\alpha & 0& c_x& 0\\ 0& \beta & c_y& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$$

（4）摄像机偏斜

$$P^{{}^{\prime }}=\left[\begin{array}{cccc}\alpha & -\alpha cot\theta & c_x& 0\\ 0& \frac{\beta }{sin\theta }& c_y& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]=MP=K\left[\begin{array}{cc}I& 0\end{array}\right]P$$

（5）内参数矩阵K

$$K=\left[\begin{array}{ccc}\alpha & -\alpha cot\theta & c_x\\ 0& \frac{\beta }{sin\theta }& c_y\\ 0& 0& 1\end{array}\right]$$

• 其中，$K$是摄像机内参数矩阵，内参数矩阵决定了摄像机坐标系下3D空间点到2D图像点的映射；
• K有个5自由度DOF；

（6）规范化摄像机

$$P^{{}^{\prime }}=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$$

• 规范化摄像机下的3D点的欧式坐标和和像素坐标系下的2D点的齐次坐标一致；
• 此时的3D点对应2D点的欧式坐标为$\left[\begin{array}{c}\frac{x}{z}\\ \frac{y}{z}\end{array}\right]$

2.2 像素坐标系-世界坐标系

（1）投影矩阵

投影矩阵有11个自由度，其中包含5个摄像机内参数+6个摄像机外参数。

（2）投影矩阵性质定理

3. 其他摄像机模型

略

4. 最小二乘解

4.1 线性方程组的最小二乘解

（1）线性方程组

（2）最小二乘解

4.2 齐次线性方程组的最小二乘解

（1）线性方程组

（2）最小二乘解

4.3 非线性方程组的最小二乘解

第二章：摄像机标定

1. 针孔模型&透镜摄像机标定问题

1.1 摄像机标定

（1）标定目标

摄像机标定：求解摄像机内、外参数矩阵 K [R T]；
因为摄像机内外参数矩阵描述了三维世界到二维像素的映射关系；

（2）标定装置

1.2 投影矩阵M求解

（1）标定方程

• 摄像机内外参数共同构成了投影矩阵，投影矩阵共有11个未知量；
• 每对点可以列出两个方程，因此，最少需要6对对应点；
• 实际操作中通常使用多于6对点来获得更加鲁棒的结果；

• 方程个数 $2n$ 个，且 $n>6$；
• 未知参数11个；
• 这是一个超定齐次线性方程组；

（2）投影矩阵M求解

1.2 提取摄像机内参数

$$M=K[RT]=[KRKT]=[Ab]$$

考虑投影矩阵M求解时，假定$||m||=1$，因此在M前面乘了一个$\rho$

（1）提取ρ

从上式第三行可得：

$$\begin{array}{rl}\because & \rho a_3^T=r_3^{T}and|r_3|=1\\ \therefore & |\rho ||a_3|=1\end{array}$$

$$\therefore \rho =\frac{\pm 1}{|a_3|}$$

（2）提取u0,v0

$$\begin{array}{rl}\because & r_1\cdot r_3=0\\ & r_2\cdot r_3=0\\ & r_3\cdot r_3=1\end{array}$$

$$\begin{array}{rl}\therefore & \rho a_1^T\cdot \rho a_3^T\\ & =(\alpha r_1^T-\alpha cot\theta r_2^T+u_0{R}_3^T)\cdot r_3^T\\ & =0-0+u_0\\ & \\ & \rho a_2^T\cdot \rho a_3^T\\ & =(\frac{\beta }{sin\theta }{r}_2^T+v_0{r}_3^T)\cdot r_3^T\\ & =0+v_0\end{array}$$

$$\begin{array}{rl}\therefore & u_0={\rho }^2(a_1\cdot a_3)\\ & v_0={\rho }^2(a_2\cdot a_3)\end{array}$$

（3）提取θ

$$\begin{array}{rl}\because & r_1\times r_2=r_3\\ & r_1\times r_3=r_2\\ & r_3\times r_3=0\\ & |a|=\sqrt{a^T\cdot a}\\ & cot\theta =\frac{cos\theta }{sin\theta }\\ & si{n}^2\theta +co{s}^2\theta =1\end{array}$$

$$\begin{array}{rl}\therefore {\rho }^2(a_1\times a_3)& =(\alpha r_1^T-\alpha cot\theta r_2^T+u_0{R}_3^T)\times r_3^T\\ & =\alpha r_2-\alpha cot\theta r_1\\ {\rho }^2(a_2\times a_3)& =(\frac{\beta }{sin\theta }{r}_2^T+v_0{r}_3^T)\times r_3^T\\ & =\frac{\beta }{sin\theta }{r}_1\end{array}$$

$$\begin{array}{rl}\therefore & {\rho }^2|a_1\times a_3|\\ & =|(\alpha r_2-\alpha cot\theta r_1)|\\ & =\sqrt{(\alpha r_2-\alpha cot\theta r_1{)}^T\cdot (\alpha r_2-\alpha cot\theta r_1)}\\ & =\sqrt{{\alpha }^2+(\alpha cot\theta {)}^2}\\ & =\sqrt{{\alpha }^2(1+(\frac{cos\theta }{sin\theta }{)}^2)}\\ & =\sqrt{(\frac{\alpha }{sin\theta }{)}^2}\\ & =\frac{|\alpha |}{sin\theta }\\ & \\ & {\rho }^2|a_2\times a_3|\\ & =|\frac{\beta }{sin\theta }{r}_1|\\ & =\sqrt{(\frac{\beta }{sin\theta }{r}_1{)}^T\cdot (\frac{\beta }{sin\theta }{r}_1)}\\ & =\sqrt{(\frac{\beta }{sin\theta }{)}^2}\\ & =\frac{|\beta |}{sin\theta }\end{array}$$

$$\begin{array}{rl}\because & {\rho }^2(a_1\times a_3)\cdot {\rho }^2(a_2\times a_3)\\ & =(\alpha r_2-\alpha cot\theta r_1)\cdot (\frac{\beta }{sin\theta }{r}_1)\\ & =-\alpha cot\theta \frac{\beta }{sin\theta }\\ & =-\alpha \frac{cos\theta }{sin\theta }\frac{\beta }{sin\theta }\\ & =-\frac{acos\theta \beta }{si{n}^2\theta }\\ & \\ & {\rho }^2|a_1\times a_3|\cdot {\rho }^2|a_2\times a_3|\\ & =\frac{|\alpha |}{sin\theta }\cdot \frac{|\beta |}{sin\theta }\\ & =\frac{|\alpha ||\beta |}{si{n}^2\theta }\end{array}$$

$$\begin{array}{rl}\therefore & \frac{{\rho }^2(a_1\times a_3)\cdot {\rho }^2(a_2\times a_3)}{{\rho }^2|a_1\times a_3|\cdot {\rho }^2|a_2\times a_3|}\\ & =-\frac{acos\theta \beta }{si{n}^2\theta }\frac{si{n}^2\theta }{|\alpha ||\beta |}\\ & =-cos\theta \end{array}$$

$$\therefore cos\theta =-\frac{(a_1\times a_3)\cdot (a_2\times a_3)}{|a_1\times a_3|\cdot |a_2\times a_3|}$$

如果 $\theta =90^o$，则$cos\theta =0$，所以$(a_1\times a_3)\cdot (a_2\times a_3)=0$，这正时之前提到的投影矩阵M满足零倾斜透视投影的条件；

（4）提取α,β

之前（3）中推导过：
$$\begin{array}{rl}\because & {\rho }^2|a_1\times a_3|=\frac{|\alpha |}{sin\theta }\\ & \\ & {\rho }^2|a_2\times a_3|=\frac{|\beta |}{sin\theta }\end{array}$$

$$\begin{array}{rl}\therefore & \alpha ={\rho }^2|a_1\times a_3|sin\theta \\ & \beta ={\rho }^2|a_2\times a_3|sin\theta \end{array}$$

如果 $\alpha =\beta$，所以 $|a_1\times a_3|=|a_2\times a_3|$，所以 $(a_1\times a_3)\cdot (a_1\times a_3)=(a_2\times a_3)\cdot (a_2\times a_3)$，这也是前面提到的零倾斜透视投影的基础上宽高比为1的另一个条件。

1.3 提取摄像机外参数

（1）提取r3

$$\because \rho a_3^T=r_3^{T}and\rho =\frac{\pm 1}{|a_3|}$$

$$\therefore r_3=\frac{\pm a_3}{|a_3|}$$

（2）提取r1

$$\begin{array}{rl}& \because \rho a_2^T\times \rho a_3^T=(\frac{\beta }{sin\theta }{r}_2^T+v_0{r}_3^T)\times r_3^T\\ & \therefore {\rho }^2(a_2\times a_3)=\frac{\beta }{sin\theta }{r}_1\end{array}$$

因为 $r_1$ 是单位向量，且${\rho }^2，\frac{\beta }{sin\theta }为实数$，
$$\begin{array}{rl}& \therefore r_1=\frac{(a_2\times a_3)}{|a_2\times a_3|}\end{array}$$

（3）提取r2

$$r_2=r_3\times r_1$$

（4）提取T

$$\begin{array}{rl}\rho [Ab]=K[RT]& =[KRKT]\\ \rho b& =KT\\ K^{-1}\rho b& =T\end{array}$$

$$\therefore T=\rho K^{-1}b$$

1.4 摄像机标定结果

2. 径向畸变摄像机标定

2.1 径向畸变摄像机模型

如下图所示的径向畸变情况下，图像的放大率随距离光轴距离增加而减小：

对于桶形畸变来说，$K_p>0$，则$\lambda >1$;
对于桶形畸变来说，$K_p<0$，则$\lambda <1$;

$$\begin{array}{rl}& \therefore \{\begin{array}{l}{u}_i=\frac{1}{\lambda }\frac{m_1{P}_i}{m_3{P}_i}\\ v_i=\frac{1}{\lambda }\frac{m_2{P}_i}{m_3{P}_i}\end{array}\\ & \therefore \{\begin{array}{l}{u}_i\lambda m_3{P}_i=m_1{P}_i\\ v_i\lambda m_3{P}_i=m_2{P}_i\end{array}\\ & \therefore \{\begin{array}{l}{m}_1{P}_i-u_i\lambda m_3{P}_i=0\\ m_2{P}_i-v_i\lambda m_3{P}_i=0\end{array}\end{array}$$

$$Qm=0$$

在第一节中此处列出的公式是:

$$Pm=0$$
P是由3D点$P_i$和2D图像点$p_i$构成的全是已知数的矩阵，$m$为待求解的投影矩阵，所以这是一个线性方程。

而Q里面除了包含由3D点$P_i$和2D图像点$p_i$构成的已知数外，还包含未知数$\lambda$，所以Q不是线性方程组；

此处即然$\lambda$是未知数，为啥不能把$\lambda$写到待求解的$m$里面呢，因为不同点的$\lambda$是不一样的。

2.2 径向畸变投影矩阵求解

由于从初始解开始迭代的话，若初始解与实际相距较远，可能收敛会很慢。
因此，可以求解系统的线性部分，以找到近似解；适用该解作为整个系统的初始条件。

2.3 求解线性部分m1，m2

核心：通过比值消掉未知数$\lambda$。

3. 2D平面变换

3.1 2D欧式变换

3.2 2D相似变换

均匀伸缩变化如下：

相似变换由缩放变换和欧式变换合成：

3.3 2D仿射变换

3.4 2D透视变换

4. 3D空间变换

4.1 3D欧式变换

4.2 3D仿射变换

4.3 透视变换

第三章：单视测量

相机标定后，也无法从2D图像恢复出3D场景，因为2D点与光心连线上的任意3D点都满足条件。

1. 无穷远点、无穷远线与无穷远平面

1.1 2D平面

（1）2D平面直线

（2）2D平面直线交点

（3）2D平面无穷远点

（4）2D平面无穷远线

1.2 3D空间

（1）3D空间的面

（2）3D空间的直线

（3）3D空间无穷远点

（4）3D空间无穷远平面

2. 影消点与影消线

2.1 2D平面无穷远点和线的变换

（1）2D平面无穷远点的变换

（2）2D平面无穷远线的变换

2.2 影消点

2.3 影消点与直线方向

2.4 影消线

2.5 影消线与平面法向量

2.6 总结

3. 单视重构

3.1 两组平行线的夹角与影消点

3.2 w的性质

3.3 单视图标定举例

由于$\omega$具有5个自由度，目前列出的3个方程无法求出$\omega$，因此假定摄像机零倾斜且具有方形像素，则${\omega }_2=0且{\omega }_1={\omega }_3$，此时就可以求出$\omega$，进而分解出$K$。

单视图恢复出的三维场景结构，无法恢复场景的实际比例；

第四章：三维重建基础与极几何

由于从单幅视图恢复场景三维结构比较困难，并且，单视图2D到3D的映射具有多义性，如下图所示，从单视图中很难辨别塔是模型还是实物；

1. 双视图与三角化

只要已知直线$l$和$l^{\prime }$，则三维点P就是两条直线的交点。因此，只要将直线$l$和$l^{\prime }$映射到同一坐标系下，就可以通过$P=l\times l^{\prime }$求出点$P$的三维坐标。

此时，问题演变成了：
已知二维像素坐标 $p$ 和 $p^{\prime }$，$K$ 和 $K^{\prime }$，以及两视图之间的变换矩阵$R$、$T$，
求解P点的三维坐标？

此时存在两种解法：线性解和非线性解；

1.1 线性解

1.2 非线性解

最小化重投影误差

1.3 实际情况中的问题定义

• 在实际情况中，由于噪声的存在，两条直线通常不相交；
• 且上述的线性解和非线性解都需要已知$K$ 和 $K^{\prime }$，$R$、$T$；
• 然而，实际情况中，摄像机的内参$K$ 和 $K^{\prime }$和两视图之间的变换矩阵$R$、$T$通常不可知，那么实际情况就变成了如下的问题：
  • 问题1：已知$p$和$p^{\prime }$，摄像机内参数$K$和$K^{\prime }$，
  • 求解：摄像机的$R$、$T$以及P点的三维坐标？
  • 问题2：已知$p$和$p^{\prime }$
  • 求解：摄像机内参数$K$和$K^{\prime }$，摄像机的$R$、$T$以及P点的三维坐标？

1.4 多视图几何的关键问题

• 摄像机几何：从一张或者多张图像中求解摄像机的内、外参数；
• 场景几何：通过二至多幅图寻找3D场景坐标；
• 对应关系：已知一个图像中的$p$点，如何在另外一个图像中找到$p^{\prime }$点；

2. 极几何与基础矩阵

2.1 极几何

极几何描述了同一场景或者物体的两个视点图像间的几何关系；

极几何可以将对应点搜索范围缩小到对应的极线上；

（1）极几何特例——平行视图

（2）极几何特例——前向平移（无旋转）

2.2 本质矩阵

本质矩阵对规范化摄像机拍摄的两个视点图像间的极几何关系进行代数描述；

$p^{\prime }$和$O_2$在$O_1$坐标系下的坐标为：
$$\begin{array}{rl}{p}^{\prime }& =R{p}^{{}^{\prime }\ast }+T\\ p^{{}^{\prime }\ast }& =R^T(p^{\prime }-T)\\ & =R^T{p}^{\prime }-R^{T}T\end{array}$$
$$\begin{array}{rl}\because O_2& =[000{]}^T\\ O_2^{\ast }& =-R^{T}T\end{array}$$

对两个向量进行叉乘：
$$\begin{array}{rl}{O}_1{p}^{{}^{\prime }\ast }\times O_1{O}_2^{\ast }& =(R^T{p}^{\prime }-R^{T}T)\times (-R^{T}T)\\ & =R^{T}T\times R^T{p}^{\prime }\end{array}$$

得到的向量垂直于 $O_1{O}_{2}P$ 极平面，因此可得：
$$\begin{array}{rl}& [R^{T}T\times R^T{p}^{\prime }{]}^T\cdot p=0\\ & [R^T\cdot (T\times p^{\prime }){]}^T\cdot p=0\\ & (T\times p^{\prime }{)}^T\cdot R\cdot p=0\\ & ([T_{\times }]\cdot p^{\prime }{)}^T\cdot R\cdot p=0\\ & p^{{}^{\prime }T}\cdot [T_{\times }]\cdot R\cdot p=0\\ & p^{{}^{\prime }T}\cdot [T\times R]\cdot p=0\\ & p^{{}^{\prime }T}\cdot E\cdot p=0\end{array}$$

最终，本质矩阵$E=T\times R=[T_{\times }]R$

2.3 基础矩阵

基础矩阵对一般的透视摄像机拍摄的两个视点的图像间的极几何关系进行代数描述。

推导思路：将一般相机变换到规范化相机，用本质矩阵来表达；
$$\begin{array}{rl}p& =K[I0]P\\ K^{-1}p& =[I0]P\end{array}$$
则规范化相机下左右两视图的像素点坐标演变为：
$$\begin{array}{rl}{p}_c& =K^{-1}p\\ p_c^{{}^{\prime }}& =K^{{}^{\prime }-1}{p}^{\prime }\end{array}$$
带入本质矩阵：
$$\begin{array}{rl}{p}_c^{{}^{\prime }T}E{p}_c& =p_c^{{}^{\prime }T}\cdot [T_{\times }]R\cdot p_c\\ & =(K^{{}^{\prime }-1}{p}^{\prime }{)}^T\cdot [T_{\times }]R\cdot K^{-1}p\\ & =p^{{}^{\prime }T}{K}^{{}^{\prime }-T}[T_{\times }]R{K}^{-1}p\\ & =p^{{}^{\prime }T}Fp\end{array}$$

最终基础矩阵F为：
$$F=K^{{}^{\prime }-T}[T_{\times }]R{K}^{-1}$$

3. 基础矩阵估计

3.1 八点法

$$p^{{}^{\prime }T}Fp=0$$

F有7个自由度，理论上7点可以解出F，但计算比较复杂;

（1）单点方程矩阵形式展开：

（2）选取八个点，列出齐次线性方程：
（3）最小二乘求解：

通常$N>8$，为超定方程组，采用最小二乘求解：

（4）满秩矩阵分解：

上述经过最小二乘估计得到的$\hat{F}$通常秩为3，为满秩矩阵，但基础矩阵$F$的秩为2。

（5）八点法求解总结

（6）八点法存在的问题

• W矩阵中各个元素的数值差异过大；
• SVD分解有数值计算问题
• 导致求解结果通常有10+pixel左右的误差；

3.2 归一化八点法

对每幅图像施加变换T（平移与缩放），让其满足如下条件：

• 原点 = 图像上点的重心；
• 各个像点到坐标原点的均方根距离等于$\sqrt{2}$;

归一化八点法的计算步骤如下：

归一化八点法的精度高，推荐使用；

4. 单应矩阵

单应矩阵描述空间中的平面在两个摄像机下的投影几何；

4.1 单应矩阵推导

4.2 单应矩阵估计

4.3 单应矩阵性质

第五章：双目立体视觉

1. 基于平行视图的双目立体视觉

1.1 平行视图基础矩阵

由于 $e^{\prime }$ 是 $O_2$ 在右视图的投影，可以列出如下等式：

给予叉乘性质（对于任何向量$\alpha$，如果$B$可逆，相差一个尺度情况下）：
$$[a_{\times }]B=B^{-T}[(B^{-1}\alpha {)}_{\times }]$$

另 $\alpha =T$，$B=K^{{}^{\prime }-1}$，则：
$$\begin{array}{rl}[T_{\times }]K^{{}^{\prime }-1}& =K^{{}^{\prime }T}[(K^{\prime }T{)}_{\times }]\\ [T_{\times }]& =K^{{}^{\prime }T}[(K^{\prime }T{)}_{\times }]K^{\prime }\end{array}$$

将$[T_{\times }]$带入到基础矩阵F中：
$$\begin{array}{rl}F& =K^{{}^{\prime }-T}[T_{\times }]R{K}^{-1}\\ & =K^{{}^{\prime }-T}{K}^{{}^{\prime }T}[(K^{\prime }T{)}_{\times }]K^{\prime }R{K}^{-1}\\ & =[(K^{\prime }T{)}_{\times }]K^{\prime }R{K}^{-1}\\ & =[e_{\times }^{\prime }]K^{\prime }R{K}^{-1}\end{array}$$

最终推导得到：
$$F=[e_{\times }^{\prime }]K^{\prime }R{K}^{-1}$$

在平行视图中，满足以下条件：

• 相机相同：$K=K^{\prime }$；
• 无旋转：$R=I$；
• 仅$x$方向平移：$T=\left[\begin{array}{c}T\\ 0\\ 0\end{array}\right]$；
• 无穷远极点：$e^{\prime }=\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]$；
  因此：
  

1.2 平行视图极几何

（1）平行视图极线

极线是水平的，平行于$u$轴！

（2）平行视图对应点搜索

对应点 $p$ 和 $p^{\prime }$ 的 $v$ 坐标是一样的；
因此，$p^{\prime }$点只需要在$p$点的$v$坐标所在的线寻找即可；

1.3 平行视图三角测量

利用三角形相似原理，可以得到
$$\frac{p_u-p_u^{\prime }}{f}=\frac{B}{Z}$$
$$p_u-p_u^{\prime }=\frac{B\cdot f}{Z}$$

由此可得：视差与深度Z成反比！
也就是说，物体离人眼越远，左右眼观察到的图像越相似；

“视差与深度Z成反比”这个结论可以方便我们从视差图中推导得到深度图。（如下图所示，视差图颜色越暗，距离双目摄像机越远）

2. 图像校正

在平行视图中，可以很方便利用视差获取深度图，但是，再实际构建的双目立体视觉系统中，如何保证两个视图是完全平行的呢，这就需要进行图像校正。

校正结果如下：

3. 对应点搜索

图像校正，$p^{\prime }$点直接演着扫描线寻找即可；

3.1 相关匹配

相关法不适用于亮度变化明显的case；

3.2 归一化相关匹配

匹配时采用的窗口大小的影响：

• 当窗口较小时，细节比较丰富，但噪声更多；
• 当噪声较大时，视差图更平滑，噪声更少，但丢失了细节；

3.3 相关法存在问题

（1）透视缩短

（2）遮挡

（3）基线选择

• 为了减少透视缩短和遮挡的影响，希望有更小的B/Z（基线深度比）比值；
• 但是，当B/Z太小时，对应点匹配的小误差意味着估算深度的大误差；
  

（4）当存在同质区域或者重复模式时，相关法会失效；

第六章：多视图几何

1. 运动恢复结构问题

三种典型的运动恢复结构任务包括：

• 欧式结构回复（摄像机内参数已知，外参数未知）
• 仿射结构恢复（摄像机为仿射像机，内外参数均未知）
• 透视结构恢复（摄像机为透视相机，内外参数均未知）

2. 欧式结构恢复

2.1 欧式结构恢复问题定义

2.2 两视图欧式结构恢复

• 以左视图为参考坐标系，则左视图$[RT]=[I0]$，并且摄像机内参数已知，那么左视图投影矩阵$M_1$已知；
• 当右视图的投影矩阵也已知时，便可以利用三角化（线性解或非线性解）求解得到三维点的坐标；
• 但是，我们只知道右视图的摄像机内参数，并不知道右视图相对于左视图的外参数$[RT]$，因此无法直接使用三角化求解，必须先求解右视图的$[RT]$。

具体求解步骤如下：

（1）求解基础矩阵F

（2）求解本质矩阵

（3）分解本质矩阵——求解$[RT]$

（4）三角化

2.3 欧式结构恢复歧义

歧义：恢复的结构与真实场景之间相差一个相似变化（旋转、平移、缩放）；

度量重构：这种恢复的场景与真实场景之间仅存在相似变换的重构称为度量重构；

3. 仿射结构恢复

3.1 仿射结构恢复问题定义

3.2 基于因式分解的仿射结构恢复

（1）数据中心化

（2）因式分解

3.3 仿射结构恢复歧义

$$D=MS=(MH)(H^{-1}S)=M^{\ast }{S}^{\ast }$$

仿射结构恢复分解不唯一，存在歧义，歧义可以有任意的$3\times 3$可逆矩阵H表达；

4. 透视结构恢复

4.1 代数法（两视图）

（1）求解基础矩阵F

（2）估计摄像机矩阵$M_1$、$M_2$

（3）三角化

4.2 代数方法（多视图）

分别对每一个图像对计算运动与结构，然后进行增量法构建：

4.3 光束法平差

• 在仿射变换结构恢复所采用的因式分解法中，需要假定所有3D点在所有摄像机均是可见的，因此当存在遮挡，或者建立对应点关系失败时，能够用于构建观测矩阵D的点很少，重建出来的点数就很少；
• 透视结构恢复采用代数法求解，容易出现误差的累积；

光束法平差通过最小化重投影误差来恢复运动和结构，是一种非线性解法；

第七章：运动恢复结构（SFM）系统设计

1. PnP问题（补充知识）

1.1 P3P求解摄像机位姿

2. SFM系统描述

2.1 SFM系统数学描述

3. 两视图SFM重构（两视图欧式结构恢复）

3.1 整体流程

3.2 RANSAC估计基础矩阵

4. 基于增量法的SFM系统（OpenMVG）

第八章：SLAM系统设计

1. SLAM介绍

Simultaneous Localization and Mapping(SLAM)：同时定位和建图

• Localization：传感器的位置和姿态；
• Mapping：地图构建
• 应用场景：定位、导航、避障、重建、交互

SLAM传感器分类：

• 携带于机器人本体的，例如轮式编码器、相机、激光等；
  安装于环境中的，如导轨、二维码标识等；

开源SLAM方案

2. ORB-SLAM系统

同时运行三个线程：

• 跟踪：确定当前帧位姿；
• 建图：完成局部地图构建；
• 回环修正：回环检测以及基于回环信息修正系统漂移；

2.1 关键词介绍

（1）地图点

• 世界坐标系下的3D点坐标；
• 观测方向，即所有可以观测到该特征点的视图所产生的观测方向均值；
• ORB特征描述子；
• 该点能被观测到的最大距离与最小距离；

（2）关键帧

• 摄像机位姿；
• 内参数；
• 该帧中提取到的全部ORB特征描述子，以及它们和地图点之间的对应关系；

（3）共视图

一种有向无权图，节点为关键帧，如两个节点共享的地图点数量大于阈值（至少15个），则存在一条边，边的权重设置为共享地图点的个数。

（4）本质图

是共视图的子图，保留所有节点，边数量相较于共视图更少，尽量减少边，其作用是加速回环校正的计算。
本质图=生成树+共视图边权重超过100的边+回环边

2.2 跟踪线程

跟踪（Tracking）：给定当前帧，从图像提取ORB特征，根据上一帧估计当前帧姿态，估计失败时，尝试全局重定位初始化位姿，构建局部地图进一步优化位姿，并确定是否设置成关键帧。

2.3 建图线程

建图（LocalMapping）：完成局部地图构建。包括对关键帧的插入，验证最近生成的地图点并进行筛选，然后生成新的地图点，实用局部光束法平差（Local BA），最后再对插入的关键帧进行筛选，去除多余的关键帧。

2.4 回环修正线程

回环修正（LoopClosing）：包含闭环检测和闭环校正两步。闭环探测先使用词袋模型找到闭环图片，然后通过Sim3算法计算相似变换。闭环校正主要是闭环融合和Essential Graph的图优化。