SLAM基础知识总结

1. 视差与深度信息

• 根据双目成像原理，两幅图像摄像机光心位置不同，而且可以看到场景中同一个场景点，则根据此场景点在两幅图像中的**#F00图像坐标（Film Coords/Image Plane）之差（Disparity/Parallax）及两摄像机光心之间的距离(Baseline)**算出场景点的深度信息
• 获取视差图：双目或运动(Stereo or Motion)
• 基本原理：三角形相似原理
  

2. 3D世界坐标点->2D像素坐标(正向投影)

• 为了进行计算，必须把3D世界坐标点变换到2D像素坐标的过程使用一个数学模型进行描述，最好的描述方式当然是矩阵，它具有平移、旋转、绽放、投影等功能。
• 整个正向投影变换过程如下图所示：
  
• 整个变换使用$M_{ext}$和$M_{int}$可以表示为：
  $$\left[\begin{array}{c}u\\ v\\ w\end{array}\right]=\left[\begin{array}{cccc}{f}_x& 0& o_x& 0\\ 0& f_y& o_y& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_x\\ r_{31}& r_{32}& r_{33}& t_x\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}U\\ V\\ W\\ 1\end{array}\right]$$
  $$\begin{gathered} f_x=f/s_x \\ {f}_y=f/s_y \end{gathered}$$
• 采有分块矩阵，其表示如下：
  $$\left[\begin{array}{c}u\\ v\\ w\end{array}\right]=\left[\begin{array}{cc}{K}_{3x3}& 0_{3x1}\end{array}\right]\left[\begin{array}{cc}{R}_{3x3}& T_{3x1}\\ 0_{1x3}& 1_{1x1}\end{array}\right]\left[\begin{array}{c}{P}_{w3x1}\\ 1\end{array}\right]$$
  $$\left[\begin{array}{c}u\\ v\\ w\end{array}\right]=\left[\begin{array}{cc}{K}_{3x3}& 0_{3x1}\end{array}\right]\left[\begin{array}{c}R{P}_w+T\\ 1\end{array}\right]=K(R{P}_w+T)$$

2.1 世界坐标$\to$摄像机坐标（$M_{ext}$）

• 在实际问题中， 场景点使用世界坐标进行描述，这样符合我们对环境的认知，因为环境是相对不变，而摄像机总是不停地运动。
• 基本原理：刚体变换=平移（T）+旋转®
• T: 此T不等于摄像机光心在世界坐标系中的位置，具体值见下面的公式
  
  

2.2 摄像机坐标$\to$图像坐标（$M_{proj}$）

• 摄像机坐标$\to$图像坐标是3D到2D的透视投影变换
  

2.3 图像坐标$\to$像素坐标($M_{aff}$)

2.3.1 内部参数-偏移量（Intrinsic Parameters-Offsets）

• 坐标变换：描述图像坐标与像素坐标间的坐标变换
• 数字图像：把图像坐标系中的图像经过扫描、数字化、重采样便生成了像素坐标图像
• 内部参数： 偏移量（Offset)
  

2.3.2 内部参数-尺度变换(Intrinsic parameters -scales)

• 内部参数：尺度变换（Scale: 即宽高比被改变了）
• 数字图像行/列数：由采样决定
  

2.3.3 仿射变换 (Affine Transformation)

• 仿射变换：内部参数-偏移量与内部参数尺度变换的统称，它是由6个参数定义的2D$\to$2D的变换，其变换矩阵如下图所示
  $$M_{aff}=\left[\begin{array}{ccc}1/s_x& 0& o_x\\ 0& 1/s_y& o_y\\ 0& 0& 1\end{array}\right]$$

2.4 内部参数变换 (M_{int})

• M_{ext}: 是由摄像机外部参数（R & T)决定的变换矩阵，所以称为外部参数变换矩阵
• M_{int}: 是由摄像机内部参数【f（$f_x=f/s_x,f_y=f/s_y$）, Offsets($o_x,o_y$), Scales($s_x,s_y$) 】决定的，所以称为内部参数变换矩阵
• 内部参数变换矩阵：是透视投影变换与仿射变换的组合
  

2.5 3D世界到2D图像的总结

• $x=PX$：$x$为2D数字图像点(2D image point)，$P$为摄像机矩阵（Camera Matrix)，$X$为3D世界场景点（3D World Point）
• 没有平移和旋转的情况， 如下图所示：
  
