SVO

SVO 从名字来看，是半直接视觉里程计，所谓半直接是指通过对图像中的特征点图像块进行直接匹配来获取相机位姿，而不像直接匹配法那样对整个图像使用直接匹配。整幅图像的直接匹配法常见于RGBD传感器，因为RGBD传感器能获取整幅图像的深度。 
  虽然semi-direct方法使用了特征，但它的思路主要还是通过direct method来获取位姿，这和feature-method不一样。同时，semi-direct方法和direct method不同的是它利用特征块的配准来对direct method估计的位姿进行优化。 
  和常规的单目一样，SVO算法分成两部分: 位姿估计，深度估计。本文对论文内容用自己的理解进行解读，并对一些关键内容会推荐一些参考文献帮助大家更进一步的学习SVO。

位姿估计

  svo 方法中motion estimation的步骤可以简单概括如下:

1. 对稀疏的特征块使用direct method 配准，获取相机位姿；
2. 通过获取的位姿预测参考帧中的特征块在当前帧中的位置，由于深度估计的不准导致获取的位姿也存在偏差，从而使得预测的特征块位置不准。由于预测的特征块位置和真实位置很近，所以可以使用牛顿迭代法对这个特征块的预测位置进行优化。
3. 特征块的预测位置得到优化，说明之前使用直接法预测的有问题。利用这个优化后的特征块预测位置，再次使用直接法，对相机位姿(pose)以及特征点位置（structure）进行优化。

  下面结合作者forster的原论文对motion estimation进行更详细的讨论。

1.sparse model-based image alignment

  使用直接法最小化图像块重投影残差来获取位姿。如图所示：其中红色的Tk,k−1为位姿，即优化变量。

 
直接法具体过程如下： 
  step1. 准备工作。假设相邻帧之间的位姿Tk,k−1已知，一般初始化为上一相邻时刻的位姿或者假设为单位矩阵。通过之前多帧之间的特征检测以及深度估计，我们已经知道第k-1帧中特征点位置以及它们的深度。 
  step2. 重投影。知道Ik−1中的某个特征在图像平面的位置(u,v)，以及它的深度d，能够将该特征投影到三维空间pk−1，该三维空间的坐标系是定义在Ik−1摄像机坐标系的。所以，我们要将它投影到当前帧Ik中，需要位姿转换Tk,k−1，得到该点在当前帧坐标系中的三维坐标pk。最后通过摄像机内参数，投影到Ik的图像平面(u′,v′)，完成重投影。 
  step3. 迭代优化更新位姿 。按理来说对于空间中同一个点，被极短时间内的相邻两帧拍到，它的亮度值应该没啥变化。但由于位姿是假设的一个值，所以重投影的点不准确，导致投影前后的亮度值是不相等的。不断优化位姿使得这个残差最小，就能得到优化后的位姿Tk,k−1。 
  将上述过程公式化如下：通过不断优化位姿Tk,k−1最小化残差损失函数。 
其中
公式中第一步为根据图像位置和深度逆投影到三维空间，第二步将三维坐标点旋转平移到当前帧坐标系下，第三步再将三维坐标点投影回当前帧图像坐标。当然在优化过程中，残差的计算方式不止这一种形式：有前向(forwards)，逆向(inverse)之分，并且还有叠加式(additive)和构造式(compositional)之分。这方面可以读读光流法方面的论文，Baker的大作《Lucas-Kanade 20 Years On: A Unifying Framework》。选择的方式不同，在迭代优化过程中计算雅克比矩阵的时候就有差别，一般为了减小计算量，都采用的是inverse compositional algorithm。 
  上面的非线性最小化二乘问题，可以用高斯牛顿迭代法求解，位姿的迭代增量ξ(李代数)可以通过下述方程计算：
其中雅克比矩阵为图像残差对李代数的求导，可以通过链式求导得到：这中间最复杂的部分是位姿矩阵对李代数的求导。很多文献都有提到过，比如DTAM作者Newcombe的博士论文，gtsam的作者Dellaert的数学笔记。不在这里展开(有两篇博客的篇幅)，可以参看清华大学王京的李代数笔记。 
  好了，先别在旁枝末叶上耗费精力，继续回到主题。到这里，我们已经能够估计位姿了，但是这个位姿肯定不是完美的。导致重投影预测的特征点在Ik中的位置并不和真正的吻合，也就是还会有残差的存在。如下图所示：
图中灰色的特征块为真实位置，蓝色特征块为预测位置。幸好，他们偏差不大，可以构造残差目标函数，和上面直接法类似，不过优化变量不再是相机位姿，而是像素的位置(u′,v′)，通过迭代对特征块的预测位置进行优化。这就是svo中提到的Feature Alignment。

2.Relaxation Through Feature Alignment

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

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

J=[∂r∂u′∂r∂v′]=[∂I(u′,v′)∂u′∂I(u′,v′)∂v′]

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

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)变量求导)。

