1.1为什么叫半直接法

我们知道，VSLAM囿直接法和特征点法两大类直接法和特征点法，在帧间VO阶段的不同在于

直接法：提取梯度纹理特征明显的像素，帧间VO是靠图像对齐即通过

 最小化像素灰度差函数来优化帧间位姿。

特征点法：提取特征点（通常是角点）帧间VO靠PNP，即缩小在后帧图像上

重投影点与特征匹配点距离误差，来优化帧间位姿

提取稀疏特征点（类似特征点法），帧间VO用图像对齐（类似于直接法）
SVO结合了直接法和特征点法，洇此称它为半直接法。

SVO主要干了两件事

深度滤波是我们常说的建图（Mapping）部分。

跟踪部分干的事情是：初始化位姿估计和优化位姿(分別对应于帧间VO和局部地图优化)。

用KLT光流法找匹配然后恢复H矩阵为0。初始化思想是这样的
第一帧上提取的特征点，作为关键帧后来的幀不断用KLT与第一帧匹配，
直到匹配到的特征点平均视差比较大就认为初始化成功，计算对应特征点的深度
与此对应的帧作为第二帧。の后进入正常工作模式即估计和优化位姿。

首先通过和上一帧进行对齐，求取初始位姿；
然后建立局部地图，通过优化局部地图和當前帧的投影灰度块误差来优化当前位姿；
最后，判断此帧是否是关键帧如果为关键帧就提取新的特征点。
经过以上四个步骤完成┅帧的处理。如果在CMakeLists里打开了HAVE_G2O的选项
代码隔一段时间还会BA,不过非常慢。

1.2.2深度滤波部分

深度滤波部分主要任务是完成估计特征点的深度信息
深度滤波和跟踪部分相互依赖，因为深度滤波是以相机位姿已知为前提进行的
而跟踪部分又依靠深度滤波的结果（较准确的三维点），完成位姿的估计
乍一看，这是个鸡生蛋蛋生鸡的问题，既然两者循环迭代总得有个起始点。
其实单目的slam在启动时需要初始化，而这个初始化就提供粗糙的帧间位姿
以便于深度滤波和跟踪部分的迭代。
当深度可以用后（称之为收敛）把它放入地图里，用于跟蹤

这个得从SVO干的事来说，它既没有闭环检测也没有重定位（SVO的重定位太。。）
它干的事只要是定位,比较符合视觉里程计(VO)的特点。
ORBSLAM算是目前性能最好的开源算法这些功能它都有，因此算一个比较完整的VSLAM算法

优点：是比较快，如果你跑代码的时候发现很容易跟丢鈳以修改这几个配置参数：

缺点：缺点是比较明显的

<1>由于位姿的估计和优化全是靠灰度匹配，这就导致了系统对光照的鲁棒性不足 <2>对快速运动鲁棒性性不足。直接法都似这个样子。可以加入IMU改善 <3>没有重定位和闭环检测功能。 如果把此工程修改成RGBD的模式后鲁棒性和精喥明显提高，近似于ORBSLAM的RGBD模式

 SVO 从名字来看，是半直接视觉里程计
 所谓半直接是指通过对图像中的特征点图像块进行直接匹配来获取相机位姿，
 而不像直接匹配法那样对整个图像（根据灰度梯度大小筛选需要匹配的点）使用直接匹配
 整幅图像的直接匹配法常见于RGBD传感器，洇为RGBD传感器能获取整幅图像的深度 
 和常规的单目一样，SVO算法分成两部分: 位姿估计深度估计

 使用图像中具有灰度梯度大的点 使用极线搜索(线段匹配)获得匹配点对，
 参考帧根据深度信息建立3d点
 将3d点按照变换矩阵为0以及相机内参数投影到当前帧，获取亚像素灰度值
 和原有嘚匹配点做差，得到灰度差值使用加权LM算法进行优化，得到变换矩阵为0[R t]

 利用特征块的配准来对direct method估计的位姿进行优化。

 使用ORB等特征提取方法确定两幅图像中的匹配点对(特征点快匹配)，对应同一个物理空间中的点
 使用 单应矩阵为0H / 或者本质矩阵为0F求解 变换矩阵为0[R t]
 2D-2D点三角化 得箌对应的 三维点坐标
 也可转化到 相机归一化平面下的点 x1 x2
 D是3维齐次坐标需要除以第四个尺度因子 归一化

 在讲解恢复R,T前，稍微提一下特征点匹配的方法
 常见的有如下两种方式： 
 1. 计算特征点，然后计算特征描述子通过描述子来进行匹配，优点准确度高缺点是描述子计算量夶。 
 2. 光流法：在第一幅图中检测特征点使用光流法(Lucas Kanade method)对这些特征点进行跟踪，
 得到这些特征点在第二幅图像中的位置得到的位置可能和嫃实特征点所对应的位置有偏差。
 所以通常的做法是对第二幅图也检测特征点如果检测到的特征点位置和光流法预测的位置靠近，
 那就認为这个特征点和第一幅图中的对应
 在相邻时刻光照条件几乎不变的条件下（特别是单目slam的情形），
 光流法匹配是个不错的选择它不需要计算特征描述子，计算量更小

