经典SLAM学习

    一、追踪

1. ORB特征提取
2. 初始姿态估计（速度估计）
3. 姿态优化（Track local map，利用邻近的地图点寻找更多的特征匹配，优化姿态）
4. 选取关键帧

1. 加入关键帧（更新各种图）
2. 验证最近加入的地图点（去除Outlier）
3. 生成新的地图点（三角法）
4. 局部Bundle adjustment（该关键帧和邻近关键帧，去 除Outlier）
5. 验证关键帧（去除重复帧）

1. 选取相似帧（bag of words）
2. 检测闭环（计算相似变换（3D<->3D，存在尺度漂移，因此是相似变换），RANSAC计算内点数）
3. 融合三维点，更新各种图
4. 图优化（传导变换矩阵），更新地图所有点

1.选取足够多的特征点（这里当然是ORB特征了）（如果没有则重置），归一化所有特征点，分别同时（并行）计算特征点的H阵和F阵，计算每个点对的symmetric transfer errors,和卡方分布的对应值比较，由此判定该点是否为内点。累计内点的总得分。（这里的SM既可以是SH也可以是SF，TM也是如此。分别是从当前帧到参考帧的变换误差和参考帧到当前帧的变换误差）

2.然后就是模型的选择了，根据公式如果大于0.4选H，反之选F

3.根据模型恢复运动（即求R和T）

4.进行了全局BA。ORB中对初始化要求高，所以要有足够的视差。另外，由于坐标系会附着在初始化成功的那帧图像的位置，因此每次初始化不能保证在同一个位置。

疑问：全局BA是优化里面对所有关键帧（除了第一帧）和所有mappoint的全局优化？

        首先 SVO是混合使用了特征点法和直接法：它跟踪了一些关键点（角点，没有描述子，由FAST实现），然后像直接法那样，根据这些关键点周围的信息，估计相机运动以及它们的位置 。（不需要知道点与点的对应关系p1,p2，根据当前的位姿估计值 来寻找p2的位置，通过优化两点的光度误差来求得位姿的转化关系）

         它假设前两个关键帧所拍到的特征点在一个平面上(四轴飞行棋对地面进行拍摄)，然后估计单应性H矩阵，并通过三角化来估计初始特征点的深度值。从而获得第一帧的特征点位置及它们的深度。

       SVO适用于摄像头垂直向下的情况（也就是无人机上,垂直向上也可以,朝着一面墙也可以），为什么呢？1.初始化的时候假设的是平面模型 2.KF的选择是个极大的限制（关键帧选取靠的 是 关于景深的一个阈值，当前帧和相邻关键帧之间任意轴向运动（x方向,y方向,z方向）超过场景平均深度的一个百分比，就会选为关键帧，和图像无关，和相机运动距离有关。），除了KF的选择原因外摄像头水平朝前运动的时候，SVO中的深度滤波做的不好。

T

r

a

c

k

track主要做两件事：①确定每帧的位姿

                                ②确定关键帧提供给Local Mapping

具体模块：

（1）将初始化的数据封装成帧，后续针对帧处理

         ①Feature对特征封装（keypoint+descriptor）

         ②Feature是Frame与MapPoint之间的纽带

         ③MapPoint对特征对应的3D点的封装

         ④MapPoint对应最好的特征描述子

（2）帧间跟踪：Track Reference Key Frame,

                    Track With Motion Model,

              因为一开始还没有建立运动模型，所以根据词袋向量进行匹配，来确定 参考帧的MapPoin跟当前帧的特征点之间的3D-2D关系，其中的3D点通过三角化得来的。后面就可以根据上一帧的运动模型估计当前帧位姿，将上一帧MapPoint投影到当前帧，寻找匹配关系。

               另当运动模型匹到的特征点较少时也采用关键帧模式（词袋向量）

 （3）重定位

         当两种模式都失败了，只能重定位来继续跟踪，此时

            a.计算当前帧的BOW向量，在关键帧词典数据库中选取若干关键帧作为候选。

           b.通过BOW匹配，寻找有足够多的特征点匹配的关键帧（匹配特征数超过15个）。

           c.利用RANSAC迭代，然后使用PnP算法求解当前帧的相机外参。

           d.优化帧，如果匹配特征数没有50个，则将候选关键帧对应的MapPoint投影到当前帧继续寻找匹配，匹配完成之后再次优化帧，如果匹配数还是没有大于50，则更改投影匹配阈值，再次匹配，只有匹配的个数大于50，才说明重定位成功，最后对大于50个匹配对的帧再次对帧进行优化

