**

理论重述

1. 概述

本文明确使用单目视觉, 针孔相机模型^[1], orb特征检测与匹配^[2]
相关理论: 对极几何, 三角测量, PnP

2. 对极几何

2.1 坐标系

• 像素坐标系:
  图像的左上角是原点， 正方向分别往右和往下
• 相机坐标系:
  原点为相机镜头的光心， 向前为z轴正方向， 向下为y轴正方向， 向右为x轴正方向
• 归一化坐标:
  相机坐标系投影到z=1的平面上的坐标， 显然， 坐标形如(X/Z, Y/Z, 1)的形式
• 世界坐标系:
  可以任意指定， 为方便起见， 本文取世界坐标为右侧相机的相机坐标系为世界坐标系

2.2 极线约束

以第一帧图像的相机坐标系为世界坐标系, 某一空间点P = [X, Y, Z]^T, 投影在两张图像的点像素坐标分别为$p_1,p_2$ 以$s$表示深度，则有
$$\{\begin{array}{l}{s}_1{p}_1=KP\\ \\ s_2{p}_2=K(RP+t)\end{array}$$
简化为以下形式，其中$E=t^{\wedge }R$，称为本征矩阵

通过E可以分解出R和t, 本身具有尺度等价性, 但是对t乘以一个非0常数, 分解也是成立的^[3]，所以通常对t进行归一化处理，使其长度为1，这将直接导致t的尺度不确定性，比如t的第一维是0.17, 我们无法确定其单位是cm还是m. 因此单目slam有一步不可避免的初始化, 初始化的图像必须有一定程度的平移, 而后的轨迹和地图必须以此步的平移为单位.

3. 三维重建

根据针孔相机模型， 有如下关系:
$$Z\left[\begin{array}{c}u\\ \\ v\\ \\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ & & \\ 0& f_y& c_y\\ & & \\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ \\ Y\\ \\ Z\end{array}\right]$$
说明：$[uv1{]}^T$ 是像素坐标，$[XYZ{]}^T$是某一点的空间坐标。

仅通过上式无法求解空间坐标，但是能求解空间点映射到归一化平面$(Z=1)$上的归一化坐标$\left[\begin{array}{ccc}{u}^{\prime }& v^{\prime }& 1\end{array}\right]$，或称为像素坐标在归一化平面上的齐次坐标，显然
$${\left[\begin{array}{ccc}{u}^{\prime }& v^{\prime }& 1\end{array}\right]}^T={\left[\begin{array}{ccc}X/Z& Y/Z& 1\end{array}\right]}^T$$
使用三角测量求解深度，设某一空间点在两帧图像上的深度为$s_1和s_2$，以第一帧图像的相机坐标系为世界坐标系，根据对极几何，
$$s_1\left[\begin{array}{c}{u}_1\\ \\ v_1\\ \\ 1\end{array}\right]=s_2\left[\begin{array}{ccc}{r}_1& r_2& r_3\\ & & \\ r_4& r_5& r_6\\ & & \\ r_7& r_8& r_9\end{array}\right]\left[\begin{array}{c}{u}_2\\ \\ v_2\\ \\ 1\end{array}\right]+\left[\begin{array}{c}{t}_1\\ \\ t_2\\ \\ t_3\end{array}\right]$$
记为
$$s_1{x}_1=s_{2}R{x}_2+t$$
其中$R$表示图像的相对旋转，$t$表示相对平移

在本文设定的坐标系下，$s_1=Z$，对上式两边左乘$x_1^{\wedge }$，可求得$s_2$，进一步求得$s_1$

上式所用$R和t$，一般通过极限约束求得本征矩阵$E$，再分解本征矩阵得到$R和t$，本实验的两张参考照片的相对变换仅在$x$轴方向平移了10cm，则旋转变换矩阵$R$必然是3阶单位阵，平移变换矩阵为${\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^T$，不必再使用opencv求解，这一点从opencv的求解结果中得到了验证。

4. 求解PnP，获得参考照片的位姿

4.1 直接线性变换

$$s\left[\begin{array}{c}{u}^{\prime }\\ \\ v^{\prime }\\ \\ 1\end{array}\right]=\left[\begin{array}{cccc}{r}_1& r_2& r_3& t_1\\ & & & \\ r_4& r_5& r_6& t_2\\ & & & \\ r_7& r_8& r_9& t_3\end{array}\right]\left[\begin{array}{c}X\\ \\ Y\\ \\ Z\\ \\ 1\end{array}\right]$$
记为
$$s{x}^{\prime }=\left[\begin{array}{c}R|t\end{array}\right]P$$
这里的$R和t$代表了测试照片相对于参考点的朝向和位置

实际上直接线性变换准确率不高，一般使用其优化算法如EPnP, UPnP等

4.2 坐标变换

任意两个笛卡尔坐标系之间的旋转变换：
$$\left[\begin{array}{c}{X}^{\prime }\\ \\ Y^{\prime }\\ \\ Z^{\prime }\\ \end{array}\right]=\left[\begin{array}{ccc}cos{\theta }_3& sin{\theta }_3& 0\\ & & \\ -sin{\theta }_3& cos{\theta }_3& 0\\ & & \\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}cos{\theta }_2& 0& -sin{\theta }_2\\ & & \\ 0& 1& 0\\ & & \\ sin{\theta }_2& 0& cos{\theta }_2\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ & & \\ 0& cos{\theta }_1& sin{\theta }_1\\ & & \\ 0& -sin{\theta }_1& cos{\theta }_1\end{array}\right]\left[\begin{array}{c}X\\ \\ Y\\ \\ Z\\ \end{array}\right]$$
说明：这里，我们假设坐标系$O-XYZ$依次绕自身X轴、Y轴、Z轴分别逆时针转${\theta }_1，{\theta }_2，{\theta }_3$后可以与坐标系$O^{\prime }-X^{\prime }{Y}^{\prime }{Z}^{\prime }$重合

使用此方程既可以将R转换为相机的相对角度，也可以将定位结果从相机坐标系变换到世界坐标系

5. 参考文献

• [1] 高翔, 张涛. 视觉slam十四讲:从理论到实践[M]. 北京:电子工业出版社. 2017:84-90.
• [2] Ethan Rublee, Vincent Rabaud, Kurt Konolige, Gary R. Bradski: ORB: An efficient alternative to SIFT or SURF[C]. ICCV 2011: 2564-2571.
• [3] 高翔, 张涛. 视觉slam十四讲:从理论到实践[M]. 北京:电子工业出版社. 2017:151-152.