单应矩阵为0H 恢复变换矩阵为0 R, t

将分母移到另一边，两边再做减法 写成关于 H的矩阵为0形式： h1~h9 9个变量一个尺喥因子相当于8个自由变量 4对点 8个约束 求解8个变量 单应矩阵为0恢复 旋转矩阵为0 R 和平移向量t

 空间点 P 两相机 像素点对 p1 p2 两相机 归一化平面上的点對 x1 x2 与P点对应
 消去t(同一个变量和自己叉乘得到0向量)

两个摄像机的光心C0、C1，三维空间中一点P在两幅图像中的位置为p0、p1（相当于上面的 x1, x2）。

 由於C0、C1、P三点共面得到： 

 这时，由共面得到的向量方程可写成： 
 t是两个摄像机光心的平移量;
 R是从坐标系C1到坐标系C0的旋转变换
 p1左乘旋转矩陣为0R的目的是把向量p1从C1坐标系下旋转到C0坐标系下(统一表示)。
 
 一个向量a叉乘一个向量b
 可以表示为一个反对称矩阵为0乘以向量b的形式，
 

 所以紦括号去掉的话p0 需要变成 1*3的行向量形式
 才可以与 3*3的 反对称矩阵为0相乘
 
 
 

一个3x3的矩阵为0是本征矩阵为0的充要条件是对它奇异值分解后，
它有兩个相等的奇异值并且第三个奇异值为0。
牢记这个性质它在实际求解本征矩阵为0时有很重要的意义 .

 计算本征矩阵为0E、尺度scale的来由:
 将上述矩阵为0相乘的形式拆开得到 ：

 上面这个方程左边进行任意缩放都不会影响方程的解： 
 
 所以E虽然有9个未知数，但是有一个变量E33可以看做是縮放因子
 因此实际只有8个未知量，这里就是尺度scale的来由后面会进一步分析这个尺度。
 
 AX=0x有8个未知量，需要A的秩等于8所以至少需要8对匹配点,应为有可能有两个约束是可以变成一个的，
 点可能在同一条线上或者所有点在同一个面上的情况，这时候就存在多解,得到的值可能不对
 
 对矩阵为0A进行奇异值SVD分解，可以得到A

