直接总结吧：这一节描述的视觉里程计就是检测特征点，特征点匹配，匹配上以后算位姿和深度。

特征点检测：

Fast特征点：以半径为3画圆，检测周围一圈一共16个点，设定阈值。有12个连续的点与该点亮度差超过阈值，确认为一个Fast特征点。（快速检测方式是先检查上下左右四个点，有三个点大于阈值才有可能是角点。因为要的是“连续”的点。有一个断掉了，就不能是连续的12个点了；然后通过非极大值抑制的方式筛选掉多余的点） ORB特征点：改进fast特征点，具体是定义图像块的矩（mpq，p、q=0或1）
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结再算质心，
连接几何中心与质心得到方向向量作为特征点的方向。
这个叫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。其实我觉得这两个符号换换更好。另外公式一堆，对于我来说，我觉得有点抓不住重点。我按适合自己的顺序总结一下：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结一个点，在两个归一化坐标平面表示为x1和x2，用R和t像上面这样建立起联系来。两边都左乘t^，t^乘以一个向量，相当于t和这个向量做叉乘，产生的新的向量垂直于t和另一个向量（右手定律嘛）另外，自己和自己叉乘结果是0 就得到了这个：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结这时候我们是想让它一边为0，好求R和t 注意等式左边，t^x2产生的向量，垂直于二者，那么t^x2再左乘一个x2T，就是两个垂直的向量求内积，结果是0 所以得到下式：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结上面这玩意就是对极几何，t^R表示成E，这个E就是本质矩阵（Essential Matrix）。不过我们刚开始说了，特征点匹配以后得到的是两个像素坐标系下的点p1和p2，并不是x1和x2，两者之间差了个内参K的。我们知道归一化平面上的点，左乘个内参就是像素平面上的点了，也就是说p=Kx，那么x就等于K-1 p 因此上面的式子变成了：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结把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个解，但是只有一个对于两个相机而言，都是正的深度。关于这块，高博书里写的是相当的含糊，我这里放个截图：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结看了这些内容，我依旧不能明白怎么根据E求R和t。比如上面的RZ是什么，我就不理解。要求的不就是旋转矩阵吗?它和R1和R2有什么关系？特意去请教了一些师兄。这块是比较难算的，因此高博书里没有展开介绍。上面7.14式子里的两个RZ应该是不一样的，查阅别人的资料：https://www.cnblogs.com/CaptainLL/p/10900486.html

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 度

具体实现上面公示里的一样，Rz就是一个向z轴上方的向量，变成矩阵的形式，然后绕z轴旋转。我观察了一下书上149页的代码，里面直接调用opencv的轮子recoverPose()，因此这块理解到这里应该就足够了。查询了资料，补上这块的数学推导过程：https://blog.csdn.net/qq_30356613/article/details/80587649 用SVD之前，求出的E可能不满足奇异值是[σ，σ，0]T，需要刻意调整下。面试容易考，E有5个自由度。 F矩阵书里没说，大概是因为E和F就差了一个内参，本质上是一码事。但是，这块内容也得补充交代，因为面试会考！！借用浙大章国峰老师的PPT内容：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结 F有7个自由度，原因是什么？因为F是3*3的矩阵，理论是9个自由度，但是由于秩为2，导致少了一个自由度，再加上尺度的自由度也没有，因此是9-2=7个自由度。为什么秩为2？因为：t^R相当于t^和R相乘，两矩阵相乘，其秩不大于各矩阵的秩。t^如下所示：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结第三列可以由前两列线性表示：第二列除以-Tz乘以Ty，第一列除以Tz乘以负Tx，之后相加。因此秩是2。 F和E的求解过程，E最少五个点，F最少七个点，但是都用八点法来做。 单应矩阵H 单应矩阵(Homography)的意义在于避免“退化”现象造成基础矩阵自由度下降的问题。一般出现退化现象是因为特征点共面或者相机发生纯旋转。对于纯旋转的情况，E=t^R，纯旋转意味着t是0，那么E就是0，根据E恢复R就完全无从谈起。对于特征点共面的情况，算H大概是更方便一些吧。公式的推导颇有些奇妙，首先：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结这个公式就是一个面的公式，假如一个面的法向量是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的关系：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结把中间的记为H，那么p2=H*p1，H就是单应矩阵。（发现没有，假如没有t，中间算出的H就是KRK-1，直接就知道R了）现在把这个公式和上面的F矩阵再做个对比： F矩阵：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结 H矩阵根据已经配对好的p1和p2，算中间的H，然后把向量和矩阵乘开，让h9为1，一组匹配点可以构造两项约束（这样八个未知数的项就都有了），用4对匹配点，算H。算出H以后，恢复Rt也是会得到四个解，最后也是排除得到R和t。自由度为8，四对匹配点，每一对匹配点贡献两个约束。（不过不能有三点共线的情况）说到这里，对极几何就结束了，有四点注意： 1.单目尺度不确定性。t不晓得单位，因此需要初始化，让两张图像有一个平移，之后的平移都以这个平移为基本单位1。 2.虽然说如果没有t，可以用H算R，但是之后三角测量必须有t，所以最好左右平移，让它初始化。 3.上面求解的方法，都是直接求线性方程组，也就是“直接线性变换法”（DLT），如果匹配的点多了，直接解肯定是没解的，不唯一，这样可以用最小二乘法。另外还可以用Ransac算法解决误匹配的问题。我针对神经网络写过一个改进的Ransac算法，这里放上地址：https://github.com/Zkjoker/ICCV-PnPRansac_mutiprocessing 如果感兴趣可以帮我点个星！里面的细节留言我可以解决。 4.总结：E是
，x是归一化相机平面上的点，E有5个自由度，最少五个点，但是用8点法。 F是
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结，p是像素平面上的点，F有7个自由度，最少7个点，一般也用8点法。 H是
，p是像素平面上的点，H有8个自由度，最少四个匹配点，因为每对匹配点可以提供两个约束。为什么要记这个东西呢，因为面试要考，不记不行。