（4）Track Local Map---跟踪局部地图（姿态优化：主要思路是在当前帧和（局部）地图之间寻找尽可能多的对应关系，来优化当前帧的位姿）（2中 无论哪种跟踪方式，跟踪成功后就要进行一个局部地图跟踪）

          局部地图就是局部keyframe（当前帧拥有共同mappoint的关键帧）和局部mappoint

（点为关键帧，只要关键帧之间的共同mappoint数大于10就用绿线链接起来）

当前帧的局部关键帧是这些至少有10个共同mappoint的关键帧，将这些mappoint投影到当前帧，找一个3D点与2D特征点的对应关系，

 论文中还提到了尺度因子d/dmin，并将地图点的特征描述子D与还未匹配上的ORB特征进行比较，根据尺度因子，找到最佳匹配。

（5）确定关键帧

 追踪过程除了以上计算位姿，还用来确定关键帧，

本环节疑问：局部地图中选取的局部mappoint已经在上面追踪过程中选取了吧，这一步不会重复吗？还是说选取的点更少，更精确，所以优化效果更好吗？

还有就是如何根据尺度因子找到最佳匹配？

1.SVO中 sparse model-based image alignment。是帧与帧之间的约束关系。

有了第一帧的特征点位置及深度，第一、二帧之间的位姿转换T是求解H的来得到（ 假设一开始的位姿为单位阵），第二帧以后采用直接法，知道上一帧的位姿和观测值，求Tk,k-1。

具体为：知道中的某个特征在图像平面的位置(u,v)，以及它的深度d， 也知道此特征的光度值（用来对比），能够将该特征投影到三维空间该三维空间的坐标系是定义在摄像机坐标系的，将这个3D点使用Tk,k-1（这里的为假设的上一相邻帧之间的位姿转换外参？）投到当前帧的坐标系上，然后通过相机内参投到当前帧的图像上，存在导致光度差，而根据灰度不变假设，通过不断的优化位姿使得光度差最小。Ik−1

     优化过程就是高斯牛顿了，值得注意的是：SVO实现了自己的高斯牛顿方法---迭代下降，使用了反向求导：即雅克比在K-1帧进行估计，而不是在K帧进行估计。好处是在迭代过程中只需计算一次 雅克比。而且能保证K-1帧的像素具有梯度（？），但没法保证K帧上的像素具有梯度，所以这样做至少可以保证像素点的梯度是明显的。

     实现当中另一个需要注意的地方是金字塔的处理。这一步估计是从金字塔的顶层开始，把上一层的结果作为下一层估计的初始值，最后迭代到底层的。顶层的分辨率最小，所以这是一个由粗到精的过程（Coarse-to-Fine），使得在运动较大时也能有较好的结果。（好吧，金字塔一直不怎么理解）