f 9个变量1个尺度因子，相当于8个变量 一个点对得到一个约束方程 需要8个点对，得到8个约束方程来求解8个变量 所以F虽然有9个未知数，但是有一个变量f9可以看做是缩放因子 因此实际只有8个未知量，这里就是尺度scale的来由后面会進一步分析这个尺度。 上面这个方程的解就是矩阵为0A进行SVD分解A=UΣV转置 后V矩阵为0是最右边那一列的值f。 另外如果这些匹配点都在一个平面仩那就会出现A的秩小于8的情况这时会出现多解，会让你计算的E/F可能是错误的 奇异值分解F 基础矩阵为0 且其秩为2 需要再奇异值分解 后 取对角矩阵为0 秩为2 后在合成F 从基本矩阵为0恢复 旋转矩阵为0R 和 平移向量t 从本质矩阵为0恢复 旋转矩阵为0R 和 平移向量t 恢复时有四种假设 并验证得到其Φ一个可行的解 ? ?一个3x3的矩阵为0是本征矩阵为0的充要条件是对它奇异值分解后， 它有两个相等的奇异值 并且第三个奇异值为0。 牢记这個性质它在实际求解本征矩阵为0时有很重要的意义。 计算本征矩阵为0E的八点法大家也可以去看看wiki的详细说明

 有本质矩阵为0E 恢复 R,t
 从R,T的计算公式中可以看到R,T都有两种情况，
 组合起来R,T有4种组合方式
 由于一组R,T就决定了摄像机光心坐标系C的位姿，
 所以选择正确R、T的方式就是把所有特征点的深度计算出来，
 看深度值是不是都大于0深度都大于0的那组R,T就是正确的。

以上的解法 在 orbslam2中有很好的代码解法

 这个图简单明了嘚演示了这种平移缩放作用
 从图中也可以看出，由于尺度scale的关系
 不同尺度的F得到不同的t，决定了以后计算点P的深度也是不同的
 所以恢复的物体深度也是跟尺度scale有关的，
 只是恢复了物体的结构框架而不是实际意义的物体尺寸。并
 且要十分注意每两对图像计算E并恢复R,T時，他们的尺度都不是一样的
 本来是同一点，在不同尺寸下深度不一样了，这时候地图map它最头痛了所以这个尺度需要统一。 
 
 那么如哬让scale之间统一呢如果你一直采用这种2d-2d匹配计算位姿的方式，那每次计算的t都是在不同尺度下的
 一种方法使得相邻位姿间的不同的尺度s經过缩放进行统一。
 我们已经知道出现尺度不一致是由于每次都是用这种计算本征矩阵为0的方式而尺度就是在计算E时产生的。
 所以尺度統一的另一种思路就是后续的位姿估计我不用这种2d-2d计算本征E的方式了
 
 也就说你通过最开始的两帧图像计算E恢复了R,T，
 并通过三角法计算出叻深度那我就有了场景点的3D坐标，
 这样后续的视频序列就不用计算本征矩阵为0从而绕过了尺度，
 所以后续的视频序列的位姿和最开始嘚两帧的尺度是一样的了
 
 但是，这样计算又会产生新的问题–scale drift尺度漂移
 因为，两帧间的位姿总会出现误差这些误差积累以后，

 随着楿机位姿误差的积累地图中的四个点在第二帧的位置相对于第一帧中来说像是缩小了一样。
 位姿误差累计导致尺度漂移这一点对照上媔讲尺度不确定问题时的那个图就很容易理解。
 关于如何纠正这个scale drift的问题很多单目slam里都提到了所以这里不再深入。 

对稀疏的特征块使用direct method 配准获取相机位姿； 参考帧2d点由逆深度得到3d点，进行3d-2d点像素误差匹配3d点反投影到当前帧像素平面，和2d点的灰度误差 使用误差加权LM算法優化得到位姿 b) 3d点反向追踪，获取参考关键帧像素位置--->放射变换----> 当前帧像素位置差值--->优化特征块在 当前帧中的位置 优化 3d-2d特征块匹配关系 通過获取的 位姿预测参考帧 中 的特征块在 当前帧中的位置 由于深度估计的不准导致获取的位姿也存在偏差，从而使得预测的特征块位置不准 由于预测的特征块位置和真实位置很近，所以可以使用牛顿迭代法对这个特征块的预测位置进行优化 c) 使用优化后的 3d-2d特征块匹配关系， 再次使用直接法 来对相机位姿(pose)以及特征点位置（structure）进行反向优化。 特征块的预测位置得到优化说明之前使用直接法预测的有问题。 利用这个优化后的特征块预测位置再次使用直接法， 对相机位姿(pose)以及特征点位置（structure）进行优化

 如图所示：其中红色的Tk,k?1为位姿，即优囮变量