• 有平移和旋转的情况，如下图所示：
  

3. 共面点成像(单应性矩阵：Homography)

3.1 单应矩阵定义

• $x^{\prime }=Hx$

3.2 目的

• 研究共面点成像的目的：为了研究两幅图之间的关系，其关系为：$x^{\prime }=Hx$
• 在知道了A、B图像之间的单应矩阵之后，就可以根据A中一点计算出在B中对应的点
• 共面点成像：即所有的场景点都处在同一个平面上，这是通用透视投影成像的特殊情况
• 可以3D$\to$2D的变换简化为2D$\to$2D的变换，且此变换是可逆的，这对求场景点的3D坐标非常方便

3.3 场景点共面的成像矩阵

3.4共面点组成的平面垂直光轴（摄像机坐标Z轴）

• 当共面点组成的平面垂直光轴时，此变换可以进一步简化
  

3.5 单应矩阵的用途

• 如果x和x’分别是场景点$P_w$在image1和image2上的像素点，则可以得知x’与x的变换关系

• 在下面的计算中，$H_1和H_2$可逆是关键，非共面点的投影可没有这种特殊待遇
  $$\begin{gathered} x=\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]x^{\prime }=\left[\begin{array}{c}{u}^{\prime }\\ v^{\prime }\\ 1\end{array}\right]P_w=\left[\begin{array}{c}p\\ q\\ 1\end{array}\right] \\ x=H_1{P}_w\Rightarrow H_1^{-1}x=P_w \\ {x}^{\prime }=H_2{P}_w\Rightarrow H_2^{-1}{x}^{\prime }=P_w \\ {H}_1^{-1}x=H_2^{-1}{x}^{\prime }\Rightarrow x^{\prime }=H_2{H}_1^{-1}x\Rightarrow \\ {x}^{\prime }=Hx \end{gathered}$$

• 根据Image1(x)Image2(x’)图像对应点对(可通过ORB匹配求出)，则可以求出H

• 求出H之后，就可以求出$P_w$??

４. 对极几何(Epipolar Geometry)

４.１目标

• 描述两张照片间的对应关系：用两个相机在不同的位置拍摄同一物体，如果两张照片中的景物有重叠的部分， 则这两张照片之间存在一定的对应关系
• 描述工具：对极几何（Epipolar Geometry）,它是美丽的对极几何，可用于描述多视图间的几何关系
• 对何几何的用途：　寻找两幅图像之间的对应关系，最直接的方法就是逐点匹配，如果加以一定的约束条件对极约束(epipolar constraint)，搜索的范围可以大大减小，且减少到一条线
• 通用的双目成像：本文前面描述的双目成像和共面点成像，对摄像机位姿和内部参数都有要求，研究起来相对简单，现在开始研究通用双目成像，需要使用对极矩阵和基本矩阵来描述：
• 对极矩阵：两个摄像机的位姿无任何约束关系（即任意R & T），需要使用对极矩阵（Epipolar Matrix）来描述
• 基本矩阵：两个摄像机内部参数不同或未，需要使用基本矩阵（Fundamental Matrix）来描述

4.2 对极几何关键概念

• 极点Epipoles：

• 极点$e_l$：右相机光心$O_r$在左像平面上的像

• 极点$e_r$：左相机光心$O_l$在右像平面上的像
  

• 极平面（Epipolar Plane）：由左右光心$O_l,O_r$, 场景点P三点确定的平面

• 极线（Epipolar Line）：极平面与左右像平面的交线，如$p_l{e}_l$和$p_r{e}_r$

• 极线约束（Epipolar Constraint）：对应点在共轭极线上（如$p_l{e}_l$和$p_r{e}_r$）
  －极线约束的用途：已知场景点Ｐ在左图的像点$p_l$，则其在右图的像点一定在对应的极线$p_r{e}_r$上，这大大减少了$p_l$在右图中寻找匹配点的搜索范围，减少为区区一条线

５. 本质矩阵（Essential Matrix）摄像机坐标

• 本质矩阵定义：在摄像机坐标系中，描述两幅视图（为了方便， 经常描述为左右图，其实只要是任意两幅有重叠部分的图像即可）中的物点矢量间的约束关系
• 本质矩阵的用途：大大减少匹配搜索空间，即缩减为在一条线上进行搜索
• 摄像机$\to$摄像机的变换类似世界坐标$\to$摄像机坐标的变换
  

