VSLAM学习(三) 单目相机位姿估计

目录
VSLAM学习(一) 三维运动、相机模型、SLAM模型
VSLAM学习(二) 非线性优化
VSLAM学习(三) 单目相机位姿估计
VSLAM学习(四) Bundle Adjustment

一、对极几何约束

相机在不同位姿拍到世界中同一个路标，会有不同的成像位置，如下图所示

世界路标$\mathbf{P}$，在相机两个不同位姿所拍到的帧${\mathbf{I}}_1,{\mathbf{I}}_2$中，分别投影在了${\mathbf{p}}_1,{\mathbf{p}}_2$两点。
这两帧时刻，相机的坐标中心点分别为${\mathbf{C}}_1,{\mathbf{C}}_2$
${\mathbf{e}}_1,{\mathbf{e}}_2$分别是这两个时刻中心点在另一帧上的投影

那么很容易看出，${\mathbf{p}}_1{\mathbf{e}}_1//\mathbf{P}{\mathbf{C}}_2$，${\mathbf{p}}_2{\mathbf{e}}_2//\mathbf{P}{\mathbf{C}}_1$

1.1 本质矩阵

记$\mathbf{P}$点在${\mathbf{I}}_1$帧和${\mathbf{I}}_2$帧的坐标系下的向量坐标分别为${\mathbf{P}}_1$和${\mathbf{P}}_2$
那么根据三维运动我们知道：

$${\mathbf{P}}_2={\mathbf{R}}_{12}{\mathbf{P}}_1+\mathbf{t}$$

两边同时左叉乘$\mathbf{t}$得

$${\mathbf{t}}^{\wedge }{\mathbf{P}}_2={\mathbf{t}}^{\wedge }{\mathbf{R}}_{12}{\mathbf{P}}_1$$
两边再同时与${\mathbf{P}}_2$作内积

$$0={\mathbf{P}}_2^T{\mathbf{t}}^{\wedge }{\mathbf{R}}_{12}{\mathbf{P}}_1$$

我们将${\mathbf{t}}^{\wedge }{\mathbf{R}}_{12}\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}\mathbf{E}$称为本质矩阵(Essential Matrix)

$${\mathbf{P}}_2^T\mathbf{E}{\mathbf{P}}_1=0$$

1.2 基础矩阵

根据相机模型我们知道