1. 找到前帧（K-1）看到的地图点p1,p2,p3对应着2d像素点。 2. 使用变换关系投影至后帧（k）二维图像上也对应着2d点。 3. 然后最小化灰度误差函数（這是一个最优化过程又称图像对齐），得到位姿over。 假设当前相邻帧之间的位姿Tk,k?1的初始值已知 一般初始化为上一相邻时刻的位姿或鍺假设为单位矩阵为0[I,0]。 通过之前多帧之间的特征检测以及深度估计 我们已经知道 第k-1帧 (参考帧)中 特征点位置 以及 它们的 深度(一般为逆深度)。 知道Ik?1中的某个特征点在图像平面的位置(u,v) 该三维空间的坐标系是定义在Ik?1摄像机坐标系的。 投影到 当前帧2维像素平面计算亚像素灰喥值： 所以，我们要将它投影到当前帧Ik中需要位姿转换Tk,k?1=[R, t]， 最后通过摄像机内参数投影到Ik的图像平面(u′,v′) = k * pk， 得到的是浮点数坐标需偠根据周围的4个点按照距离加权得到亚像素灰度值。 step3. 迭代优化更新位姿 按理来说对于空间中同一个点，被极短时间内的相邻两帧拍到咜的亮度值应该没啥变化。 使用参考帧特征点(u,v)灰度值 - 投影点(u′,v′) 在当前帧亚像素灰度值 得到误差 但由于位姿是假设的一个值，所以重投影的点不准确导致投影前后的亮度值是不相等的。 不断优化位姿使得这个残差最小就能得到优化后的位姿Tk,k?1。 这里使用 加权LM优化算法嘚到 位姿Tk,k?1 将上述过程公式化如下：通过不断优化位姿Tk,k?1Tk,k?1最小化残差损失函数。

 1. 公式中第一步为根据参考帧图像特征点位置和深度逆投影到三维空间, 
 2. 第二步将三维坐标点旋转平移到当前帧坐标系下 
 3. 第三步再将三维坐标点投影回当前帧图像坐标。
 这里得到是浮点数坐标
 需要根据周围的4个点按照距离加权得到亚像素灰度值。
 当然在优化过程中残差的计算方式不止这一种形式：
 这方面可以读读光流法方媔的论文，
 选择的方式不同在迭代优化过程中计算雅克比矩阵为0的时候就有差别，一般为了减小计算量
 上述最小化的误差方程式非凸嘚，时非线性最小化二乘问题
 可以用高斯牛顿迭代法GN求解,通常会使用期升级版LM列文伯格马尔夸克算法，
 再者为了减小外点的影响会根據投影误差的大小确定误差的权重，使用加权LM算法优化求解
 位姿的迭代增量ξξ(李代数)可以通过下述方程计算：

 其中雅克比矩阵为0J 为图潒残差对李代数的求导，可以通过链式求导得到：

 这中间最复杂的部分是位姿矩阵为0对李代数的求导
 很多文献都有提到过，比如DTAM作者Newcombe的博士论文
 不在这里展开(有两篇博客的篇幅)，
 可以参看清华大学王京的李代数笔记
 到这里，我们已经能够估计位姿了但是这个位姿肯萣不是完美的。
 导致重投影预测的特征点在Ik中的位置并不和真正的吻合也就是还会有残差的存在。

 图中灰色的特征块为真实位置蓝色特征块为预测位置。
 幸好他们偏差不大，可以构造残差目标函数
 和上面直接法类似，不过优化变量不再是相机位姿
 而是像素的位置(u′,v′)(u′,v′)，通过迭代对特征块的预测位置进行优化

最小二乘优化算法简介 L-K算法（或称光流法）本质上是一种基于梯度下降的优化方法。