5.1 物点矢量($P_l,P_r$)间的本质矩阵(E)-摄像机坐标

• 根据向量的减法:
  $P_r^{\prime }$ = $P_l$-T
• 把向量$P_r^{\prime }$进行旋转，可得：
  $P_r=R{P}_r^{\prime }=R(P_l-T)$
• R是正交矩阵，则R可逆：
  $P_l-T=R^{-1}{P}_r=R^T{P}_r$
• 三向量（$P_l,T,P_l-T$）共面，它们的混合积为零(混合积对应于有向体积)：
  
  $(P_l-T{)}^T\cdot T\times P_l=0\Rightarrow (R^T{P}_r{)}^T\cdot T\times P_l=0\Rightarrow$
  $P_r^{T}R\cdot T\times P_l=0$
• 把向量表示为把对称矩阵的形式，从而把叉乘可以写为矩阵的形式：
  
• 本质矩阵的由来
  
  $P_r^{T}R\cdot T\times P_l=0\Rightarrow$
  $P_r^{T}R\cdot S{P}_l=0\Rightarrow$
  $P_r^{T}RS{P}_l=0\Rightarrow$
  $P_r^TEP_l = 0 $
  上式中的E即为本质矩阵，它等于RS(S是向量T的对称矩阵表示形式)，即E只依赖摄像机外部参数（R & T)
• 本质矩阵的属性：
• 它的秩为2
• 它只依赖摄像机外部参数(R & T)
• 它定义了在摄像机坐标系中，左右视图之间**物点矢量($P_l,P_r$)之间的约束关系，以及像点矢量($p_l,p_r$)**间的约束关系
• 本质矩阵(E)定义的左右图约束关系是什么？
  -$P_r$位于$E{P}_l$定义的在右图的极线上，左视图上的一点（$P_l$）乘本质矩阵($E$)的结果为$P_l$在右图上对应的极线
  -$P_l$位于$E^T{P}_r$定义的在左图的极线上，本质矩阵($E^T$)乘右视图上的一点（$P_r$）的结果为$P_r$在左图上对应的极线
  -现在明白极线的用途了吧！知道左图上的一点，就可以找到其在右图上对应的极线，如果要搜索左图上此点在右图上的匹配点，则在此对应的极线上搜索即可

5.2 像点矢量($p_l,p_r$)间的本质矩阵(E)-摄像机坐标

• 在摄像机坐标系中，描述像点矢量$p_l,p_r$间的约束关系 （下面的等式中，$Z_r,f_r,Z_l,f_l$均为常数）
  
• 在图像平面中的点可以被看作为观察射线，下面的描述是等价的：
  • (u,v) 2D film point （图像平面上的2D点）
  • (u,v,f) 3D point on film plane (图像平面上的3D点)
  • k(u,v,f) viewing ray into the scene （看场景的观察射线）
  • k(X, Y, Z) ray through point P in the scene [设: k=f/Z, 则：u=fX/Z, v=fY/Z]. （在场景中通过点P的射线）
• 极线方程
  
• 总结：物点矢量间的本质矩阵与像点矢量间的本质矩阵相等 ，即摄像机坐标系下的本质矩阵（E）只有一个。

5.3 本质矩阵-对极几何描述

• 基本原理：三个矢量（x, x’, T）共面
  
• 点x和x’位于归一化图像坐标：归一化图像坐标的原点在图像的光心，x轴被$f_x$归一化，y轴被$f_y$归一化，因此它是没有尺寸概念的
  
• 本质矩阵(E）的属性
  

6. 基本矩阵 (Fundamental Matrix)像素坐标

• 由于本质矩阵只能描述摄像机坐标系下两个视图间的约束关系，所以它只依赖摄像机的外部参数(R & T)
• 如果需要描述像素坐标系下两个图像间的约束关系，它必须在基本矩阵的基础上再进行仿射变换，即依赖内部参数（$f_x=f/s_x,f_y=f/s_y,o_x,o_y$)和外部参数(R & T)
  
