SLAM5-视觉里程计的算法

一、视觉里程计的定义
视觉里程计主要是根据相邻图像信息粗略估计出相机的运动，为后端提供初始值，常用的算法有特征点法和直接法
特征点：特征点=关键点+描述子。关键点是指像素坐标，描述子指坐标周围像素的信息。有很多特征点，SIFT特征，SURF特征，ORB特征
SIFT特征：尺度不变特征变换（Scale-invariant feature transform）：大致方法是首先搜索所有尺度下图像的位置，通过高斯微分函数来识别潜在的对于尺度和旋转不变的兴趣点，然后在候选位置通过拟合精细的模型来确定位置和尺度，再基于图像梯度，分配给关键点一个或多个方向，最后在每个关键点的周围邻域，在选定的尺度下测量图像局部梯度来描述关键点。（总的来说就是根据梯度描述关键点）

SURF特征：加速稳健特征（Speeded Up Robust Features）：加速版的SIFT。通过修改滤波器的尺寸和模糊度从而得到不同层级的影像，别的和SIFT差不多。

对于速度来说，ORB最快，SURF次之，SIFT最慢。

对于尺度变换的鲁棒性来说：SURF>SIFT>ORB(没有）

对于旋转模糊鲁棒性来说：SURF>ORB~SIFT

ORB特征：ORB特征分为两步，1、利用FAST角点，提取关键点 2、BRIEF描述子计算
s
FAST角点：对每个像素，以半径为3画圆，检测周围一圈一共16个点，设定阈值。有12个连续的点与该点亮度差超过阈值，确认为一个Fast特征点。（快速检测方式是先检查上下左右四个点，有三个点大于阈值才有可能是角点。因为要的是“连续”的点。有一个断掉了，就不能是连续的12个点了；然后通过非极大值抑制的方式筛选掉多余的点）
FAST关键点：角点没有尺度和旋转的描述；通过构建图像金字塔，角点可以具有尺度特征，通过定义图像块的矩，得到特征点的方向向量，从而具有旋转描述。
旋转描述：
在小图像块B中，定义B的矩是

定义B的质心是

连接B的集合中心O和C，就可以得到方向向量OC。

BRIEF描述子比较关键点附近两个随机像素p和q的大小关系，用0和1表示。如果选128个p和q，就是128维二进制向量

二、把两张图片中的特征点进行匹配

（1）暴力匹配，用汉明距离（描述子的不同位数）计算描述子距离，最近的作为匹配点。

（2）快速近似最近邻（FLANN）算法，这个书里没写细节。

三、根据匹配的特征点估计相机的运动(T)

因为匹配的特征点的信息不同，有不同的方法来进行估计
2D-2D:单目相机，只知道像素点2D坐标，使用对极几何
3D-3D:双目，RGBD，知道像素点3D坐标，使用ICP
3D-2D:两张图片，一张用双目，一张用单目拍摄，使用PnP
对极几何：如果相机1到相机2经过了R，t变换，相机内参为K，那么对于同一个点P在两个相机的成像p1,p2存在对极约束关系：
对极约束推导过程：

两边都左乘，t^乘以一个向量，相当于t和这个向量做叉乘，产生的新的向量垂直于t和另一个向量（右手定律嘛）

t^ x2产生的向量，是垂直于二者的向量，那么t^x2再左乘一个x2T，就是两个垂直的向量求内积，结果是0

由于p=Kx，可以得到对极约束：

t^R表示成E，这个E就是本质矩阵
除去p2T,p1之外的部分是F，F是基础矩阵
根据对极约束求解相机的运动：
1、根据匹配的像素点求出E或者F
2、根据E或F求出R，t就求出了相机的运动
如何求E：
E是33的矩阵，但是对E乘以任意非零常数之后仍然满足对极约束，所以E具有尺度等价性，可以只用8对匹配点来求解E矩阵；
考虑一堆匹配点，他们的归一化坐标是x1=[u1,v1,1],x2=[u2,v2,1],设E的9个元素为未知量，根据对极约束，求解下面的公式即可
X1EX2=0；
实际上t有3个自由度，R有3个自由度，加上E有尺度等价性，最少可以用5个匹配点就可以求解E矩阵
求出来的E没有内在性质（奇异值不是k,k,0的形式）怎么办
取此时的E=Udiag{ (k1+k2)/2, (k1+k2)/2,0}
如何根据E求t,R:
将E利用奇异值分解SVD如下，U,V是正交矩阵

根据E的内在性质

求出R,t可能的解

其中的未知量Rz等于旋转向量转换为的矩阵：
Matrix3d R_z1 = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); //定义旋转矩阵,沿 Z 轴旋转 90 度
Matrix3d R_z2 = AngleAxisd(-M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix(); //定义旋转矩阵沿 Z 轴旋转 -90 度

把任意一个空间点P,代入求出的四个可能解中，检测该点在两个相机下的深度，就可以确定哪个是正确的解了。

如何求F：
1、F的秩为2：
t ^ R相当于t^ 和R相乘，两矩阵相乘，其秩不大于各矩阵的秩。t^如下所示：

第三列可以由前两列线性表示：第二列除以-Tz乘以Ty，第一列除以Tz乘以负Tx，之后相加。

因此秩是2。
2、F有7个自由度：
因为F是3*3的矩阵，理论是9个自由度，但是由于秩为2，导致少了一个自由度，再加上尺度等价性，因此是9-2=7个自由度。
3、F的求解
F和E的求解过程，E最少五个点，F最少七个点，但是都用八点法来做

针对对极约束的缺点，提出了单应矩阵H
单应矩阵(Homography)的意义在于避免“退化”现象造成基础矩阵自由度下降的问题。一般出现退化现象是因为特征点共面或者相机发生纯旋转。对于纯旋转的情况，E=t^R，纯旋转意味着t是0，那么E就是0，根据E恢复R就完全无从谈起。
H推导过程：
假设特征点p1,p2在P平面共面，P平面的法向量是n=(A,B,C)，还有一个点P(x,y,z)落在该面上，那么该面的公式就是：Ax+By+Cz+d=0，写成向量形式就是：

根据这个式子，得到-nTP/d=1,
把这个1，乘到p2=K（RP+t）的t后面，然后提出,括号内的公因子P，再利用p2和P的坐标转换，建立一个p1和p2的关系：又可以写成p2=Hp1

H求解过程：
H矩阵根据已经配对好的p1和p2，算中间的H，然后把向量和矩阵乘开，让h9为1，一组匹配点可以构造两项约束（这样八个未知数的项就都有了），用4对匹配点，算H。算出H以后，恢复Rt也是会得到四个解，最后也是排除得到R和t。

初始化和求解矩阵的注意事项：
1.因为E有迟钝等价性，所以解得的t不晓得单位，因此需要初始化，让两张图像有一个平移，之后的平移都以这个平移为基本单位1。

2.虽然说如果没有t，可以用H算R，但是之后三角测量必须有t，所以最好左右平移，让它初始化。

3.上面求解的方法，都是直接求线性方程组，也就是“直接线性变换法”（DLT），如果匹配的点多了，直接解肯定是没解的，不唯一，这样可以用最小二乘法。另外还可以用Ransac算法解决误匹配的问题。我针对神经网络写过一个改进的Ransac算法，这里放上地址：https://github.com/Zkjoker/ICCV-PnPRansac_mutiprocessing

根据相机的运动估计特征点的空间位置：
三角测量：因为是2D点，所以要通过三角测量来估计特征点的深度。
推导过程：设s1,s2为深度，x1,x2是归一化相机坐标。根据相机之间的坐标转换有：

两边都左乘x1^,左边相当于自己和自己做叉乘，是0，右边就是

这里R和t在对极几何已经求完了，x1和x2又可以根据在图像上的位置求内参的逆矩阵求得，只有s2一个未知数。通过最小二乘可以算出来。

值得注意的是，平移太小，三角测量的精度不够；平移太大的话，特征点匹配又可能失效。这二者构成一对矛盾；三角测量我们可以讲，仅仅通过一对匹配点来求位姿，三角化是多次观测到这个点，联立最小二乘，根据图像中的位置把它给算出来。反正二者是有区别的。

3D-2D估计相机运动有直接线性变换法、P3P、非线性优化（Bundle Adjustment）三种方法。

直接线性变化法：特征点x1像素坐标是（u1,v1,1）,在世界坐标系下的坐标是（X，Y，Z）

中间的矩阵是增广矩阵R|t，左边三列代表旋转R,右边代表平移t，一对匹配点可以求两个t,所以要6对匹配点；由于求出来的R不一定满足SO（3）的条件，所以最好把R进行一次QR分解，将R用一个最好的旋转矩阵来近似
P3P： 把这个问题变为3D-3D的问题来解决，输入数据为三组匹配点，对应关系如下图：
A，B，C是三个点在世界坐标系下面的坐标，a,b,c是三个点在图片上的 坐标

P3P推导过程：
OA,OB,OC是未知量。x=OA/OC,y=OB/OC，用三个大三角形的余弦定理得到：

把等式右侧的三项设为v，uv，wv，u=BC2/AB2，w=AC2/AB2是已知的，把第一个式子代入2,3，得到下面，求出x,y代入1式，即可求出OC，也就得到了在相机坐标系下的三维坐标。

关于这点，有几个值得注意：

1.cos值如何已知？

2D点图像位置已知，光心位置已知，可以计算其夹角，进而计算余弦。

2.求出的OA,OB,OC是什么？

可以当成是场景深度。场景深度和第三维的s是不一样的，s也是深度，但不是距离光心的深度，应该是所在平面距离光心所在平面的深度。但是可以互相求解。

3.之后怎么解位姿？

相机坐标系下3D坐标已知，世界坐标系下的A，B，C已知，可以据此求解3D-3D的ICP问题

非线性优化（Bundle Adjustment）
不利用几何关系，而是对求解出的PnP问题的初值T做进一步的优化。已知n对匹配的3D空间点P【X，Y，Z】世界坐标和他们的图片投影U【u，v】，那么理论上应该是sU = KT*P,但是由于有误差，灯饰不成立，所以可以把误差求和构建最小二乘问题，然后找到最好的T，使得误差最小。
重投影误差：把预测的图片投影U，与实际观察到图片上的投影做差

为了优化这个目标函数，要进行求导，找到最小量。用的方法是李代数扰动（因为不方便直接对位姿求导为0），所以是对扰动的小量求导，也就是：

P’是P变换到相机坐标系下的坐标【X’，Y‘，Z’】
对于左边，e=||ui-u||22，ui为常量，所以实际是u对于P‘求导

对于右边，[I,-(P’)^],也就是：

[ 1, 0, 0, 0, Z’, -Y‘ ]

[ 0, 1, 0, -Z’, 0, X‘ ]

[ 0, 0, 1, Y’, -X‘, 0 ]

[ 0, 0, 0, 0, 0, 1 ]
最终结果：

此外，重投影误差也可以求解关于场景坐标P的导数，这个比较简单，等于先对相机坐标系下P’求导，再右乘R。因为P’=RP+t。

3D-3D：也叫作ICP问题，就是两组匹配好的世界坐标系下面的3D点（P与P），如何使用它们来估计相机的T。主要使用两种方式：代数方法和非线性优化方法。

代数方法：SVD方法
定义SVD误差：
两个3D点，坐标是P和P’，P’根据RP+t恢复得到，P和P‘应当是同一个点，定义二者之间的误差为e，构建误差平方和。

求解误差：第三项为0，求出使得第一项最小的R，代入第二项，使得第二项为0，就可以解出t。
根据以下步骤化简第一项：

求解优化后的项：前两项与R无关，因为第二项R和自身转置乘积为1（R的固有性质），就是优化第三项。

第三项：

进一步写成：-tr(RW),
对W进行SVD分解，W=U*奇异值构成的对角矩阵 *V；W满秩的时候，R=UV（的转），然后根据R恢复t。如果R的行列式为负，取-R
这里求出来的W能够让RW的迹最大，从而-tr（RW)最小，然后总的和最小，最接近0
非线性优化：类似于BA方式。误差函数是(p-Tp’),对误差函数进行李代数扰动，迭代更新，最后找到合适的位姿。

值得注意的有两点：

1.有人已经证明，用ICP求解问题有唯一解或无穷多解情况。如果是唯一解的情况，那么极小值就是全局最优值。因此用ICP求解有一大好处：可以任意选定初始值。

2.可以混合使用PnP和ICP，深度已知就用ICP算法。

代码1：根据orb特征匹配两幅图片：https://blog.csdn.net/Summer_star_summer/article/details/107440645
代码2：手写ORB特征
代码3：2d-2d对极几何约束匹配
https://blog.csdn.net/Summer_star_summer/article/details/107441184
代码4：3d-2d使用Opencv自带的函数，高斯牛顿迭代或者g2o
https://blog.csdn.net/Summer_star_summer/article/details/107442038
代码5:3d-3d，使用SVD分解求R,t；使用非线性优化g2o求解T
https://blog.csdn.net/Summer_star_summer/article/details/107445208