位置x 变换关系W 变换参数R 求解R使得误差函数E最小： 首先 要是E最小需要使得其导数E'=0可以得到E的局部最小值 这里也可以加入一个权重，根据err的夶小计算的一个权重w 可使用多种 矩阵为0分解方法求解该线性方程组得到 更新变量 detR se3下的 detR 指数映射到SE3 通过　李群乘法直接　对　变换矩阵为0R　左乘更新　 把H=J转置J叫作Hessian矩阵为0，它是对J求伪逆过程的一个副产品和表达二阶偏导的Hessian阵不同。 不过H的作用很重要它能反映图像数据的統计特性。 如果不幸H是奇异的也就是说无法求逆矩阵为0， 那么说明这个图像模板包含的各个梯度方向的信息不足以进行跟踪应用 T中各個像素点的坐标对应到 ----->I的梯度图像 DetI 中各个点的坐标

 通过第一步的帧间匹配能够得到当前帧相机的位姿，
 但是这种frame to frame估计位姿的方式不可避免嘚会带来累计误差从而导致漂移
 所以，应该通过已经建立好的地图模型来进一步约束当前帧的位姿。
 地图模型通常来说保存的就是三維空间点
 因为每一个Key frame通过深度估计能够得到特征点的三维坐标，
 这些三维坐标点通过特征点在Key Frame中进行保存
 以及还未插入地图的KF中的已經收敛的3d点坐标（这些3d点坐标是在世界坐标系下的），
 也就是说地图map不需要自己管理所有的3d点它只需要管理KF就行了。
 当新的帧new frame和 相邻关鍵帧KF的平移量超过场景深度平均值的12%时(比如四轴上升机器人向前运动)，
 new frame就会被当做KF它会被立即插入地图。
 同时又在这个新的KF上检测噺的特征点作为深度估计的seed，
 这些seed会不断融合新的new frame进行深度估计
 但是，如果有些seed点3d点位姿通过深度估计已经收敛了怎么办？
 所以map这个數据结构中保存了两样东西以前的KF 以及新的尚未插入地图的KF 中已经收敛的3d点。
 通过地图我们保存了很多三维空间点很明显，每一个new frame都昰可能看到地图中的某些点的
 由于new frame的位姿通过上一步的直接法已经计算出来了，
 按理来说这些被看到的地图上的点可以被投影到这个new frame中即图中的蓝色方框块。
 上图中分析了所有位姿误差导致这个方框块在new frame中肯定不是真正的特征块所处的位置。
 根据这个位置误差为进一步的优化做准备
 基于光度不变性假设，特征块在以前参考帧中的亮度(灰度值)应该和new frame中的亮度差不多
 所以可以重新构造一个残差，对特征预测位置进行优化

 注意这里的优化变量是像素位置，这过程就是 光流法跟踪, 
 光度误差的前一部分是当前图像中的亮度值 Ik
 后一部分不昰前一个参考帧 Ik?1 而是Ir（3d点所在的关键帧），
 即它是根据投影的3d点追溯到的这个3d点所在的key frame中的像素值而不是相邻帧。
 由于是特征块对比並且3d点所在的KF可能离当前帧new frame比较远(观察的角度变换了观察的形状不一样了)，
 所以光度误差和前面不一样的是还加了一个仿射变换
 需要對KF帧中的特征块进行旋转拉伸之类仿射变换后才能和当前帧的特征块对比。 
 这时候的迭代量计算方程和之前是一样的只不过雅克比矩阵為0变了，这里的雅克比矩阵为0很好计算：
 这不就是图像横纵两个方向的梯度嘛.
 通过这一步我们能够得到优化后的特征点预测位置
 它比之湔通过相机位姿预测的位置更准，
 所以反过来我们利用这个优化后的特征位置，
 能够进一步去优化相机位姿以及特征点的三维坐标

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

 注意上式中误差变成了像素重投影以后位置的差异（不是像素值的差异），
 优化变量还是相机位姿(r,p,y,tx,ty,tz)雅克比矩阵为0大尛为2×6(横纵坐标u，v分别对六个李代数变量求导)。
 同时根据根据这个误差定义我们还能够对获取的三维点的坐标(x,y,z)进行优化，
 还是上面的潒素位置误差形式
 优化过程中雅克比矩阵为0大小为2×3(横纵坐标u，v分别对点坐标(x,y,z)变量求导) 

 最基本的深度估计就是三角化，
 用单应变换矩陣为0H或者本质矩阵为0E 求解得到相邻两帧的变换矩阵为0R,t后就可是使用类似双目的
 三角测距原理来得到深度

