近年工作中，唯一能和之前所学的机器人方向比较有关联的，就是虚拟拍摄系统中的视觉定位系统了。于是借着这个机会，在这一年多的时间里，断断续续，又学习和回顾了一些和SLAM比较相关的知识。俗话说的好：“好记性不如烂笔头，烂笔头不如电子档，电子档不如写在网络上”。于是，我就决定将我的好记性记录一下。
虚拟拍摄的本质核心是要将实拍摄像头的参数信息，实时传递到虚拟场景中，让虚拟摄像机与其参数同步。因此本质上就是一个SLAM的问题，即：计算实拍相机到底在哪里（定位），以及这个场景里有哪些东西是我的定位参考（建图）。对于SLAM的东西，个人觉得理解起来确实有一定的难度。所以我自己的一些理解可能也会是错的。以后发现了，慢慢再来修正，也希望有人能指出我的一些错误。OK，Let‘s GO！
关于相机模型
建立相机模型，主要是为了讨论一下，摄像机的内外参数问题，也就是空间一点\(P(X，Y，Z)\)在以下几个坐标系的转换：
- 世界坐标系->相机坐标系
- 相机坐标系->成像平面坐标系
- 成像平面坐标系->像素坐标系
通常的模型图示如下图1所示，空间点\(P\)通过光心\(O\)在成像平面形成倒立的像。但是我们通常获取到的数字图像其实也是正的，不是反的。因此，我们可以将这个模型，简单的转动一下，把成像平面转到前面来（如图2所示），这样有助于我们公式的推导与理解。
设空间一点\(P\)的世界坐标为\(P（X，Y，Z）\)。则其在相机坐标系的表示为：
$$
\begin{pmatrix}
x\\y\\ z\\1
\end{pmatrix}=(R|T)\begin{bmatrix}
X\\Y\\ Z\\ 1
\end{bmatrix}
$$
其中，\(R, T\)表示由世界坐标系到相机坐标系的旋转矩阵和平移向量。之后，根据相似三角形的性质，我们可以得出以下等式：
$$
\frac{x}{x^{‘}}=\frac{z}{f}
\Rightarrow
x^{‘}=\frac{fx}{z}\\
\frac{y}{y^{‘}}=\frac{z}{f}
\Rightarrow
y^{‘}=\frac{fy}{z}
$$
其中\(f\)是相机的焦距信息。当然，一般而言，从相机坐标系转换到成像平面的过程中，存在一个尺度问题, 即物理距离到像素的比例，假设\(x^{‘}\)方向的比例为\(\alpha\), 而\(y^{‘}\)方向的比例为\(\beta\)，则可以定义：
$$
f_{x}=\alpha f\\ f_{y}=\beta f
$$
那么，公式x可以写为：
$$
x^{‘}=\frac{f_{x}x}{z}\\ y^{‘}=\frac{f_{y}y}{z}
$$
此时，我们得到了成像平面上，\(P\)对应点\(P^{‘}\)的像素坐标值。但是，目前坐标值是以图像中心为原点的，而通常我们处理的图像是以图像的一个角，例如右上角作为原点的，这个坐标系，一般称为uv坐标，因此，需要将\(x^{‘}, y^{‘}\)进行一个平移操作：
$$
u=x^{‘}+u_{0}\\ v=y^{‘}+v_{0}
$$
整合以上各式，用齐次坐标来表示，则：
$$
\begin{bmatrix} u\\ v \\ 1 \end{bmatrix}=
\begin{bmatrix}
\alpha & 0 & u_{0} \\
0 & \beta & v_{0}\\
0 & 0 & 1 \end{bmatrix}
\frac{1}{z}
\begin{bmatrix}
f &0 &0 &0 \\ 0&f &0 & 0\\ 0&0 &1 & 0 \end{bmatrix}
(R|T) \begin{bmatrix} X\\ Y \\ Z \\ 1 \end{bmatrix}
$$
整理一下公式，可以化为：
$$
\begin{bmatrix} u\\ v \\ 1 \end{bmatrix}=
\frac{1}{z}\begin{bmatrix}
f_{x}\alpha & 0 & u_{0} \\
0 & f_{y}\beta & v_{0}\\
0 & 0 & 1 \end{bmatrix}
(R|T) \begin{bmatrix} X\\ Y \\ Z \\ 1 \end{bmatrix}=\frac{1}{z}KP\begin{bmatrix} X\\ Y \\ Z \\ 1 \end{bmatrix}
$$
其中，\(K\)被称为内参矩阵，\(P\)被称为外参矩阵。
对极几何
关于对极几何，主要简单分析一下基本概念和一些公式，因为如果是视觉SLAM的问题，可以理解为，已知连续运动的相机的画面中匹配的特征位置，求相机的运动（定位），以及特征在空间中的位置（建图）这样一个问题。对于单目相机来说，对极几何的知识可能是有用的。
Essential Matrix & Fundamental Matrix
对极几何的通常形式，如下图所示：
在图中\(P\)是空间中的一点，\(O_{1}\)和\(O_{2}\)分别是相机在不同位置时的光心位置。我们以\(O_{1}\)为参考系来分析，则点\(P(X,Y,Z)\)在1成像平面的投影\(p_{1}\)满足：
$$
Zp_{1}=KP
$$
\(K\)为内参矩阵。同理，在\(O_{1}\)的参考系下，在2成像平面的投影\(p_{2}\)为：
$$
Zp_{2}=K(RP+t)
$$
\(R,t\)是将\(O_{2}\)变换到\(O_{1}\)的旋转和平移。注意：当前的\(p_{1}, p_{2}\)是以像素坐标表示的：
$$
p_{1}=\begin{bmatrix} u_{1} \\ v_{1} \\ 1 \end{bmatrix} \\
p_{2}=\begin{bmatrix} u_{2} \\ v_{2} \\ 1 \end{bmatrix}
$$
将其去内参处理，得到：
$$
\left\{\begin{matrix}
x_{1}=K^{-1}p_{1}\\
x_{2}=K^{-1}p_{2}\\
\end{matrix}\right.~~~where:x_{2}=Rx_{1}+t
$$
此时的\(x{1},x{2}\)表示的是点\(P(X,Y,Z)\)的两个投影，在相机坐标系下的坐标，其形式为：
$$
x_{1}=\begin{bmatrix} x_{1c}\\ y_{1c} \\ z_{1c} \end{bmatrix}\\
x_{2}=\begin{bmatrix} x_{2c}\\ y_{2c} \\ z_{2c} \end{bmatrix}
$$
之后，在\(x_{2}=Rx_{1}+t \)上用\(t\)做叉乘然后化简：
$$
t\otimes x_{2}=t \otimes Rx_{1}+t\otimes t\\
t\otimes x_{2}=t \otimes Rx_{1}+0\\
x_{2}^{T}t\otimes x_{2}=x_{2}^{T}t \otimes Rx_{1}\\
0=x_{2}^{T}t \otimes Rx_{1}\\
x_{2}^{T}Ex_{1}=0
$$
这里的\(E\)就是Essential Matrix。将内参带入以后：
$$
p_{2}^{T}K^{-1}t\otimes RK^{-1}p_{1}=0\\
p_{2}^{T}Fp_{1}=0
$$
\(F\)就是Fundamental Matrix。 上述两个矩阵分别描述了空间同一点在不同位置处，所处的投影点，基于图像坐标和相机坐标的对应关系。 一般可以使用八点法来求出矩阵，从而算出\(R\)和\(t\)。
单应矩阵
如果，两幅图像的对应点集满足一个关系：在同一平面，那么这个问题可以简化为用更少的点求解，这个关系称为单应关系。这个更加符合虚拟拍摄系统的解决方案，因为通常一个比较简单的做法，是把待识别的标志放置与天棚上，使其在同一平面。 下面我们来推到一下这个矩阵，基于上述的描述，我们有：
$$
p_{2}=K(RP+t)
$$
假设所有点所在的平面方程为：
$$
n^{T}P+d=0 \Rightarrow \frac{-n^{T}P}{d}=1
$$
将上式带入,并化简后得到：
$$
p_{2}=K(RP+t(\frac{-n^{T}P}{d}))\\
p_{2}=K(R-(\frac{-rn^{T}}{d}))P\\
p_{2}=K(R-(\frac{-rn^{T}}{d}))K^{-1}p_{1}
p_{2}=Hp_{1}
$$
这个\(H\)矩阵就称为单应矩阵。
基于EKF的SLAM问题描述
终于进入正式的主题——SLAM。 对于虚拟拍摄中的SLAM问题，其实是一个三维空间定位的问题，为了便于理解，这里用一个二维的例子来理解一下SLAM的原理，但在这之前，先简单理解一下扩展卡尔曼滤波系统（EKF）。
EKF
先简单说一下线性卡尔曼滤波器，简单来说，就是对于一个优化问题，我们能够列出一个状态方程，用来预估下一时刻的状态。那么，我们的预估模型肯定不是100%正确的，于是又需要一个观测模型来帮助我们修正一下最终的结果。直白一点说，状态预估模型是不可靠的，观测模型也是不可靠的，但是两者结合以后可以帮助我们得到一个相对可靠一点的结果。对于线性模型，我们的两个方程很简单：
$$
x_{t}^{-}=Fx_{t-1}+Bu_{t}+w\\
z_{t}=Hx_{t}+v
$$
那么对于非线性的模型怎么办呢？对于一个非线性的状态转化模型\(x_{t}^{-}=f(X_{t-1},u_{t-1},w)\)与观测模型\(z_{t}=h(x_{t},v)\)，我们可以采用泰勒展开的方式，用一阶偏导数来更新状态，那么卡尔曼滤波的5个方程将会变成如下形式：
$$
x_{t}^{-}=f(x_{t-1},u_{t-1},w_{t-1})\\
P_{t}^{-}=FP_{t-1}F^{T}+UQU^{T}\\
K=P_{t}^{-}H_{t}^{T}(H_{t}P_{t}^{-}H_{t}^{T}+R)^{-1}\\
x_{t}=x_{t}^{-}+K(z_{t}-h(x_{t}^{-},v_{t-1}))\\
P_{t}=(I-KH_{t})P_{t}^{-}
$$
其中，\(Q,R\)为状态噪声和观测噪声的协方差矩阵,而\(F,U,H\)分别为：
$$
F=\frac{\partial f}{\partial x}, U=\frac{\partial f}{\partial u}, H=\frac{\partial z}{\partial x}
$$
这里，我们举个二维例子来实际感受一下EKF的过程。说有个车长为\(WB\)小车在平面上移动，车上装有一个距离探测器，能够在前方的某一个范围内检测Landmarker的信息。整个系统就包括小车与检测到的Landmarks。该小车的状态定义为\(X=\{x,y,\phi\}\),表示小车在平面的坐标与其车头和水平方向的夹角。对于小车的控制量定义为\(u=\{v,\varphi\}\),表示小车的速度，以及当前的转角度方向，如下图所示：
那么，可以定义出小车的状态转换方程如下：
$$
X_{t}^{-}=f(X_{t-1},u)=\begin{bmatrix}
X_{t-1}(1)+v \cdot dt \cdot cos(\phi+\varphi)\\
X_{t-1}(2)+v \cdot dt \cdot sin(\phi+\varphi)\\
X_{t-1}(3)+\frac{v \cdot dt \cdot sin(\varphi)}{WB}
\end{bmatrix}
$$
非线性模型中，需要用状态转换方程的一阶偏导来更新卡尔曼滤波器，因此需要\(f\)分别对\(\{x,y,\phi\}\)求偏导数求出\(F\)，同理分别对\(\{v,\varphi\}\)求偏导数求出\(U\):
$$
F=\frac{\partial f}{\partial X}=\begin{bmatrix}
1& 0 & -v \cdot dt \cdot sin(\phi+\varphi)\\
0& 1 & v \cdot dt \cdot cos(\phi+\varphi)\\
0& 0& 1
\end{bmatrix}
$$
$$
U=\frac{\partial f}{\partial u}=\begin{bmatrix}
dt \cdot cos(\phi+\varphi) & -v \cdot dt \cdot sin(\phi+\varphi)\\
dt \cdot sin(\phi+\varphi)& v \cdot dt \cdot cos(\phi+\varphi)\\
\frac{dt \cdot sin(\varphi)}{WB} & \frac{v \cdot dt \cdot cos(\varphi)}{WB}
\end{bmatrix}
$$
小车在当前的运动模型下运动，我们可以根据上式来估测小车的位置。之后，我们模拟小车有一个距离探测器，能够在前方的某一个范围内检测Landmarker的信息。对于Landmarker的检测，就是观测值，我们将观测方程定义如下：
$$
z_{t}=h(X_{t})=\begin{bmatrix}
\sqrt{[x_{i}-X_{t}(1)]^{2}+[y_{i}-X_{t}(2)]^{2}}\\
actan(\frac{y_{i}-X_{t}(2)}{x_{i}-X_{t}(1)})-\phi
\end{bmatrix}
$$
如下图所示：
前文说过，整个系统由小车与检测到的Landmarks组成，因此，对上述的观测方程求\(H\),需要对小车的状态\(X=\{x,y,\phi\}\)求偏导，也需要对Landmarks的状态\(X{i}=\{x_{i},y_{i}\}\)求偏导，即：
$$
H=[\frac{\partial z}{\partial X}|\frac{\partial z}{\partial X_{i}}]
$$
其中：
$$
\frac{\partial z}{\partial X}=\begin{bmatrix}
\frac{x_{i}-X_{t}(1)}{-d} & \frac{y_{i}-X_{t}(2)}{d} & 0\\
\frac{y_{i}-X_{t}(2)}{d^{2}} & \frac{x_{i}-X_{t}(1)}{d^{2}} & -1
\end{bmatrix}
$$
$$
\frac{\partial z}{\partial X_{i}}=\begin{bmatrix}
\frac{x_{i}-X_{t}(1)}{d} & \frac{y_{i}-X_{t}(2)}{d} \\
\frac{y_{i}-X_{t}(2)}{-d^{2}} & \frac{x_{i}-X_{t}(1)}{d^{2}}
\end{bmatrix}
$$
最后，将上述各方程带入EKF滤波器的5个迭代方程即可。
虚拟拍摄中的单目SLAM流程
$$
u=\begin{bmatrix}
V\\
\Omega
\end{bmatrix}=\begin{bmatrix}
\alpha \Delta t\\
\beta \Delta t
\end{bmatrix}
$$
$$
f(x_{t+1})=\begin{bmatrix}
r_{t+1}\\
q_{t+1}\\
v_{t+1}\\
\omega_{t+1}
\end{bmatrix}=\begin{bmatrix}
r_{t}+(v_{t}+V)\Delta t\\
q_{t}+(\omega_{t}+\Omega)\Delta t\\
v_{t}+V\\
\omega_{t}+\Omega
\end{bmatrix}
$$
$$
h(x_{t})=KR(L_{i}-r_{t})+offset\\
H=\frac{\partial h}{\partial x}
$$
$$
x_{t}=f(x_{t-1},u)\\
z_{t}=h(x_{t})
$$
$$
P(x|z,u)
$$
$$
p_{2}=Hp_{1}
$$