$$\{\begin{array}{rl}{s}_1{\mathbf{p}}_1& =\mathbf{K}{\mathbf{P}}_1\\ s_2{\mathbf{p}}_2& =\mathbf{K}{\mathbf{P}}_2\end{array}$$

其中$s_1,s_2$分别为P点在相机两帧里的深度
　　${\mathbf{p}}_1,{\mathbf{p}}_2$分别为P点在相机图片像素坐标的齐次坐标

将$\mathbf{K}$移到左边：

$$\{\begin{array}{rl}{s}_1{\mathbf{K}}^{-1}{\mathbf{p}}_1& ={\mathbf{P}}_1\\ s_2{\mathbf{K}}^{-1}{\mathbf{p}}_2& ={\mathbf{P}}_2\end{array}$$

带入之前本质矩阵式中，得

$$0={\mathbf{p}}_2^T({\mathbf{K}}^{-1}{)}^T\mathbf{E}{\mathbf{K}}^{-1}{\mathbf{p}}_1$$

我们将$({\mathbf{K}}^{-1}{)}^T\mathbf{E}{\mathbf{K}}^{-1}\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}\mathbf{F}$称为基础矩阵(Fundamental Matrix)

$${\mathbf{p}}_2^T\mathbf{F}{\mathbf{p}}_1=0$$

以上两个关于本质矩阵和基础矩阵的等式称为对极几何约束

二、求解旋转与平移

2.1 八点法

将本质矩阵约束中的坐标归一化

$${\mathbf{x}}_2^T\mathbf{E}{\mathbf{x}}_1=0$$

记归一化坐标${\mathbf{x}}_1=(u_1,v_1,1{)}^{\mathrm{T}}$,${\mathbf{x}}_2=(u_2,v_2,1{)}^{\mathrm{T}}$
记本质矩阵$\mathbf{E}=\left(\begin{array}{ccc}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right)$

那么

$$(u_2,v_2,1)\left(\begin{array}{ccc}{e}_1& e_2& e_3\\ e_4& e_5& e_6\\ e_7& e_8& e_9\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=(u_2{u}_1,u_2{v}_1,u_2,v_2{u}_1,v_2{v}_1,v_2,u_1,v_1,1)\left(\begin{array}{c}{e}_1\\ e_2\\ e_3\\ e_4\\ e_5\\ e_6\\ e_7\\ e_8\\ e_9\end{array}\right)=0$$

现在要求解这9个值，由于等式等于0，可以去掉一个尺度自由度，那么只需8个方程即可求解。而$\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R}$，平移和旋转各有3个自由度，去掉尺度就只有5个自由度了，所以理论上最少只需要5对点就能求解，但是比较麻烦。

取8对点的方法称为八点法，将Essential矩阵展开成向量格式
记${\mathbf{a}}_1=(u_2{u}_1,u_2{v}_1,u_2,v_2{u}_1,v_2{v}_1,v_2,u_1,v_1,1)$
$\mathbf{e}=({\mathrm{v}\mathrm{e}\mathrm{c}}^{\mathrm{T}}\mathbf{E}{)}^{\mathrm{T}}=(e_1,e_2,\cdot \cdot \cdot ,e_9{)}^{\mathrm{T}}$

则${\mathbf{a}}_1^{\mathrm{T}}\mathbf{e}=0$
同样方法再找7对点，将对应系数记为${\mathbf{a}}_2\cdot \cdot \cdot {\mathbf{a}}_8$
可以得到方程组

$$\left(\begin{array}{c}{\mathbf{a}}_1\\ {\mathbf{a}}_2\\ {\mathbf{a}}_3\\ {\mathbf{a}}_4\\ {\mathbf{a}}_5\\ {\mathbf{a}}_6\\ {\mathbf{a}}_7\\ {\mathbf{a}}_8\end{array}\right)\mathbf{e}=0$$

即可解出$\mathbf{e}$的值
也就是求出了本质矩阵$\mathbf{E}$的值

然后将矩阵$\mathbf{E}$进行奇异值分解，便可得到$\mathbf{R}$与$\mathbf{t}$
(暂略)

如果多于8对点求解，可以使用以下两种方法

• 最小二乘求解$\underset{\mathbf{e}}{\min }\Vert \mathbf{A}\mathbf{e}{\Vert }_2^2=\underset{\mathbf{e}}{\min }{\mathbf{e}}^{\mathrm{T}}{\mathbf{A}}^{\mathrm{T}}\mathbf{A}\mathbf{e}$
• 使用RANSAC方法：
  ① 有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
  ② 用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
  ③ 如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
  ④ 然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
  ⑤ 最后，通过估计局内点与模型的错误率来评估模型。

2.2 单应矩阵

所有取出的点共面时，八点法退化，无法求解。
当所有点共面时，设该平面$\mathbf{\pi }=({\mathbf{n}}^{\mathrm{T}},d)$，即单位法向量为$\mathbf{n}$，截距为$d$
则有

$${\mathbf{n}}^{\mathrm{T}}\mathbf{P}+d=0or-\frac{{\mathbf{n}}^{\mathrm{T}}\mathbf{P}}{d}=1$$

可将1带入下式：

$$\begin{array}{rl}{\mathbf{p}}_2& =\frac{1}{s_1}\mathbf{K}({\mathbf{R}}_{12}{\mathbf{P}}_1+\mathbf{t})\\ & =\frac{1}{s_1}\mathbf{K}({\mathbf{R}}_{12}{\mathbf{P}}_1+\mathbf{t}\cdot (-\frac{{\mathbf{n}}^{\mathrm{T}}\mathbf{P}}{d}))\\ & =\frac{1}{s_1}\mathbf{K}({\mathbf{R}}_{12}-\frac{\mathbf{t}{\mathbf{n}}^{\mathrm{T}}}{d}){\mathbf{P}}_1\\ & =\frac{s_2}{s_1}\mathbf{K}({\mathbf{R}}_{12}-\frac{\mathbf{t}{\mathbf{n}}^{\mathrm{T}}}{d}){\mathbf{K}}^{-1}{\mathbf{p}}_1\\ & \stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}\mathbf{H}{\mathbf{p}}_1\end{array}$$

即

$$\left(\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right)=\left(\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right)\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)$$

得