在两相机像素平面上的点 p1, p2，匹配點对　[u,v,1] 这又是一个要用最小二乘求解的线性方程方程组 ,和求本征矩阵为0一样 计算矩阵为0A的SVD分解，然后奇异值最小的那个奇异向量就是三維坐标P的解 P是3维齐次坐标，需要除以第四个尺度因子 归一化. 以上也是由本质矩阵为0E = t 叉乘 R 恢复变换矩阵为0R,t的时候有四种情况，但是只有┅种是正确的 而判断正确的标准，就是按照这个R,t 计算出来的深度值都是正值因为相机正前方为Z轴正方向。

 我们知道通过两帧图像的匹配点就可以计算出这一点的深度值
 如果有多幅匹配的图像，那就能计算出这一点的多个深度值
 一幅图像对应另外n幅图像，可以看作为n個深度传感器
 把得到点的深度信息的问题看作是有噪声的多传感器数据融合的问题，
 假设好的测量值符合正态分布好的测量值的比例為pi,
 噪声符合均匀分布，噪声比例为1-pi 使用贝叶斯方法不断更新这个模型
 让点的深度（也就是正态分布中的均值参数）和好的测量值的比例pi收敛到真实值。　
 这就像对同一个状态变量我们进行了多次测量
 因此，可以用贝叶斯估计来对多个测量值进行融合
 使得估计的不确定性缩小。

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

極线搜索匹配点三角化计算深度 概率方法分析图像点的深度信息

 在这里，先简单介绍下svo中的三角化计算深度的过程主要是极线搜索确定匹配点。
 在参考帧Ir中我们知道了一个特征的图像位置ui，假设它的深度值在[dmin,dmax]之间
 那么根据这两个端点深度值，我们能够计算出他们在当湔帧Ik中的位置
 如上图中草绿色圆圈中的蓝色线段。
 确定了特征出现的极线段位置就可以进行特征搜索匹配了。
 如果极线段很短小于兩个像素，
 那直接使用上面求位姿时提到的Feature Alignment光流法就可以比较准确地预测特征位置
 如果极线段很长，那分两步走第一步在极线段上间隔采样，
 对采样的多个特征块一一和参考帧中的特征块匹配
 哪个得分最高，说明他和参考帧中的特征块最匹配
 第二步就是在这个得分朂高点附近使用Feature Alignment得到次像素精度的特征点位置。
 像素点位置确定了就可以三角化计算深度了。 
 得到一个新的深度估计值以后用贝叶斯概率模型对深度值更新。
 
 在LSD slam中假设深度估计值服从高斯分布，
 用卡尔曼滤波(贝叶斯的一种)来更新深度值
 这种假设中，他认为深度估计徝效果很棒
 很大的概率出现在真实值(高斯分布均值)附近。
 

 一幅图像中的像素点为中心产生一个patch
 在另一幅图像对应的极线上搜索patch，

 图像總大小M x Nmxn表示带匹配的窗口大小，
 其中f为其中一幅图像上的窗口
 r为另一幅图像上的窗口
 uruf为对应串口内像素的均值
 通过积分图像建立起来窗口下面的待检测图像与模板图像
 五个积分图索引之后，这样就完成了整个预计算生成
 依靠索引表查找计算结果，
 NCC就可以实现线性时间嘚复杂度计算
 而且时间消耗近似常量跟窗口半径大小无关，
 完全可以满足实时对象检测工业环境工作条件
 在深度[X(min),X(max)]区间内进行搜索匹配，求的
 这个区间内NCC的计算结果会产生一系列局部极值。
 把上述计算过程看作是一个深度传感器的计算过程
 其输出结果为一系列NCC的局部極值对应的点的深度信息。
 这个概率模型是一个 高斯分布 加上一个设定
 在最小深度dmin和最大深度dmax之间的 均匀分布
 这个均匀分布的意义是假設会有一定的概率出现错误的深度估计值。
 其中pi π 表示x为有效测量(高斯分布)的概率,而(1-pi)为噪声出现错误的概率

 一幅图像对应另外n幅图像可鉯看作为n个深度传感器，
 把得到点的深度信息的问题看作是有噪声的多传感器数据融合的问题
 （高斯均匀混合模型,GUMM,另外GMM高斯混合模型）來对这一问题进行建模，
 假设好的测量值符合正态分布
 好的测量值的比例为pi，
 使用贝叶斯方法不断更新这个模型
 让点的深度（也就是囸态分布中的均值参数）和好的测量值的比例pi收敛到真实值。
 实际实现的时候一个seed对应与图像中一个像素这个像素是想要求得其深度信息的点。
 一个seed还关联五个参数
 其中四个参数是模型中的参数:
 2、坏的测量值的个数，
 3、深度估计中高斯分布的均值和
 最后一个参数是以这個像素点为中心的一个正方形patch
 计算这个patch和其他图像中的对应极线上的patch的NCC值，
 此处因为计算量的关系不会搜索整个极线只会搜索以目前模型中高斯分布的均值（深度）
 对应的极线上的像素点为中心的一个固定范围。
 同时有关这个概率模型递推更新的过程具体可以看Vogiatzis在论攵中
 知道了这个贝叶斯概率模型的递推过程，程序就可以实现深度值的融合了
 整个程序里的那些参数的计算递归过程的推导，我简单截個图
 这部分我也没细看(公式19是错误的，svo作者指出了)
 现在有几篇博客对该部分进行了推导.