2.Relaxation Through Feature Alignment（帧与地图之间的约束关系---更精确）

       上一步中，当前帧位姿是基于上一帧的结果得来的，使用多了，会造成累计误差从而导致漂移，这一步就是构建地图模型来进一步约束当前帧的位姿。

       地图模型是保存3D点的，因为每一个Key frame通过深度估计能够得到特征点的三维坐标，这些三维坐标点通过特征点在Key Frame中进行保存。所以SVO地图上保存的是Key Frame 以及还未插入地图的KF中的已经收敛的3d点坐标（这些3d点坐标是在世界坐标系下的，可以投影到任意帧上），也就是说地图map不需要自己管理所有的3d点，它只需要管理KF就行了。

       KF被检测出来，插入地图中，便在新的KF上检测新的特征点作为深度估计的seed（种子），这些seed不断和新帧进行深度估计，但是，如果有些seed点3d点位姿通过深度估计已经收敛了（就是不确定性小于一定的阈值），怎么办？map用一个point_candidates来保存这些尚未插入地图中的点。所以map这个数据结构中保存了两样东西，以前的KF以及新的尚未插入地图的KF中已经收敛的3d点

        地图点记录了哪些帧上的特征点能够观测到它，比如，通过能够找到地图点P2，反过来，地图点P2也知道能够观测到它。

        地图里的map point众多，我们只需要把临近关键帧(overlap_kfs)观测到的point点做投影，如P1,P2，当然还有那些没插入kf上的point_candidates_也要往cur_frame上投。投的时候先利用关键帧和cur_frame的距离排序，最近的关键帧上的先投影，找到关键帧后，遍历关键帧上的特征点，通过特征点找到地图点，然后一一投影到当前帧。注意，一个地图point点只需要投一次。

         要注意，一个地图点可能连着多个特征点，比如P2P2到底选择ref_frame i中Pi2还是ref_frame j中的Pj2作为和cur_frame中匹配的特征点呢？地图中的point点通过变量obs_记录了它和哪些特征是联系起来的，这里它知道自己连着，地图point点会通过一些筛选机制来选择是用Pi2还是Pj2，具体实现在getCloseViewObs函数中

       在new frame中灰色的特征块为真实位置，蓝色特征块为预测位置（直接法求得的位置）。幸好，他们偏差不大，基于光度不变性假设，特征块在以前参考帧中的亮度应该和new frame中的亮度差不多。所以可以重新构造一个残差，对特征预测位置进行优化：

       注意这里的优化变量是像素位置，这过程就是光流法跟踪嘛。并且注意，光度误差的前一部分是当前图像中的亮度值，后一部分不是而是，即它是根据投影的3d点追溯到的这个3d点所在的key frame中的像素值，而不是相邻帧。由于是特征块对比并且3d点所在的KF可能离当前帧new frame比较远，所以光度误差和前面不一样的是还加了一个仿射变换，需要对KF帧中的特征块进行旋转拉伸之类仿射变换后才能和当前帧的特征块对比。

        这时候的迭代量计算方程和之前是一样的，只不过雅克比矩阵变了，这里的雅克比矩阵很好计算：

这不就是图像横纵两个方向的梯度嘛。

        通过这一步我们能够得到优化后的特征点预测位置，它比之前通过相机位姿预测的位置更准，所以反过来，我们利用这个优化后的特征位置，能够进一步去优化相机位姿以及特征点的三维坐标。所以位姿估计的最后一步就是Pose and Structure Refinement。 

优化

优化部分其实和track中的track local map有重复地方,首先要添加上面得到的关键帧,local mapping的线程主要确定更多的约束对关键帧的位姿和map point点的位置进行修正.

1.添加关键帧

每添加一个keyframe，更新Covisibility Graph，Spanning Tree，Map以及计算该帧的词袋表示确定匹配，为三角化做准备

2.新mappoint的创建

当前关键帧(cur_keyframe)与在covisibility graph中相邻的关键帧组（共同的map point数单目设置20）之间未匹配的特征点通过BOW得到大致匹配，然后根据极限约束得到匹配特征点对，然后利用三角化得到新的mappoint，然后检测其阈值和尺度（条件），

确定Map Point的相关属性（平均观察方向，观测距离，最佳描述子等）

3.对于新增加的mappoint在其他关键帧可能也会检测到，或者说已经存在的等情况，即选出最佳的mappoint点进行三角化，具体做法：

4.把相连的关键帧上的mappoint点投影到当前帧上来，（3是将当前帧的mappoint点投到每个邻接关键帧上，4是将所有邻接关键帧上的mappoint点投到当前帧上找匹配）总的来说也就是把没有匹配上的约束关系加上                                

5.对当前关键对应的mappoint进行修改，同样，重新更新Covisibility Graph，Spanning Tree

6.有了这些新的约束，就可以进行局部的BA。

尺度不一样的话，他的大小会不一样。（尺度与特征点到mappoint的一个距离有关系，具体待学习）

