欢迎使用CSDN-markdown编辑器

1. 基本概念
1） 视觉导航系统分为三个模块：图像预处理模块、运动参数估计模块（自己在哪）、地图创建（这是什么地方）和导航模块（怎么离开这个地方）
2） 视觉里程计是其中的一部分，即根据飞行器在飞行过程中拍摄的序列图 像来估算微型飞行器的运动参数
3） 视觉里程计一般涉及前两个模块

2. 微型飞行器运动参数估算数学建模

1）摄像机模型，解决三维场景中的点和图像平面的点的映射问题
2）采用针孔模型（pin-hole model）
3）涉及四种坐标系：
(1) 成像平面坐标系（X-Y坐标系）
(2) 图像坐标系（u-v坐标系）
(3) 摄像机坐标系（xyzO）
(4) 世界坐标系（xwywzwOw）

具体来解释每一个坐标系以及各个坐标系的联系：
(1) 成像平面坐标系（X-Y坐标系）
定义该坐标系原点为摄像机光轴与图像平面的交点，X,Y轴分别与成像平面的两条边平行，原店一般位于图像中心，但是由于硬件制作的原因，存在偏差
(2) 图像坐标系（u-v坐标系）
坐标系原点位于图像左上角，u轴沿图像边界自左向右，v轴沿图像平面从上向下，以像素为单位，坐标（u,v）对应的值就是该像素点的灰度值。
(3) 图像坐标系与成像平面坐标系的转换

$$\begin{equation} {\begin{bmatrix} u\v\1\end{bmatrix}}= {\begin{bmatrix}{} {{s_x}}&0&{{c_x}}\0&{{s_y}}&{{c_y}}\0&0&1 \end{bmatrix}} \left[ {\begin{array}{*{20}{c}} X\Y\1 \end{array}} \right] \end{equation}$$
其中 ， 为X轴和Y轴方向上单位物理尺寸上的像素数目， 为成像平面坐标系原点在u-v坐标系中的坐标
(4) 摄像机坐标系（xyzO）
定义该坐标系原点为摄像机光心，x,y轴分别与成像平面坐标系的X轴与Y轴平行
(5) 摄像机坐标系与成像平面坐标系的转换
$$\begin{equation} X = f\frac{x}{z},Y = f\frac{y}{z}$ \end{equation}$$
(6) 摄像机坐标系与图像坐标系的转换

$$\begin{align} z\left[ {\begin{array}{*{20}{c}} u\\ v\\ 1 \end{array}} \right] &= \left[ {\begin{array}{*{20}{c}} {{s_x}}&0&{{c_x}}\\ 0&{{s_y}}&{{c_y}}\\ 0&0&1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} f&0&0&0\\ 0&f&0&0\\ 0&0&1&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} x\\ y\\ z\\ 1 \end{array}} \right] \ &= \left[ {\begin{array}{*{20}{c}} {{f_x}}&0&{{c_x}}&0\ 0&{{f_y}}&{{c_y}}&0\\ 0&0&1&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} x\\ y\\ z\\ 1 \end{array}} \right]\end{align}$$

(7) 世界坐标系（ ${x_w}{y_w}{z_w}{O_w}$）

$$\begin{equation} \left[ {\begin{array}{*{20}{c}} x\\ y\z\1\end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {R_w^c}&{t_w^c}\0&1 \end{array}} \right]\begin{bmatrix} {{x_w}}\{{y_w}}\{{z_w}}\\ 1 \end{bmatrix} \end{equation}$$

设点在t1时刻相对于摄像机坐标系 的坐标为Oxy,p=(x,y,z)；在t2时刻相对于摄像机坐标系 O’x’y’的坐标为p’=(x’,y’,z’)
设摄像机在t1时刻相对于t2时刻的旋转矩阵为R,平移矢量为T
有 p’=Rp+T
3. 基于特征点对应的飞行器的运动参数估计

已知两摄像机坐标系下的点坐标满足：

$$\begin{equation} \left[ {\begin{array}{*{20}{c}} {x‘}\{y‘}\{z‘} \end{array}} \right] = R\left[ {\begin{array}{*{20}{c}} x\y\z \end{array}} \right] + T \end{equation}$$
两边叉乘 ，有
$$\begin{equation} T \times \left[ {\begin{array}{*{20}{c}} {x‘}\{y‘}\{z‘} \end{array}} \right] = T \times R\left[ {\begin{array}{*{20}{c}} x\y\z \end{array}} \right] \end{equation}$$
两边点乘 ，有
$$\begin{equation} (x‘,y‘,z‘)(T \times \left[ {\begin{array}{*{20}{c}} {x‘}\{y‘}\{z‘} \end{array}} \right]) = (x‘,y‘,z‘)E\left[ {\begin{array}{*{20}{c}} x\y\z \end{array}} \right] \end{equation}$$
$$\begin{equation} E = T \times R \end{equation}$$

E为本征矩阵，充要条件为对其奇异值分解后，有两个相等的奇异值，其第
三个奇异值为0
由式左边为零知两组特征点满足关系：