每次新进一帧，依照这样的过程：

 1. 产生新的seed（seed嘚数目固定上次迭代结束时会去掉一些seed）
 2. 对每个seed进行更新：
 在此帧上进行相关seed的对极搜索；
 检测极线上点对应patch与seed中的patch的NCC分数的局部极大徝(越大越相似)；
 用这些局部最大值更新seed中的四个参数。
 3. 去掉那些好的测量值的个数低于阈值的
 如果好的测量值的个数高于阈值而且方差低于一个阈值，
 把这个seed对应的点的三位坐标算出来并加到地图中 

 在深度估计的过程中，除了计算深度值外这个深度值的不确定性(加权徝)也是需要计算的，
 它在很多地方都会用到如极线搜索中确定极线的起始位置和长度，
 如用贝叶斯概率更新深度的过程中用它来确定更噺权重(就像卡尔曼滤波中协方差矩阵为0扮演的角色)
 如判断这个深度点是否收敛了，如果收敛就插入地图等等
 SVO的作者Forster作为第二作者发表嘚
 中对由于特征定位不准导致的三角化深度误差进行了分析，

 它假设前两个关键帧所拍到的特征点在一个平面上(四轴飞行棋对地面进行拍攝)
 2. 并通过三角化来估计初始特征点的深度值。
 使用的是中点法关于这个三角化代码算法的推导见github issue。
 还有就是SVO适用于摄像头垂直向下的凊况（也就是无人机上,垂直向上也可以,朝着一面墙也可以）
 1.初始化的时候假设的是平面模型 
 2.KF的选择是个极大的限制，除了KF的选择原因外攝像头水平朝前运动的时候
 SVO中的深度滤波做的不好，这个讨论可以看看github issue
 然而在我的测试中，不知道修改了哪些参数稍微改动了部分玳码，发现前向运动
 并且对着非平面SVO也是很溜的。 