Discussion:

  作者在论文discussion中简单阐述了他的motion estimationg 方法和直接使用直接法的优点，也说明了对比直接使用光流跟踪再优化位姿的优点，可以简单看看。

depth估计

  最基本的深度估计就是三角化，这是多视角几何的基础内容(可以参看圣经Hartly的《Multiple View Geometry in Computer Vision》中的第十二章structure computation；可以参看我的相应博客)。我们知道通过两帧图像的匹配点就可以计算出这一点的深度值，如果有多幅图像，那就能计算出这一点的多个深度值。这就像对同一个状态变量我们进行了多次测量，因此，可以用贝叶斯估计来对多个测量值进行融合，使得估计的不确定性缩小。如下图所示：

一开始深度估计的不确定性较大(浅绿色部分)，通过三角化得到一个深度估计值以后，能够极大的缩小这个不确定性(墨绿色部分)。 
  在这里，先简单介绍下svo中的三角化计算深度的过程，主要是极线搜索确定匹配点。在参考帧Ir中，我们知道了一个特征的图像位置，假设它的深度值在[dmin,dmax]之间，那么根据这两个端点深度值，我们能够计算出他们在当前帧Ik中的位置，如上图中草绿色圆圈中的线段。确定了特征出现的极线段位置，就可以进行特征搜索匹配了。如果极线段很短，小于两个像素，那直接使用上面求位姿时提到的Feature Alignment光流法就可以比较准确地预测特征位置。如果极线段很长，那分两步走，第一步在极线段上间隔采样，对采样的多个特征块一一和参考帧中的特征块匹配，用Zero mean Sum of Squared Differences 方法对各采样特征块评分，那个得分最高，说明他和参考帧中的特征块最匹配。第二步就是在这个得分最高点附近使用Feature Alignment得到次像素精度的特征点位置。像素点位置确定了，就可以三角化计算深度了。 
  得到一个新的深度估计值以后，用贝叶斯概率模型对深度值更新。在LSD slam中，假设深度估计值服从高斯分布，用卡尔曼滤波(贝叶斯的一种)来更新深度值。这种假设中，他认为深度估计值效果很棒，很大的概率出现在真实值(高斯分布均值)附近。而SVO的作者采用的是Vogiatzis的论文《Video-based, real-time multi-view stereo》提到的概率模型：这个概率模型是一个高斯分布加上一个设定在最小深度dmin和最大深度dmax之间的均匀分布。这个均匀分布的意义是假设会有一定的概率出现错误的深度估计值。有关这个概率模型来由更严谨的论证去看看Vogiatzis的论文。同时，有关这个概率模型递推更新的过程具体可以看Vogiatzis在论文中提到的Supplementary material，论文中告知了下载地址。知道了这个贝叶斯概率模型的递推过程，程序就可以实现深度值的融合了，结合supplementary material去看svo代码中的updateSeeds(frame)这个程序就容易了，整个程序里的那些参数的计算递归过程的推导，我简单截个图，这部分我也没细看(公式19是错误的，svo作者指出了)，现在有几篇博客对该部分进行了推导，卢彦斌：svo原理解析,东北大学孙志明：svo的Supplementary matterial 推导过程。 
 
  在深度估计的过程中，除了计算深度值外，这个深度值的不确定性也是需要计算的，它在很多地方都会用到，如极线搜索中确定极线的起始位置和长度，如用贝叶斯概率更新深度的过程中用它来确定更新权重(就像卡尔曼滤波中协方差矩阵扮演的角色)，如判断这个深度点是否收敛了，如果收敛就插入地图等等。SVO的作者Forster作为第二作者发表的《REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time》中对由于特征定位不准导致的三角化深度误差进行了分析，如下图： 
 
它是通过假设特征点定位差一个像素偏差，来计算深度估计的不确定性。具体推导见原论文，简单的几何关系。 
  最后，简单说下SVO的初始化过程：它假设前两个关键帧所拍到的特征点在一个平面上(四轴飞行棋对地面进行拍摄)，然后估计单应性H矩阵，并通过三角化来估计初始特征点的深度值。SVO初始化时triangulation的方法具体代码是vikit/math_utils.cpp里的triangulateFeatureNonLin()函数，使用的是中点法，关于这个三角化代码算法的推导见github issue。还有就是SVO适用于摄像头垂直向下的情况（也就是无人机上,垂直向上也可以,朝着一面墙也可以），为什么呢？1.初始化的时候假设的是平面模型 2.KF的选择是个极大的限制，除了KF的选择原因外摄像头水平朝前运动的时候，SVO中的深度滤波做的不好，这个讨论可以看看github issue，然而在我的测试中，不知道修改了哪些参数，稍微改动了部分代码，发现前向运动，并且对着非平面SVO也是很溜的。