$$\{\begin{array}{rl}{u}_2& =u_1{h}_1+v_1{h}_2+h_3\\ v_2& =u_1{h}_4+v_1{h}_5+h_6\\ 1& =u_1{h}_7+v_1{h}_8+h_9\end{array}$$

归一化$\mathbf{H}$(令$h_9=1$)，③式带入①、②并移项得

$$\{\begin{array}{rl}{u}_2& =u_1{h}_1+v_1{h}_2+h_3-u_2{u}_1{h}_7-u_2{v}_1{h}_8\\ v_2& =u_1{h}_4+v_1{h}_5+h_6-v_2{u}_1{h}_7-v_2{v}_1{h}_8\end{array}$$

即
$$\left(\begin{array}{cccccccc}{u}_1& v_1& 1& 0& 0& 0& -u_2{u}_1& -u_2{v}_1\\ 0& 0& 0& u_1& v_1& 1& -v_2{u}_1& -v_2{v}_1\end{array}\right)\left(\begin{array}{c}{h}_1\\ h_2\\ h_3\\ h_4\\ h_5\\ h_6\\ h_7\\ h_8\end{array}\right)=\left(\begin{array}{c}{u}_2\\ v_2\end{array}\right)$$

一对点两个方程，所以只需要4对点即可求解
算出$\mathbf{H}$，继而求解$\mathbf{R},\mathbf{t},\mathbf{n},\mathbf{K},d$
这一步贼麻烦，暂略…

三、三角测量深度估计

求出平移和旋转之后，就可以来求某一个路标点在相机坐标下的位置了，确定深度了。

3.1 直接求解

由三维运动方程知（归一化坐标）：
$$s_2{\mathbf{x}}_2=s_1{\mathbf{R}}_{12}{\mathbf{x}}_1+\mathbf{t}$$

那么现在的任务就是求出$s_1,s_2$，即是深度值
两边同时左叉乘${\mathbf{x}}_2$
$$0=s_1{\mathbf{x}}_2^{\wedge }{\mathbf{R}}_{12}{\mathbf{x}}_1+{\mathbf{x}}_2^{\wedge }\mathbf{t}$$
便可求出$s_1$的值，继而求出$s_2$

然而，由于噪声的存在，我们之前确定的$\mathbf{R},\mathbf{t}$并不完全准确，导致$s_1,s_2$也不准
故而通过

$$s_1{\mathbf{x}}_1={\mathbf{P}}_1$$

求出的路标P的坐标肯定也是个歪的

所以我们一般是对P点做最小二乘解

3.2 最小二乘解

以第一帧相机坐标系为准
对于第k帧中的路标P的坐标${\mathbf{x}}_k$有

$$s_k{\mathbf{x}}_k={\mathbf{R}}_{1k}\mathbf{P}+{\mathbf{t}}_k=({\mathbf{R}}_{1k}|{\mathbf{t}}_k)\mathbf{P}$$

展开

$$s_k\left(\begin{array}{c}{u}_k\\ v_k\\ 1\end{array}\right)=\left(\begin{array}{cc}\begin{array}{ccc}{r}_{k,1}& r_{k,2}& r_{k,3}\\ r_{k,4}& r_{k,5}& r_{k,6}\\ r_{k,7}& r_{k,8}& r_{k,9}\end{array}& \begin{array}{c}{t}_{k,1}\\ t_{k,2}\\ t_{k,3}\end{array}\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)$$

即

$$\{\begin{array}{rl}{s}_k{u}_k& =({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_1}\mathbf{P}\\ s_k{v}_k& =({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_2}\mathbf{P}\\ s_k& =({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_3}\mathbf{P}\end{array}$$

其中$({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_n}$表示增广矩阵$({\mathbf{R}}_{1k}|{\mathbf{t}}_k)$的第n行

消去第三行
$$\{\begin{array}{rl}{u}_k({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_3}\mathbf{P}-({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_1}\mathbf{P}& =0\\ v_k({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_3}\mathbf{P}-({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_2}\mathbf{P}& =0\end{array}$$

即
$$\left(\begin{array}{c}{u}_k({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_3}-({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_1}\\ v_k({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_3}-({\mathbf{R}}_{1k}|{\mathbf{t}}_k{)}_{{}_2}\end{array}\right)\mathbf{P}=0$$

将所有的帧都整合起来，记

$$\mathbf{A}\stackrel{\mathrm{d}\mathrm{e}\mathrm{f}}{=}\left(\begin{array}{c}{u}_1({\mathbf{R}}_{11}|{\mathbf{t}}_1{)}_{{}_3}-({\mathbf{R}}_{11}|{\mathbf{t}}_1{)}_{{}_1}\\ v_1({\mathbf{R}}_{11}|{\mathbf{t}}_1{)}_{{}_3}-({\mathbf{R}}_{11}|{\mathbf{t}}_1{)}_{{}_2}\\ u_2({\mathbf{R}}_{12}|{\mathbf{t}}_2{)}_{{}_3}-({\mathbf{R}}_{12}|{\mathbf{t}}_2{)}_{{}_1}\\ v_2({\mathbf{R}}_{12}|{\mathbf{t}}_2{)}_{{}_3}-({\mathbf{R}}_{12}|{\mathbf{t}}_2{)}_{{}_2}\\ ⋮\\ u_n({\mathbf{R}}_{1n}|{\mathbf{t}}_n{)}_{{}_3}-({\mathbf{R}}_{1n}|{\mathbf{t}}_n{)}_{{}_1}\\ v_n({\mathbf{R}}_{1n}|{\mathbf{t}}_n{)}_{{}_3}-({\mathbf{R}}_{1n}|{\mathbf{t}}_n{)}_{{}_2}\end{array}\right)$$

则

$$\mathbf{A}\mathbf{P}=0$$

那么，可对P求最小二乘解

$$\underset{\mathbf{P}}{\min }\frac{1}{2}\Vert \mathbf{A}\mathbf{P}{\Vert }_2^2=\underset{\mathbf{P}}{\min }\frac{1}{2}{\mathbf{P}}^{\mathrm{T}}{\mathbf{A}}^{\mathrm{T}}\mathbf{A}\mathbf{P}$$