$$\begin{equation} (x‘,y‘,z‘)E\left[ {\begin{array}{*{20}{c}} x\y\z \end{array}} \right] = 0 \end{equation}$$

齐次，有：

$$\begin{equation} ({x_0},y{}_0,1)\left( {\begin{array}{*{20}{c}} {{E_{11}}}&{{E_{12}}}&{{E_{13}}}\{{E_{21}}}&{{E_{22}}}&{{E_{23}}}\{{E_{31}}}&{{E_{32}}}&{{E_{33}}} \end{array}} \right)\left[ {\begin{array}{*{20}{c}} {{x_1}}\{{y_1}}\z \end{array}} \right] = 0 \end{equation}$$
计算本征矩阵，将矩阵相乘的形式拆开得到：
$$\begin{equation} \left( {\begin{array}{*{20}{c}} {{x_0}{x_1}}&{{x_0}{y_1}}&{{x_0}}&{{y_0}{x_1}}&{{y_0}{y_1}}&{{y_0}}&{{x_1}}&{{y_1}}&1 \end{array}} \right)\left( {\begin{array}{*{20}{c}} {{E_{11}}}\{{E_{12}}}\{{E_{13}}}\{{E_{21}}}\{{E_{22}}}\{{E_{23}}}\{{E_{31}}}\{{E_{32}}}\{{E_{33}}} \end{array}} \right) = 0 \end{equation}$$
上面方程左边进行任意缩放都不会影响方程的解，即：
$$\begin{equation} \left( {\begin{array}{*{20}{c}} {{x_0}{x_1}}&{{x_0}{y_1}}&{{x_0}}&{{y_0}{x_1}}&{{y_0}{y_1}}&{{y_0}}&{{x_1}}&{{y_1}}&1 \end{array}} \right){E_{33}}\left[ {\begin{array}{*{20}{c}} {{E_{11}}/{E_{33}}}\{{E_{12}}/{E_{33}}}\{{E_{13}}/{E_{33}}}\{{E_{21}}/{E_{33}}}\{{E_{22}}/{E_{33}}}\{{E_{23}}/{E_{33}}}\{{E_{31}}/{E_{33}}}\{{E_{32}}/{E_{33}}}\1 \end{array}} \right] = 0 \end{equation}$$
所以E虽然有9个未知数，但是有一个变量E33可以看做是缩放因子，只有8个未知量
AX=0有8个未知量, r(A)=8, 至少需要8个匹配点对，求解最小二乘问题，对A做SVD分解， 解为V的最后一列元素。
但是，实际计算中，匹配点坐标存在误差，这使得计算出的E可能不会满足之前提到的性质，所以我们需要把计算出的E投影到真正的本征矩阵空间，也就是使得它的三个奇异值中两个相等，一个为0

如何投影到本征矩阵空间？
强制改变该矩阵的奇异值（证明见YI Ma著作《An Invitation to 3D vision》p86）
考虑到$E$矩阵已经缩放过，所以更多的是直接令奇异值为(1,1,0)，即：

$$\begin{equation} \begin{array}{l} [U,D,V] = svd[Escale]\Escale = U*diag([1,1,0])*V‘ \end{array} \end{equation}$$
求解出$E$$后，接下来需要从$E$中恢复$R$和$T$
我们定义E = T \timesR = {[T]_ \times }R 为 的反对称矩阵，恢复 和 :

其中 表示绕Z轴旋转90度得到的旋转阵：

$$\begin{equation} R = UR_Z^T( \pm \frac{\pi }{2}){V^T},{[T]_ \times } = UR_Z^T( \pm \frac{\pi }{2})DU{}^T \end{equation}$$
从R,T的计算公式中可以看到R,T都有两种情况，即R,T组合有4中组合方式
选择特征点深度为正的一组R,T为摄像机坐标系的位姿
那么如何计算特征点深度？（三角测量法）
现在需要讨论尺度scale，从R，T的计算公式中，可以发现平移向量T和E的奇异值有关：
$$\begin{equation} R_Z^T( \pm \frac{\pi }{2}) = \left[ {\begin{array}{*{20}{c}} 0&{ - 1}&0\1&0&0\0&0&1 \end{array}} \right] \end{equation}$$
之前推导过程中所有对E进行的尺度伸缩实际上都是作用在奇异值上
$$\begin{equation} Escale = U*diag([1,1,0])*V‘ \end{equation}$$
也就是说这个尺度scale实际作用的是两个相机之间的平移

由于尺度scale的关系，不同的t，决定了以后计算点P的深度也是不同的，所以恢复的物体深度也是跟尺度scale有关的，这就是论文中常说的结构恢复structure reconstruction，只是恢复了物体的结构框架，而不是实际意义的物体尺寸。
知道了相机的轨迹以后，使用三角法就能计算某个点的深度（见Hartley著作《Multiple view geometry》chapter10 and chapter12）对于三维空间中一点P，假设第一个摄像机坐标系C1为世界坐标系，P在C1中的表示为： ，C1的外参矩阵M1为单位矩阵：