（这里的POS1就是关键帧但不是局部关键帧） （局部关键帧的图节点相当于pose点，局部mappoint的图节点相当于路标点，他们都是优化变量，优化的目标函数是mappoint所有观测的投影误差）

7.mappoint的剔除

 上面提到每次新加入的mappoint都会检测

（这里的mappoint的剔除全包括优化后的）

8.关键帧的剔除

主要是剔除冗余关键帧。

疑问：新建的mappoint都是在Covisibility Graph中局部关键帧建立的？

3.Pose and Structure Refinement

        在一开始的直接法匹配中，我们是使用的光度误差，这里由于优化后的特征位置和之前预测的特征位置存在差异，这个能用来构造新的优化目标函数

         上式中误差变成了像素重投影以后位置的差异（不是像素值的差异），优化变量还是相机位姿，雅克比矩阵大小为2×6(横纵坐标u，v分别对六个李代数变量求导)。这一步是就叫做motion-only Bundler Adjustment。同时根据根据这个误差定义，我们还能够对获取的三维点的坐标(x,y,z)进行优化，还是上面的误差像素位置误差形式，只不过优化变量变成三维点的坐标，这一步叫Structure -only Bundler Adjustment，优化过程中雅克比矩阵大小为2×3(横纵坐标u，v分别对点坐标(x,y,z)变量求导)（十四讲P195）

Mapping部分

Mapping部分主要是计算特征点的深度。

我们知道通过两帧图像的匹配点就可以计算出这一点的深度值，如果有多幅图像，那就能计算出这一点的多个深度值。这就像对同一个状态变量我们进行了多次测量，因此，可以用贝叶斯估计来对多个测量值进行融合，使得估计的不确定性缩小。

        一开始深度估计的不确定性较大(浅绿色部分)，通过三角化得到一个深度估计值以后，能够极大的缩小这个不确定性(墨绿色部分)。

         在这里，先简单介绍下svo中的三角化计算深度的过程，主要是极线搜索确定匹配点。在参考帧IrIr中，我们知道了一个特征的图像位置，假设它的深度值在[dmin,dmax][dmin,dmax]之间，那么根据这两个端点深度值，我们能够计算出他们在当前帧IkIk中的位置，如上图中草绿色圆圈中的线段。确定了特征出现的极线段位置，就可以进行特征搜索匹配了。

          如果极线段很短，小于两个像素，那直接使用上面求位姿时提到的Feature Alignment光流法就可以比较准确地预测特征位置。如果极线段很长，那分两步走，第一步在极线段上间隔采样，对采样的多个特征块一一和参考帧中的特征块匹配，用Zero mean Sum of Squared Differences 方法对各采样特征块评分，那个得分最高明他和参考帧中的特征块最匹配。第二步就是在这个得分最高点附近使用Feature Alignment得到次像素精度的特征点位置。像素点位置确定了，就可以三角化计算深度了。

      得到一个新的深度估计值以后，用贝叶斯概率模型对深度值更新。在LSD slam中，假设深度估计值服从高斯分布，用卡尔曼滤波(贝叶斯的一种)来更新深度值。（十四讲P326）这种假设中，他认为深度估计值效果很棒，很大的概率出现在真实值(高斯分布均值)附近。而SVO的作者采用的是Vogiatzis的论文《Video-based, real-time multi-view stereo》提到的概率模型。

    这个概率模型是一个高斯分布加上一个设定在最小深度dmindmin和最大深度dmaxdmax之间的均匀分布。这个均匀分布的意义是假设会有一定的概率出现错误的深度估计值。

由于累计的误差，尺度会不再统一，导致尺度漂移

1.检测回环

（假设pose2和pose6为回环，则检测过程中pose2为候选闭环帧，要pose678都对应pose2则pose2为闭环帧）

2.计算相似变换（pose2与posse678的匹配计算）

3.闭环融合

（当前帧由于累计误差，得到的mappoint可能不是很准确，所以用闭环帧的mappoint，同时将当前帧的相邻帧与闭环帧的相邻帧对应mappoint）（当前帧可能不与闭环帧闭环，所以要重新构建covisibility graph）（covisibility graph中的约束关系太多，所以对Essential graph优化）。

4.优化