相机模型
坐标定义
-
世界坐标系
\[P_w(m):(x_w,y_w,z_w) \] -
相机坐标系
\[P_c(m):(x_c,y_c,z_c) \] -
图像坐标系
\[P(mm):(x,y) \] -
像素坐标系
\[p(pixel):(u,v) \]
坐标转换
-
世界坐标系$\longrightarrow $相机坐标系
\[P_c=T_{cw}P_w \]\[\begin{bmatrix} x_w\\ y_w \\ z_w\\1\end{bmatrix}=\begin{bmatrix}R & t\\0^T & 1\end{bmatrix}\begin{bmatrix}x_c\\y_c\\z_c\\1\end{bmatrix} \] -
相机坐标系$\longrightarrow $图像坐标系
\[\frac{z_c}{f}=\frac{x_c}{x}=\frac{y_c}{y}，f为焦距 \] -
像素坐标系$\longleftrightarrow $图像坐标系
-
像素坐标系以图像左上角为原点
-
图像坐标系理论上以图像中心为原点，但是由于相机生产制作产生的误差，需要我们对相机进行标定才能确定坐标原点
-
两个坐标系之间相差了一个缩放和一个原点的平移。我们设像素坐标在 u 轴上缩放了 1/dx倍，在 v 上缩放了 1/dy倍，那么我们可以表示为
\[\begin{cases} u=\frac{x}{dx}+c_x\\v=\frac{y}{dy}+c_y\end{cases} \]用矩阵形式可以表示为
\[\begin{bmatrix}u \\v \\1\end{bmatrix}=\begin{bmatrix} \frac{1}{dx}&0&c_x\\0&\frac{1}{dy}&c_y\\0&0&1\end{bmatrix}\begin{bmatrix}x\\y\\1\end{bmatrix} \]
-
-
像素坐标系$\longrightarrow $相机坐标系
\[\begin{cases} u=\frac{x}{dx}+c_x\\v=\frac{y}{dy}+c_y\end{cases}\longleftrightarrow \begin{bmatrix}u \\v \\1\end{bmatrix}=\frac{1}{z_c}\begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}\begin{bmatrix}x_c\\y_c\\z_c\end{bmatrix}=\frac{1}{z_c}KP_c=p \]由于齐次坐标的性质，有时转换时也会省略\(z_c\)
-
像素坐标系\(\longrightarrow\)世界坐标系
\[p=\begin{bmatrix}u \\v \\1\end{bmatrix}=\frac{1}{z_c}\begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}\begin{bmatrix}x_c\\y_c\\z_c\end{bmatrix}=\frac{1}{z_c}KTP_w \]
视觉里程计
相机的运动衡量其实就是上一时刻拍摄到的帧和当前时刻帧之间相机所做的平移和旋转操作所以计算R和t的过程就是视觉里程计
基于特征匹配的视觉里程计
基于特征的方法是当前视觉里程计的主流方式，对于两张图像，首先选取一些具有代表性的点，称为特征点。之后，仅针对这些特征点估计相机的运动，同时估计特征点的空间位置。图像里其他非特征点的信息，则被丢弃
特征点方法把一个对图像的运动估计转换为对两组点之间的运动估计。于是，它的主要问题为：
- 我们如何获取图像特征点？如何匹配它们？
- 如何根据已知特征点，计算相机的运动？
计算特征点
常用的特征点有Harris角点、SIFT、SURF、ORB等，下图采用的是Sift特征点
对于每一个特征点，为了说明它与其他点的区别，人们还使用“描述子”（Descriptor）对它们加以描述。描述子通常是一个向量，含有特征点和周围区域的信息。如果两个特征点的描述子相似，我们就可以认为它们是同一个点。根据特征点和描述子的信息，我们可以计算出两张图像中的匹配点。
根据匹配好的特征点估计相机运动
在匹配好特征点后，我们可以得到两个一一对应的像素点集。接下来要做的，就是根据两组匹配好的点集，计算相机的运动了。在普通的单目成像中，我们只知道这两组点的像素坐标。而在双目和RGBD配置中，我们还知道该特征点离相机的距离。因此，该问题就出现了多种形式：
-
2D-2D形式：通过两个图像的像素位置来估计相机的运动。
-
3D-2D形式：假设已知其中一组点的3D坐标，以及另一组点的2D坐标，求相机运动。
-
3D-3D形式:两组点的3D坐标均已知，估计相机的运动。
那么问题就来了：是否需要为这三种情况设计不同的计算方法呢？答案是：既可以单独做，也可以统一到一个大框架里去做。
- 单独做的时候，2D-2D使用对极几何的方法，3D-2D使用PnP求解算法，而3D-3D则称为ICP方法（准确地说，ICP不需要各点的配对关系）。
- 统一的框架，就是指把所有未知变量均作为优化变量，而几何关系则是优化变量之间的约束。由于噪声的存在，几何约束通常无法完美满足。于是，我们把与约束不一致的地方写进误差函数。通过最小化误差函数，来求得各个变量的估计值。这种思路也称为Bundle Adjustment(BA，中文亦称捆集优化或光束法平差)。
代数方法简洁优美，但是它们对于噪声的容忍性较差。存在误匹配，或者像素坐标存在较大误差时，它给出的解会不可靠。而在优化方法中，我们先猜测一个初始值，然后根据梯度方向进行迭代，使误差下降。Bundle Adjustment非常通用，适用于任意可以建模的模型。但是，由于优化问题本身非凸、非线性，使得迭代方法往往只能求出局部最优解，而无法获得全局最优解。也就是说，只有在初始值足够好的情况下，我们才能希望得到一个满意的解。
因此，在实际的VO中，我们会结合这两种方法的优点。先使用代数方法估计一个粗略的运动，然后再用Bundle Adjustment进行优化，求得可精确的值。
2D-2D
问题描述
已知条件： 每个特征点的像素坐标；两张图像之间的特征点的匹配关系；相机内参
求解：相机的R和t
特征点不共面——计算基础矩阵或者本质矩阵
我们需要把像素坐标系转化为世界坐标系，因为只有这一转换过程包含\(R\)和\(t\)
因为相机内参已知，我们首先可以把像素坐标转化为相机坐标
-
为何此处的\(z_c\)不见了？
- 因为像素坐标\(p=\begin{bmatrix}u \\v \\1\end{bmatrix}\)是齐次坐标，是否乘上\(z_c\)对齐次与非齐次之间的转换并不构成影响
- 此处也没有涉及旋转转换
-
为什么不直接将像素坐标系转换为世界坐标系？
- 因为这一阶段的转化只涉及同一帧图片两个坐标系之间坐标的转化，而不涉及选定世界坐标系的问题，当我们需要在两帧图像直接进行转换计算时，我们才需要选定世界坐标系，因为不同的世界坐标系对应不同的结果
这时我们将\(P_{c_1}\)和\(P_{c_2}\)两个坐标连接起来
因为选择任意一个点作为世界坐标系原点都可以，不妨我们选择相机的初始位置\(P_{c_1}\)作为此处世界坐标系的原点\(P_w\)，这样处理的话，我们就可以把世界坐标系和像素坐标系之间的关系转换为不同时刻相机坐标系之间的计算
- 为何此处用\(R,t\)参与计算而不使用\(T\)
- 因为此处已经不是齐次坐标系，而是两个普通的坐标
- 如果要使用\(T\)，则需要对\(P_{c_1},P_{c_2}\)增加一维变成齐次坐标，同时其他的转换矩阵都需要进行变换
上述关系式中将\(P_{c_1},P_{c_2}\)分别用\(p_1,p_2\)表示代入后可得
两边同时乘\(t^{\wedge}\)，\(t^{\wedge}\)是反对称符号\(a=[a_1,a_2,a_3]，a^{\wedge}=A=\begin{pmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{pmatrix}\)，\(a^{\wedge}\cdot b=a\times b，可以用右手定则确定结果，四指方向为a\rightarrow b，大拇指方向为结果向量方向\)
因为\(t^{\wedge }t=0\)，所以消去得
两边同乘\((K^{-1}p_2)^T\)
因为\(t^{\wedge}p_2\)得到的向量垂直于\(t^{\wedge}\)和\(p_2\)，所以\(t^{\wedge}p_2(K^{-1}p_2)^T=0\)
本质矩阵E
\(P_1\)是为物体\(P\)在\(O_1\)相机坐标系的位置，\(P_2\)是为物体\(P\)在\(O_2\)相机坐标系的位置，\(O_2\)相对于\(O_1\)的旋转矩阵为\(R\)，位移为\(T\)，则\(P_2=R(P_1-T)\)，由于\(R\)为正交矩阵，可以改写成\((P_1-T)=R^TP_2\)。由于\(O_1P_1,O_1O_2,O_2P_2\)三个向量共面，所以混合积为0，即\((P_1-T)^T\cdot T\times P_1=(R^TP_2)^T\cdot T\times P_1=0\\P^T_2R\cdot (T\times P_1)=0\)
将叉乘写成矩阵相乘形式：
令\(S=\begin{bmatrix} 0&-T_z&T_y\\ T_z&0&-T_x\\ -T_y&T_x&0\end{bmatrix}\)，\(S\)是一个秩为2的矩阵，则\(P^T_2RSP_1=0\)
显然，\(P_1,P_2\)可以通过矩阵\(E=RS\)来约束，我们称\(E\)为本质约束矩阵(Essential Matrix)，其具有两个性质：
- 秩为2
- 只依赖于外部参数\(R,T\)
基础矩阵
由本质矩阵可知
此处\(p_1,p_2\)是在单位距离坐标系下的位置，如果我们想要分析图像，则需要转到像素坐标系下：
从而有
我们称矩阵\(F\)为基础矩阵\(F=M_2^{-T}RSM^{-1}_1\)，性质为：
- 秩为2
- 依赖于相机内参和外部参数\(R,T\)
特征点共面——计算单应矩阵
所以最终2D-2D问题转化为：根据匹配点的像素坐标，求出\(E\ or\ F\)或者\(H\)，然后求出\(R\ or\ t\)
存在的问题
-
尺度不确定性
-
用上面的方法估计出的相机平移向量t的值并没有单位，也就是说相机移动的距离只有相对值，没有绝对值，即齐次方程存在多解情况。这是单目相机固有的尺度不确定性问题，无法从根本上解决。
因此单目SLAM中一般把初始化后的t归一化，即把初始化时移动的距离默认为1，此后的距离都以这个1为单位。
-
-
初始化的纯旋转问题
- 单目初始化不能只有旋转，必须要有一定程度的平移，否则由于t趋近于0，导致无从求解R或者误差非常大。
- 估计出来的\(t\)是两帧之间的相机位移，其范数就是位移的长度，举例说明：如果向前2米，再向右1米，向后1米，向左1米，实际的轨迹应该是个英文字母"P"，但是是纯2D-2D的VO的话，t的范数都为1，画出的轨迹会是一个"口"字。所以如果是纯2D-2D的VO，画出的轨迹是不对的（即使整体乘以一个scale也不行，除非知道每一步的t的范数/长度），所以要正确的画出轨迹，必须要这样：第一第二帧用2D-2D，然后三角化得到3D点，后续帧通过3D-2D进行位姿估计，这样画出的轨迹整体与现实差一个比例scale，只需要对第一帧的t乘以某个scale，就可以得到跟现实一比一的轨迹，这应该这就是初始化的一种具体操作方式。
-
多于8对点的情况
- 如果匹配的点对数多于8（大多数情况都是这样），可以考虑充分利用这些点，而不是只从中选择8对用于计算。推荐的算法是随机采样一致性（Random Sample Consensus，RANSAC），该算法可以有效地避免错误数据对整体结果的影响。在代码中，只需要将
findFundamentalMat函数的第三个参数从CV_FM_8POINT换成CV_FM_RANSAC就可以了。
- 如果匹配的点对数多于8（大多数情况都是这样），可以考虑充分利用这些点，而不是只从中选择8对用于计算。推荐的算法是随机采样一致性（Random Sample Consensus，RANSAC），该算法可以有效地避免错误数据对整体结果的影响。在代码中，只需要将