$$\begin{equation} {M_1} = \left( {\begin{array}{*{20}{c}} 1&0&0&0\0&1&0&0\0&0&1&0 \end{array}} \right) \end{equation}$$
点P和光心的连线交第一个图像平面于点p1 ,注意这里的p1是在摄像机坐标系的坐标表示(齐次化)，同理，和第二个摄像机光心连线交第二幅图像于点p2 。pi在各自摄像机坐标系中的表示为：
$$\begin{equation} {p_1} = \left( {\begin{array}{*{20}{c}} {{x_1}}\{{y_1}}\1 \end{array}} \right),{p_2} = \left( {\begin{array}{*{20}{c}} {{x_2}}\{{y_2}}\1 \end{array}} \right) \end{equation}$$
摄像机坐标系C2的外参数矩阵为M2，由于摄像机坐标系C1就是世界坐标系，所以有
$$\begin{equation} {M_2} = \left( {\begin{array}{*{20}{c}} {{r_{11}}}&{{r_{12}}}&{{r_{13}}}&{{t_x}}\{{r_{21}}}&{{r_{22}}}&{{r_{23}}}&{{t_y}}\{{r_{31}}}&{{r_{32}}}&{{r_{33}}}&{{t_z}} \end{array}} \right) = \left( {\begin{array}{*{20}{c}} {R_1^2}&{t_1^2} \end{array}} \right) \end{equation}$$
有了外参矩阵，我们就可以得到这些点坐标的关系：
$$\begin{equation} \begin{array}{l} {Z_1}{p_1} = {M_1}P\{Z_2}{p_2} = {M_2}P \end{array} \end{equation}$$
由于光心O，三维坐标点P，以及pi三点共线，所以向量Opi、CiP的叉乘应该为0，
上述方程又可以转化为
$$\begin{equation} \begin{array}{l} {p_1} \times {M_1}P = 0\{p_2} \times {M_2}P = 0 \end{array} \end{equation}$$
$$\begin{equation} \left( {\begin{array}{*{20}{c}} {[{{({p_1})}_ \times }]{M_1}}\{[{{({p_2})}_ \times }]{M_2}} \end{array}} \right)P = 0 \end{equation}$$

用最小二乘求解,和求本征矩阵一样，计算矩阵A的SVD分解，然后奇异值最小的那个奇异向量就是三维坐标P的解。
计算出来的P的坐标就是P在世界坐标系中的坐标，这里就是P在摄像机坐标系C1中的表示

总结
提到本征矩阵恢复的R,T组合有四种组合形式，我们需要通过计算点的深度来判断R,T的哪种组合是正确的，和这篇博客结合起来，获得R,T正确组合的流程和代码如下有了外参矩阵，我们就可以得到这些点坐标的关系：
1.利用八点法求出本征矩阵E
2.将E投影到真正本征空间（令D=[1,1,0]）
3.恢复旋转矩阵R和平移矩阵T（四组）
4.对每一组R,T：

1）重构相对于C1的特征点P的3D位置

$$\begin{equation} \begin{array}{l} A = \left( {\begin{array}{*{20}{c}} {[{{({p_1})}_ \times }]{M_1}}\{[{{({p_2})}_ \times }]{M_2}} \end{array}} \right)\[U,D,V] = svd(A)\P = V(:,4)\P1est = P/P(4) \end{array} \end{equation}$$

2）计算相对于C2的特征点P的3D位置

$$\begin{equation} P2est = {M_2}*P1est \end{equation}$$
3）如果两者的Z坐标均大于0,选择该组R,T
注意：以上计算P三维坐标的推导过程中，可以看到和本征矩阵E是息息相关的，E和我们的尺度紧密相连，所以计算出来的深度和尺度scale也是直接相关的。

尺度如何恢复？

1.2Dto2D的方法：对两对图像计算E并恢复R,T时，他们的尺度都不一样，本来是同一点，在不同尺寸下，深度不一样，无法构建地图（mapping），尺度需要统一

同一点在两帧图像上的深度比值与两位移比值相同，通过前一个位移参数的幅值乘以一个比例系数求下一个位移参数的幅值

2.3Dto2D的方法：通过最开始的两帧图像计算E恢复了R,T，并通   过三角法计算出了深度，那我就有了场景点的3D坐标，后续的视频序列就可以通过3Dto2d (opencv里的solvePnp) 来进行位姿估计。

确定初始深度？
对于飞行器来说，可通过其他导航装置获得（IMU等），多传感器融合

如何解决尺度漂移？
3Dto2D方法又会产生新的问题–scale drift。因为，两帧间的位姿总会出现误差，这些误差积累以后，使得尺度不再统一（SLAM后端优化）

下篇博客会继续将这些问题。

博客中的参考资料
1.YI Ma著作《An Invitation to 3D vision》
2.Hartley著作《Multiple view geometry》
3.白巧克力亦唯心的博客http://blog.csdn.net/heyijia0327