三角测量

这块内容不多：有同一个点在两个图里的归一化相机坐标x1和x2 现在都变到相机坐标系下，也就是都乘以各自的第三维s： s1x1是x1位置的相机坐标系下的坐标，s2x2是x2位置的相机坐标系下的坐标把s2x2变换到x1相机坐标系下，因为是同一个点，所以肯定相同：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结我们的目的是想求s1和s2，这时候要用技巧了，两边都左乘x1^,左边相当于自己和自己做叉乘，是0，右边就是：
显然这里R和t在对极几何已经求完了，x1和x2又可以根据在图像上的位置求内参的逆矩阵求得，只有s2一个未知数。通过最小二乘可以算出来。值得注意的是，平移太小，三角测量的精度不够；平移太大的话，特征点匹配又可能失效。这二者构成一对矛盾。注意，在深蓝学院举办的视觉SLAM进阶-VIO视觉惯性里程计里面讲到的三角化，是这样的：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结这个和书中的三角测量还是有点区别的，有点像PNP那节的“直接线性变换法”（下面会说），y是世界坐标系下的坐标，u，v是像素坐标系下的坐标，P相当于是增广矩阵R|t，在多个位置观测到同一个点，联立上面的方程组，u1乘以第三行减去第一行，u2乘以第三行减去第二行……最后这样求解：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结取y=u4，即为这个最小二乘解，也是这个点在世界坐标系下的坐标。（数学证明我就不列出来了，之后我找完工作，或者找工作期间，会总结一个VIO的笔记，到时候在里面公布出来）这块14讲里是没有的，希望同学们尽量看懂，这个补充看不懂就算了，以后再学也行。总之三角测量我们可以讲，仅仅通过一对匹配点来求位姿，三角化是多次观测到这个点，联立最小二乘，根据图像中的位置把它给算出来。反正二者是有区别的。另外，不是特征点的像素点，就完全不能知道深度了吗？当然不是！具体理论见：SLAM14讲学习笔记（九）单目稠密重建、深度滤波器详解与补充（纠正第13讲建图中的错误），是通过极线搜索与块匹配找到特征点的对应关系，通过深度滤波器来实现深度值的更新与确定。

3D-2D

最开始先直接总结下：这部分的内容的前提是一个点的3D坐标已知，另一个只知道在图像上的2D点坐标。书里只介绍了直接线性变换法、P3P、非线性优化（Bundle Adjustment）三种。第一种就是根据线性代数直接算，硬刚；第二种P3P是已知三个点的世界坐标和这三个点的像素坐标，把这三个点的相机坐标系下的坐标算出来，转换成ICP问题；第三种是非线性优化，最小化重投影误差来解决（这种是把相机位姿和空间点位置都看成优化变量）。

直接线性变化法：

SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结中间的t不是T，而是增广矩阵R|t。这个里面，s未知，12个t未知，别的已知。都展开以后，前两行的约束里会有s，可以用第三行消掉它。比如假设第一行是t1T，t1T*P=s*u1，s还等于t3T*P，把s代入，得到两个式子：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结把t1，t2，t3写成未知数的形式，用线性方程组求解。一对匹配点，才能都把t1，t2，t3包含进来，所以t有12维，得用6对匹配点。 求出增广矩阵以后，值得一提的是，需要对左边的3*3的矩阵块用一个最好的旋转矩阵来近似，可以用QR分解完成。（可以注意到，这块和我上面补充的“三角化”的内容，是一个逆过程。三角化那块，意思是我把增广矩阵知道了，我就要根据二维点算深度。这块恰恰是反的，我知道了深度的3D点，我要求的是增广矩阵）

P3P：