• 基本矩阵定义：$F=M_r^{-T}E{M}_l^{-1}$
• 基本矩阵的特性：
• 它的秩为2
• 它依赖内部参数（$f_x=f/s_x,f_y=f/s_y,o_x,o_y$)和外部参数 (R & T)

6.1 基本矩阵-对极几何描述

• Camera resectioning：是估计一个针孔相机模型的参数的过程，针孔相机参数为3x4的矩阵，叫做Camera Matrix。这个过程叫做Camera Calibration。
  

• $RT$ (摄像机外部参数): 表示从3D世界坐标系到3D摄像机坐标系的坐标系统变换，同时，它也定义了在世界坐标系中，摄像机光心的位置和摄像机的方向

• $T$ (世界坐标系原点的位置)：它表示世界坐标系原点在摄像机坐标系中的位置；它经常被误认为是摄像机的位置

• $C$ (摄像机位置)：它表示摄像机光心在世界坐标系中的位置。
  $T=-RC \Rightarrow C=-R^{-1}T=-RTT $ (R是旋转矩阵，即行列式为+1的正交矩阵)

• 公式推导过程：
  $\bar{x}=Kx{\bar{x}}^{\prime }=K^{\prime }{x}^{\prime }\Rightarrow$
  $x=K^{-1}\bar{x}{x}^{\prime }=K^{\prime -1}{\bar{x}}^{\prime }\Rightarrow$

  $ {x’^TEx = 0} \Rightarrow$

  ${\bar{x}}^{\prime T}{K}^{\prime -T}E{K}^{-1}\bar{x}=0\Rightarrow$

  ${\bar{x}}^{\prime T}F\bar{x}=0\Rightarrow$

  $F=K^{\prime -T}E{K}^{-1}\Rightarrow$

  $E=K^{\prime T}FK$

• 基本矩阵(F)的属性
  

7. 三个矩阵对比分析（单应+本质+基本）

• F是E的推广：把假设"校准的摄像机"去掉了
• $x$：图像坐标系中的点
• $\bar{x}$：像素坐标系中的点
• 本质矩阵（E）+基本矩阵(F)：用于描述2个视图间的对极几何关系
• 变换种类：（1）透视变换：world$\to$camera (2) 射影变换：image$\to$image
• points aligned to camera coordinate axis
  
  | 矩阵属性 | 本质矩阵 (Essential) | 基础矩阵 (Fundamental) | 单应矩阵 (Homography) |
  |:-------|:----------|:-----|:-----|
  |用途 | 把图像点映射到极线 | 把像素点映射到极线) | 把像素点映射到像素点 |
  |矩阵定义 |$E=RS=R[t_{\times }]$ |$F=K^{\prime -T}E{K}^{-1}$ | |
  |相互关系 |$E=K^{\prime T}FK$ |$F=K^{\prime -T}E{K}^{-1}$ | |
  |映射方程 | $l^{\prime }=Ex$ | ${\bar{l}}^{\prime }=F\bar{x}$ | ${\bar{x}}^{\prime }=H\bar{x}$ |
  |映射关系 |$x^{\prime T}Ex=0$ |${\bar{x}}^{\prime T}F\bar{x}=0$ |${\bar{x}}^{\prime }=H\bar{x}$ |
  |坐标系 |图像坐标系(归一化的) |像素坐标系|像素坐标系|
  |摄像机 |校准摄像机 |未校准摄像机|未校准摄像机|
  |矩阵维数 |$3\times 3$ |$3\times 3$ |$3\times 3$ |
  |矩阵的秩 |2(不可逆) |2(不可逆) |3(可逆) |
  |矩阵自由度 |5 | 7 | 8 |
  |依赖参数 | 外参(R&T) | 内参（$f_x,f_y,c_x,c_y$)和外参(R&T) | ? |
  |场景结构 |独立于场景结构(即任意场景) |独立于场景结构(即任意场景) | 场景点在同一平面或离相机很远，是基本矩阵的特殊情况 |
  |计算方法1|仅由两幅图的匹配点计算(5点算法) |仅由两幅图的匹配点计算(8点算法) |仅由两幅图的匹配点计算 |
  |用途|可求出R & T |可求出E，但必须已知内参K| |
  | | | | |

参考:

1. CSE/EE 486: Computer Vision I Fall 2007 Course Overview
2. 16-385 Computer Vision Carnegie Mellon University (Kris Kitani)