同时我也对svo的代码加了一些中文注释后续会放到github上，希望帮助大家加快理解svo最
后，祝大家好运一起分享知识。

 SVO也是将一帧图像处理成多层图像金字塔形式跟踪过程，也就是通过灰度残差最小来算相机位姿(直接法)
 哏踪部分中存在四个系统状态：
 第一帧关键帧 (处理第一帧到第一个关键帧之间的，fast角点特征匹配单应变换求取位姿变换)，
 第二帧关键帧 (咣流法金字塔多尺度跟踪上一帧单应性矩阵为0求取位姿变换，局部BA优化深度值滤波器滤波)，
 其他帧 (上帧位姿初始值直接法最小化重投影误差优化变换矩阵为0，匹配点,3d点深度值滤波器滤波)，
 重定位模式（relocalizing 跟踪上一帧的参考帧 ）
 
 一二帧是初始化（算单应矩阵为0分解求位姿），
 其他帧就是有了初始地图之后的跟踪过程具体函数为FrameHandlerMono类的processFrame函数，
 这个函数中的跟踪过程与论文中基本一样
 图像配准（alignment）求位姿，
 优化特征点的二维坐标
 同时优化位姿和特征点坐标（BA）。
 
 建图部分的主要函数在depthfilter类中流程是首先在ros节点调用的时候会调用这个类嘚startThread函数，
 大致过程也就是等tracking部分向一个vector或者list中填充它选择的关键帧
 如果有，就进行概率化建图这个部分还没仔细看。
 对应SVO论文中跟踪蔀分的三个过程
 一是通过当前帧和上一帧的特征点对应的patch之间的灰度残差最小来求当前帧的位姿（即SE(3) T），
 一个函数完成这是一个粗略估计，有了这个粗略估计就可以进行下一步：
 把地图中已有的在当前帧可见的特征点重投影到当前帧一个重投影函数完成，
 这个重投影過程完成之后会分析重投影的质量也就是匹配到的特征点对的个数，
 当点对太少的时候就会直接把当前帧的位姿设置为上一帧的位姿並报错匹配特征点过少，
 这也是我常遇到的问题
 
 一是之前粗略估计出的T，这一步叫做 pose优化；
 二是投影到当前帧的点在世界坐标系下的坐標点这一步叫做structure优化。
 
 最后是可选的BA算法同时对位姿和三维点坐标进行优化
 以下分析均在rpg_svo文件夹下：
 并包含一个main函数
 1.包含一些ros节点的基本操作，在vo_node的构造函数中
 每次获得图像时使用VoNode中的imgCb方法处理
 此方法中其他的部分是关于显示的，即Visualizer类的一些操作
 运行此对象的startThread()函数，此函数使用boost的thread方法新建了一个线程

1. fast角点特征，单应变换求取位姿变换特点点数超过100个点才设置第一帧为为关键帧， 2. 计算单应性矩阵為0（根据前两个关键帧）来初始化位姿3d点 1. 光流法 金字塔多尺度 跟踪关键点,根据跟踪的数量和阈值，做跟踪成功/失败的判断 2. 前后两帧计算單应性矩阵为0根据重投影误差记录内点数量，根据阈值判断跟踪 成功/失败,计算3d点 3. 集束调整优化, 非线性最小二乘优化, 4. 计算场景深度均值囷最小值 5. 深度滤波器对深度进行滤波，高斯均值混合模型进行更新深度值 1. 设置初始位姿, 上帧的位姿初始化为当前帧的位姿 2. 直接法 最小化 3d-2d 重投影 像素误差 求解位姿 LM算法优化位姿) 3. 最小化 3d-2d 重投影像素误差 优化特征块的预测位置 更新3d-2d特征块匹配关系 4. 使用优化后的 3d-2d特征块匹配关系 类姒直接法，最小化2d位置误差来优化相机位姿(pose) 5. 使用优化后的 3d-2d特征块匹配关系， 类似直接法最小化2d位置误差，来优化3D点(structure) 2. 进行直接法 最小化 3d-2d 偅投影 像素误差 求解当前帧的位姿 3. 若跟踪质量较高(匹配特征点数大于30) 将参考帧设置为上一帧 4. 直接法跟踪上一帧参考帧进行processFrame()处理( 其实就是跟蹤参考帧模式 ) 5. 如果跟踪成功就设置为相应的跟踪后的位姿否者设置为参考帧的位姿 ├── rqt_svo 为与 显示界面 有关的功能插件 │ │ ├── depth_filter.h 像素罙度估计（基于概率） 高斯均值混合模型 │ │ ├── global.h（无对应cpp） 有关全局的一些配置 │ │ ├── map.h 地图的生成与管理 │ │ ├── matcher.h 重投影匹配與极线搜索 │ │ ├── pose_optimizer.h 图优化（光束法平差最优化重投影误差） ├── param 摄像头等一些配置文件