这个看图比较头疼，简化了很多符号。总体上是用余弦定理做的：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结以上的图，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，式子变成：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结求出x，y以后，代入第上面三个式子中的第一个，可以求出v，v是AB2/OC2，就可以求出OC，别的也就依次都知道了。关于这点，有几个值得注意： 1.cos值如何已知？ 2D点图像位置已知，光心位置已知，可以计算其夹角，进而计算余弦。 2.求出的OA,OB,OC是什么？可以当成是场景深度。场景深度和第三维的s是不一样的，s也是深度，但不是距离光心的深度，应该是所在平面距离光心所在平面的深度。但是可以互相求解。 3.之后怎么解位姿？相机坐标系下3D坐标已知，可以据此求解3D-3D的ICP问题。另外补充：还有EPNP算法书中没有介绍，感兴趣的同学可以阅读：SLAM学习笔记（十六）快速了解EPnP算法原理

Bundle Adjustment

这个内容是构建重投影误差，让重投影误差（观测值减去预测值）最小化。把相机位姿和空间点位置都看成优化变量，那么重投影误差函数相当于有两个自变量，一个是位姿，一个是对空间点P的位置求导。记P变换到相机坐标系下的位置是P’。如果是对位姿求偏导，用扰动的方式，误差e先对P’求偏导，P’再对位姿的李代数求偏导，两个分开得到两个雅克比矩阵，然后乘在一起，得到一个2*6的雅克比矩阵。这样就求得了误差函数对位姿的导数矩阵。关于这里的雅克比矩阵，是比较重要的，因为g2o源码里显式的给出了雅克比矩阵的定义，如果不理解雅克比矩阵是怎么求出来的，代码就不能看懂。只当成黑盒子调用的话，理解不够深入。这个雅克比矩阵在后面的光度误差里面还会重复出现，因此我总结了一下：SLAM14讲学习笔记（五）视觉里程计（光流法和直接法）和对于雅克比矩阵的理解，看到这里的人一定要看一下这节！ 如果是对P求偏导，第一项也是e先对P’求偏导，第二项P‘对P求偏导结果就是个旋转矩阵R，因此这部分的导数矩阵就是第一项和R相乘。综上，都是加一个小量扰动，然后通过求解对于小量的偏导，从而解决不能直接求偏导的问题。详情请看我的非线性优化这节的笔记：SLAM14讲学习笔记（三）非线性优化基础

3D-3D

这部分内容也是两种方式：代数方法和非线性优化方法。代数方法是SVD方法，非线性优化还是类似于BA方式。

SVD方式：

SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结这部分内容只看上面的步骤，觉得有些抽象。其实定义的误差比较简单，两个3D点，坐标是P1和P2，P2’根据R*P1+t恢复得到，P2和P2‘应当是同一个点，定义二者之间的误差为e，构建误差平方和。误差函数是经过特殊技巧处理的：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结自己加了又减去一些东西，最后化简得到的东西，等式右边三项，第一项只有R优化，优化它最小，就能得到R，代入得到第二项的t，第三项根据质心的定义为0（质心就是所有位置求和的均值），这样也就是上面的三个步骤了！那么用代数方法优化
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结该咋优化？就是SVD方式。书里这直接给出了步骤，把它拆开，得到三项：
前两项与R无关，因为第二项R和自身转置乘积为1（R的固有性质），就是优化第三项。第三项这么变：
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结之后定义了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步骤）针对这两点问题，我特意又去看了高博的网课。第一个问题，高博是这么说的：矩阵论里有，反正有这么个性质。（好吧）第二个问题，他说“感兴趣的自己去看paper，反正步骤就是这么个步骤” （当然我一点也不感兴趣，我很不喜欢数学推导。他没有解释为什么没看到对目标函数优化的那个min步骤，我的理解是这样的：令目标函数为0，优化出来的结果，恰好就是这个R=UVT的形式）注：最近学激光SLAM，又用到了ICP，因此对这里有了更深的理解。首先，不优化是因为这里用的是SVD方法，不是非线性优化的迭代步骤。至于为什么要这么求解（对W进行SVD分解），因为这里是想让RW的迹最大，从而-tr（RW)最小，然后总的和最小，最接近0。
SLAM14讲学习笔记（四）视觉里程计（特征点法）和重难点总结注意Track这里写错了，应该是Trace。意思是要构造一个正定对称矩阵，如果让R取这个VUT，那么可以使得RW的迹最大（取别的都没这个大）

非线性优化方法：

这块就是老生常谈了，对误差函数进行李代数扰动，迭代更新，最后找到合适的位姿。参见我在非线性优化那节的笔记。值得注意的有两点： 1.有人已经证明，用ICP求解问题有唯一解或无穷多解情况。如果是唯一解的情况，那么极小值就是全局最优值。因此用ICP求解有一大好处：可以任意选定初始值。 2.可以混合使用PnP和ICP，深度已知就用ICP算法。