SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结
直接总结吧：这一节描述的视觉里程计就是检测特征点，特征点匹配，匹配上以后算位姿和深度。
特征点检测：
Fast特征点：以半径为3画圆，检测周围一圈一共16个点，设定阈值。有12个连续的点与该点亮度差超过阈值，确认为一个Fast特征点。（快速检测方式是先检查上下左右四个点，有三个点大于阈值才有可能是角点。因为要的是“连续”的点。有一个断掉了，就不能是连续的12个点了；然后通过非极大值抑制的方式筛选掉多余的点）
ORB特征点：改进fast特征点，具体是定义图像块的矩（mpq，p、q=0或1）再算质心，连接几何中心与质心得到方向向量作为特征点的方向。这个叫Oriented Fast 关键点。然后计算BRIEF描述子：比较关键点附近两个像素p和q的大小关系，用0和1表示。如果选128个p和q，就是128维二进制向量。
SIFT特征点：尺度不变特征变换（Scale-invariant feature transform）：大致方法是首先搜索所有尺度下图像的位置，通过高斯微分函数来识别潜在的对于尺度和旋转不变的兴趣点，然后在候选位置通过拟合精细的模型来确定位置和尺度，再基于图像梯度，分配给关键点一个或多个方向，最后在每个关键点的周围邻域，在选定的尺度下测量图像局部梯度来描述关键点。（总的来说就是根据梯度描述关键点）
SURF特征点：加速稳健特征（Speeded Up Robust Features）：加速版的SIFT。通过修改滤波器的尺寸和模糊度从而得到不同层级的影像，别的和SIFT差不多。
对于速度来说，ORB最快，SURF次之，SIFT最慢。
对于尺度变换的鲁棒性来说：SURF>SIFT>ORB(没有）
对于旋转模糊鲁棒性来说：SURF>ORB~SIFT
特征点匹配：
（1）暴力匹配，用汉明距离（描述子的不同位数）计算描述子距离，最近的作为匹配点。
（2）快速近似最近邻（FLANN）算法，这个书里没写细节。
初步筛选错误匹配：
经验方式：给定一个最小距离，如果汉明距离小于最小距离的两倍，就认为匹配没有太大的问题。
2D-2D
走到现在这里，特征点已经匹配出来了。也就是说，我们现在得到了一些像素点的配对（p1，p2）……想要根据这个恢复R和t。
接下来主要两个内容，对极几何求位姿，三角测量用求出来的位姿求深度。
对极几何
关于这块，高博的书里像素坐标用的是p1，p2，归一化平面坐标用的x1，x2。其实我觉得这两个符号换换更好。另外公式一堆，对于我来说，我觉得有点抓不住重点。我按适合自己的顺序总结一下：
一个点，在两个归一化坐标平面表示为x1和x2，用R和t像上面这样建立起联系来。
两边都左乘t^，t^乘以一个向量，相当于t和这个向量做叉乘，产生的新的向量垂直于t和另一个向量（右手定律嘛）
另外，自己和自己叉乘结果是0
就得到了这个：
这时候我们是想让它一边为0，好求R和t
注意等式左边，t^x2产生的向量，垂直于二者，那么t^x2再左乘一个x2T，就是两个垂直的向量求内积，结果是0
所以得到下式：
上面这玩意就是对极几何，t^R表示成E，这个E就是本质矩阵（Essential Matrix）。
不过我们刚开始说了，特征点匹配以后得到的是两个像素坐标系下的点p1和p2，并不是x1和x2，两者之间差了个内参K的。
我们知道归一化平面上的点，左乘个内参就是像素平面上的点了，也就是说p=Kx，那么x就等于K-1 p
因此上面的式子变成了：
把p留下来，中间的写成F，这个F就是所谓的基础矩阵（Fundametal Matrix）
所以总结为：两边是两个像素点，中间就是基础矩阵F（太基础了，不能再往深里化了）；两边是两个归一化点，中间就是本质矩阵E。
其实E和F是一码事，因为内参K基本已知。二者求出一个就行了。
书中给出的是求本质矩阵E的过程。求法就是直接设一个E矩阵，九个点分别是e1……e9，两边设两个归一化的点的向量，展开乘，化出一个线性方程组的形式。大小是8*9。（除掉了一个常数，最后一个是1。实际上只需要最少5个点，因为t^R一共6个*度，又去掉了一个常数。但是为了解这个方程，所以用8个点）
走到这里，E就求解出来了。但是E并不等同于位姿，因为E是t^R，想知道确切的位姿，需要拆开E，这个用的方法是SVD分解，就用上面说的E的奇异值性质，给了一个先验知识，说是E的奇异值满足[σ，σ，0]T的形式。分解出来是4个解，但是只有一个对于两个相机而言，都是正的深度。关于这块，高博书里写的是相当的含糊，我这里放个截图：
看了这些内容，我依旧不能明白怎么根据E求R和t。比如上面的RZ是什么，我就不理解。要求的不就是旋转矩阵吗?它和R1和R2有什么关系？特意去请教了一些师兄。
这块是比较难算的，因此高博书里没有展开介绍。上面7.14式子里的两个RZ应该是不一样的，都是各自左边的R1和R2旋转90度得到的矩阵。我观察了一下书上149页的代码，里面直接调用opencv的*recoverPose()，因此这块理解到这里应该就足够了。
用SVD之前，求出的E可能不满足奇异值是[σ，σ，0]T，需要刻意调整下。
F矩阵书里没说，大概是因为E和F就差了一个内参，本质上是一码事。
单应矩阵H
单应矩阵(Homography)的意义在于避免“退化”现象造成基础矩阵*度下降的问题。一般出现退化现象是因为特征点共面或者相机发生纯旋转。对于纯旋转的情况，E=t^R，纯旋转意味着t是0，那么E就是0，根据E恢复R就完全无从谈起。
对于特征点共面的情况，算H大概是更方便一些吧。
公式的推导颇有些奇妙，首先：
这个公式就是一个面的公式，假如一个面的法向量是n=(A,B,C)，还有一个点P(x,y,z)落在该面上，那么该面的公式就是：
Ax+By+Cz+d=0
然后根据这个式子，得到-nT*P/d=1,把这个式子乘到p=K（RP+t）的t后面，然后提出P，建立一个p1和p2的关系：
把中间的记为H，那么p2=H*p1，H就是单应矩阵。（发现没有，假如没有t，中间算出的H就是KRK-1，直接就知道R了）
现在把这个公式和上面的F矩阵再做个对比：
F矩阵：
H矩阵根据已经配对好的p1和p2，算中间的H，然后把向量和矩阵乘开，让h9为0，一组匹配点可以构造两项约束（这样八个未知数的项就都有了），用4对匹配点，算H。算出H以后，恢复Rt也是会得到四个解，最后也是排除得到R和t。
说到这里，对极几何就结束了，有三点注意：
1.单目尺度不确定性。t不晓得单位，因此需要初始化，让两张图像有一个平移，之后的平移都以这个平移为基本单位1。
2.虽然说如果没有t，可以用H算R，但是之后三角测量必须有t，所以最好左右平移，让它初始化。
3.上面求解的方法，都是直接求线性方程组，也就是“直接线性变换法”（DLT），如果匹配的点多了，直接解肯定是没解的，不唯一，这样可以用最小二乘法。另外还可以用Ransac算法解决误匹配的问题。我针对神经网络写过一个改进的Ransac算法，这里放上地址：https://github.com/Zkjoker/ICCV-PnPRansac_mutiprocessing
如果感兴趣可以帮我点个星！里面的细节留言我可以解决。
三角测量
这块内容不多：
有同一个点在两个图里的归一化相机坐标x1和x2
现在都变到相机坐标系下，也就是都乘以各自的第三维s：
s1x1是x1位置的相机坐标系下的坐标，s2x2是x2位置的相机坐标系下的坐标
把s2x2变换到x1相机坐标系下，因为是同一个点，所以肯定相同：
我们的目的是想求s1和s2，这时候要用技巧了，两边都左乘x1^,左边相当于自己和自己做叉乘，是0，右边就是：
显然这里R和t在对极几何已经求完了，x1和x2又可以根据在图像上的位置求内参的逆矩阵求得，只有s2一个未知数。通过最小二乘可以算出来。
值得注意的是，平移太小，三角化的精度不够；平移太大的话，特征点匹配又可能失效。这二者构成一对矛盾。
另外，不是特征点的像素点，就完全不能知道深度了吗？当然不是！具体理论见：https://blog.****.net/zkk9527/article/details/89095967，是通过极线搜索与块匹配找到特征点的对应关系，通过深度滤波器来实现深度值的更新与确定。
3D-2D
最开始先直接总结下：
这部分的内容的前提是一个点的3D坐标已知，另一个只知道在图像上的2D点坐标。
书里只介绍了直接线性变换法、P3P、非线性优化（Bundle Adjustment）三种。
第一种就是根据线性代数直接算，硬刚；第二种P3P是已知三个点的世界坐标和这三个点的像素坐标，把这三个点的相机坐标系下的坐标算出来，转换成ICP问题；第三种是非线性优化，最小化重投影误差来解决（这种是把相机位姿和空间点位置都看成优化变量）。
直接线性变化法：
中间的t不是T，而是增广矩阵R|t。
这个里面，s未知，12个t未知，别的已知。都展开以后，前两行的约束里会有s，可以用第三行消掉它。
比如假设第一行是t1T，t1T*P=s*u1，s还等于t3T*P，把s代入，得到两个式子：
把t1，t2，t3写成未知数的形式，用线性方程组求解。一对匹配点，才能都把t1，t2，t3包含进来，所以t有12维，得用6对匹配点。
求出增广矩阵以后，值得一提的是，需要对左边的3*3的矩阵块用一个最好的旋转矩阵来近似，可以用QR分解完成。
P3P：
这个看图比较头疼，简化了很多符号。总体上是用余弦定理做的：
以上的图，OA,OB,OC是未知量。x=OA/OC,y=OB/OC，用余弦定理得到：
把等式右侧的三项设为v，u*v，w*v，u=BC2/AB2，w=AC2/AB2是已知的，cos都已知，那么目前是x，y不知道，v不知道。把第一个式子代入后俩，消去v，式子变成：
求出x，y以后，代入第上面三个式子中的第一个，可以求出v，v是AB2/OC2，就可以求出OC，别的也就依次都知道了。
关于这点，有几个值得注意：
1.cos值如何已知？
2D点图像位置已知，光心位置已知，可以计算其夹角，进而计算余弦。
2.求出的OA,OB,OC是什么？
可以当成是场景深度。场景深度和第三维的s是不一样的，s也是深度，但不是距离光心的深度，应该是所在平面距离光心所在平面的深度。但是可以互相求解。
3.之后怎么解位姿？
相机坐标系下3D坐标已知，可以据此求解3D-3D的ICP问题。
Bundle Adjustment
这个内容是构建重投影误差，让重投影误差（观测值减去预测值）最小化。把相机位姿和空间点位置都看成优化变量，那么重投影误差函数相当于有两个自变量，一个是位姿，一个是对空间点P的位置求导。记P变换到相机坐标系下的位置是P'。
如果是对位姿求偏导，用扰动的方式，误差e先对P'求偏导，P’再对位姿的李代数求偏导，两个分开得到两个雅克比矩阵，然后乘在一起，得到一个2*6的雅克比矩阵。这样就求得了误差函数对位姿的导数矩阵。关于这里的雅克比矩阵，是比较重要的，因为代码里的雅克比矩阵是自己写的，如果不理解雅克比矩阵是怎么求出来的，代码就不能看懂。这个雅克比矩阵在后面的光度误差里面还会重复出现，因此我总结了一下：https://blog.****.net/zkk9527/article/details/89033936，看到这里的人一定要看一下这节！
如果是对P求偏导，第一项也是e先对P’求偏导，第二项P‘对P求偏导结果就是个旋转矩阵R，因此这部分的导数矩阵就是第一项和R相乘。
综上，都是加一个小量扰动，然后通过求解对于小量的偏导，从而解决不能直接求偏导的问题。详情请看我的非线性优化这节的笔记：https://blog.****.net/zkk9527/article/details/88955019
3D-3D
这部分内容也是两种方式：代数方法和非线性优化方法。
代数方法是SVD方法，非线性优化还是类似于BA方式。
SVD方式：
这部分内容只看上面的步骤，觉得有些抽象。其实定义的误差比较简单，两个3D点，坐标是P1和P2，P2’根据R*P1+t恢复得到，P2和P2‘应当是同一个点，定义二者之间的误差为e，构建误差平方和。
误差函数是经过特殊技巧处理的：
自己加了又减去一些东西，最后化简得到的东西，等式右边三项，第一项只有R优化，优化它最小，就能得到R，代入得到第二项的t，第三项根据质心的定义为0（质心就是所有位置求和的均值），这样也就是上面的三个步骤了！
那么用代数方法优化该咋优化？
就是SVD方式。书里这直接给出了步骤，
把它拆开，得到三项：
前两项与R无关，因为第二项R和自身转置乘积为1（R的固有性质），就是优化第三项。
第三项这么变：
之后定义了W（∑后的东西），对W进行SVD分解，W满秩的时候，R=UVT，然后根据R恢复t。
书里代码所说，ICP目前还没有ICP算法，需要手动实现。
关于这里，我有两个疑问：
第一，等式左边到等式中间是怎么变的？迹有这种性质吗？
第二，为什么根据W的SVD分解，就可以得到R了？（在对极几何那节，本质矩阵E=t^R算出来以后，对E进行奇异值分解，那里我还能理解，虽然那个步骤我看不太懂，但是opencv里有*可以直接调用，稀里糊涂的也就知道R和t了。但是这节ICP的这里，根据一个目标函数一直推推推，最后挑出其中的一项作SVD分解，就直接求得了R和t吗？目标函数不应该是应该让它接近0吗？我没看到有这个步骤，好像是知道了每个点的去质心坐标，直接就求得W，然后算出R来了，没看到对目标函数优化的那个min步骤）
针对这两点问题，我特意又去看了b站高博的网课。（偷偷说一下，我感觉高博用嘴皮子讲课的水平确实不咋地，很像大学课堂上那些照本宣科的老师，其实我感觉如果是我讲的水平说不定更好，嘻嘻）
第一个问题，高博是这么说的：矩阵论里有，反正有这么个性质。（好吧）
第二个问题，他说“感兴趣的自己去看paper，反正步骤就是这么个步骤” （当然我一点也不感兴趣，我很不喜欢数学推导。他没有解释为什么没看到对目标函数优化的那个min步骤，我的理解是这样的：令目标函数为0，优化出来的结果，恰好就是这个R=UVT的形式）
非线性优化方法：
这块就是老生常谈了，对误差函数进行李代数扰动，迭代更新，最后找到合适的位姿。参见我在非线性优化那节的笔记。
值得注意的有两点：
1.有人已经证明，用ICP求解问题有唯一解或无穷多解情况。如果是唯一解的情况，那么极小值就是全局最优值。因此用ICP求解有一大好处：可以任意选定初始值。
2.可以混合使用PnP和ICP，深度已知就用ICP算法。