SVO论文笔记

写在前面

最近看了一下SVO的论文，发现原来在N年前就已经有这么优秀的VO了，甚是惭愧，因此发奋总结了其论文中的要点，有些问题先埋着，需要看完代码进行补充。

---

SVO的框架结构

SVO的整体框架如下图，可以看到整个SVO的结构是由两个线程构成的，第一个线程有三个主要的操作，a. Sparse Model-based Image Alignment; b. Feature Alignment; c. Pose&Structure Refinement。第二个线程主要是map的过程，一旦有一帧被定义为keyframe，就提取其中的feature，并初始化深度滤波器，如果不是keyframe，则一直更新深度滤波器，如果某个feature的深度收敛了，则生成3D点放入Map中作为landmark

---

Motion Estimate Thread

该线程中主要求解当前帧的位姿(Sparse Model-based Image Alignment; Feature Alignment)和local BA(Pose&Structure Refinement)

Sparse Model-based Image Alignment

该部分类似于光流，不过求解的不是对应关系，而是相对位姿。
其优化函数为:
$$T_{k-1}^k=argmin{\iint }_{\Re }\rho [\delta I(T,u)]du$$
其中:

1. $\rho [\ast ]=0.5\Vert \ast {\Vert }^2$，可见整个过程是一个最小二乘法的问题；
2. $\delta I(T,u)=I_k(\pi (T{\pi }^{-1}(u,du)))-I_{k-1}(u)$，这个残差对比的是图像位置上的像素值的灰度，其中$u\in \Re ,\Re$表示即可以在k-1帧图像上看到，又可以通过投影在k帧上看到
3. 对于比对灰度值的算法来说，一般都会用一个patch size中的全部像素的灰度进行对比，因此下面的公式中都不仅仅使用一点像素，而是使用多点像素进行求解的，但是在这个过程中，作者为了提高计算的速度，并没有进行patch的投影，算是以量取胜吧

把上述的notation带入到优化函数中就可以得到
$$T_{k-1}^k=argmin\sum _{i\in \Re }\frac{1}{2}\Vert \delta I(T_{k-1}^k,u_i){\Vert }^2(1)$$
对于该方法来说，因为假设的前提是帧与帧之间的motion较小，且光强不变，因此论文中该处将当前帧的位姿设为了和上一帧一样的位姿，也就是说初始的相对位姿为0，而优化函数求解的就是扰动，基于这样的思路，对$\delta I(T_{k-1}^k,u_i)$进行一阶展开
$$\begin{gathered} \delta I(\zeta ,ui)=I_k(\pi (T_{k-1}^k\ast pi))-I_{k-1}(\pi (T(\zeta )\ast pi))(2) \\ \delta I(\zeta ,ui)=\delta I(0,ui)+\frac{d\delta I(0,ui)}{d\zeta }\zeta (3) \\ \frac{d\delta I(0,ui)}{d\zeta }=\frac{-I_{k-1}(a)}{da}\times \frac{da}{db}\times \frac{db}{dT(\zeta )}\times \frac{dT(\zeta )}{d\zeta } \\ Notation:a=\pi (T(\zeta )\ast pi)b=T(\zeta )\ast pi \end{gathered}$$
此处，$T_0=I,T(\zeta )$是最终我们优化的变量，这里并没有采用直接的求解位姿的方法，而是采用inverse composition的方法，即$T_{k-1}^k=T_{k-1}^{k}T(\zeta {)}^{-1}$，可以看到随着优化的进行，$T(\zeta )$越来越小，$T_{k-1}^k$从0趋近于正确的值。(这里埋一个问题，为什么不用直接的更新方法而使用这样的更新方法呢？)
对于公式(1)，很明显为了求解这样的最小二乘问题，只需要使得其对于$\zeta$的导数为0就可以了，因此其导数为
$$\begin{gathered} \sum _{i\in \Re }\delta I(\zeta ,ui{)}^T\nabla \delta I(\zeta ,ui)=(\delta I(0,ui)+\frac{d\delta I(0,ui)}{d\zeta }\zeta {)}^T\ast \frac{d\delta I(0,ui)}{d\zeta }=>J^{T}J\zeta =-J^{T}b(4) \\ Notation:J=\frac{d\delta I(0,ui)}{d\zeta }b=\delta I(0,ui) \end{gathered}$$
可以看到，直接法求解过程中整个计算量还是很小的，一个图像梯度，一个2D对3D求导，一个SE(3)的扰动模型

论文给出上图说明上述的过程

Feature Alignment

在上一步骤中，我们获得了一个比较粗略(coarse)的相对位姿，虽然粗略，但是也基本上是得到了位姿和跟踪到的点了（这些点都是上一步骤求解出位姿之后通过映射关系映射过来的），因为位姿是粗略的，所以得到的这些映射的点的位置($u^{\prime }$)也是粗略的，所以该步骤就是使用参考帧上的点修正一下这些粗略的点的位置，论文中称为Feature Alignment
首先是优化函数
$$u_i^{\prime }=argmin\frac{1}{2}\Vert I_k(u_i^{\prime })-A_i{I}_r(u_i){\Vert }^2$$
其中由于参考帧距离当前帧比较远，因此中间需要一个仿射变换把当前帧的图像状态与参考帧的图像状态变得一致。
鉴于这个的优化比较简单，导数就是一个梯度，因此这里不进行展开了。

论文给出上图说明上述的过程

Pose&Structure Refinement

经过上述的两个过程，论文描述基本上亚像素的误差平均在0.3左右，接下来的这一步中，SVO依旧进行位姿的优化，优化函数如下：
$$T_w^k=argmin\frac{1}{2}\sum _i\Vert u_i-\pi (T_w^k,pi){\Vert }^2$$
这个部分就是一个BA，不过论文也说了，最后一步也会有一个local BA，修正位姿和3D点

论文给出上图表示上述的过程

Discussion

论文讨论了该方法中的前几个步骤，讨论了如果使用传统的LK光流法的话，时间会增加，同时较大距离的跟踪需要较大的patch以及金字塔的应用，同时因为会存在跟丢的情况，因此需要外点检测，而在SVO的方法中，因为有feature alignment的步骤，使得特征点的位置还是比较准确的，因此能保证没有外点

---

Mapping Thread

该线程中主要的工作就是判断当前帧是否可以作为key frame，之后进行深度滤波器的迭代以及初始化工作
下面主要说明深度滤波器的相关推导

depth-filter

深度滤波器主要是使用贝叶斯公式进行特征点深度的迭代推导，如下图所示

在深度滤波器中，论文把真实深度的分布看做一个正态分布+均匀分布的模型
$$p(x|Z,\pi )=\pi N(x|Z,{\tau }^2)+(1-\pi )U(x)(5)$$
其中$\pi$表示该次测量为有效的概率。

---

1. 近似表示后验分布

可以看到上面的式子其实还是很难去看本次测量到底是说与有效还是无效的，因此论文引入了一个变量$y_n$，这个变量是一个二值的变量，为1时表示深度满足高斯分布，为0时说明深度服从均匀分布，说明真实的深度在一个范围内都可能存在，于是上式变为
$$\{\begin{array}{l}p(x_n|Z,\pi ,y_n)=N(x_n|Z,{\tau }^2{)}^{y_n}U(x_n{)}^{1-y_n}\\ p(y_n|\pi )={\pi }^{y_n}(1-\pi {)}^{1-y_n}\end{array}$$
设 X = { x 1 , x 2 , . . . , x n } X=\{x_1, x_2, ..., x_n\} X={x1​,x2​,...,xn​}， Y = { y 1 , y 2 , . . . , y n } Y=\{y_1, y_2,...,y_n\} Y={y1​,y2​,...,yn​}，则所有变量的联合分布如下
$$\begin{gathered} p(X,Y,Z,\pi )=p(X,Y|Z,\pi )p(Z,\pi )=p(X|Y,Z,\pi )p(Y|Z,\pi )p(Z,\pi )=p(X|Y,Z,\pi )p(Y|\pi )p(Z)p(\pi ) \\ =>p(X,Y,Z,\pi )=[\prod _{n=1}^{N}p(x_n|y_n,Z,\pi )p(y_n|\pi )]p(z)p(\pi )(6) \end{gathered}$$
上式中因为Z和Y，$\pi$没有直接的关系，因此满足独立分布。
由于我们并不知道真实的分布$p(y_n,Z,\pi |x_n)$，我们使用近似的方法对真实的分布进行近似，假如我们要近似的分布为$q(y_n,Z,\pi )$,此时论文做出如下假设$q(Y,Z,\pi )=q_Y(Y)q_{Z,\pi }(Z,\pi )$，当然这里我认为并不是因为他们是相互独立的，而是通过边缘化的手段得到的。为什么要经过这样的变换呢？回到我们最初的想法，我们最终要求解的是$p(Z,\pi )$，即使我们通过别的分布来近似，我们最终要求的实际上也必须是$q(Z,\pi )$，所以可以看到，这个公式主要的作用其实是把Y给边缘化出去了，保留下来了我们真正需要求的东西了。
回到正题，我们做这一步是为了干什么呢？其实我们现在假设了一个分布q()，但是我们并不知道这个分布到底是什么-_-，下面我们就来求这个神秘的分布。
根据熵的原理，如果我们的近似分布q()与p()真的是相似的，那么他们的熵必然也是差不多的，有$lnq()=lnp()$，所以
$$lnq(Z,\pi ,Y|X)=lnP(Z,\pi ,Y|X)=ln(\frac{P(Z,\pi ,Y,X)}{P(X)})=ln(P(Z,\pi ,Y,X))+const(7)$$
正如上面的推论所述，我们近似的分布q()与联合分布$p(Z,\pi ,Y,X)$有直接的关系，此时，我们如果把公式(7)中的Y边缘化掉，那么我们就可以很清楚的得到两者之间的关系了，即有下面的公式
$$ln{q}_{Z,\pi }(Z,\pi ,Y|X)=E_y[ln(P(Z,\pi ,Y,X))]+const(8)$$
比较幸运的是，在公式(8)中，我们可以很容易的就看到了p()是公式(6)，因此全部带入之后，就可以得到论文中的公式

随后再把ln去掉，就得到

作者认为这样的形式是符合高斯分布×Beta分布的

2. 近似后验的迭代公式

通过第一步，我们知道了我们近似的后验长什么样子，那现实的问题是，如何更新近似的后验q()呢？
这时候就要用到贝叶斯公式了(虽然上面已经用了多次贝叶斯了)
$$p(Z,\pi |X)=p(X|Z,\pi )p(Z,\pi )=>\overline{q}(Z,\pi )=p(X|Z,\pi )q(Z,\pi )(9)$$
将公式(5)和高斯×Beta分布带入之后就可以得到迭代公式
$$\begin{gathered} q(Z,\pi )=[\pi N(x|Z,{\tau }^2)+(1-\pi )U(x)]N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b) \\ =\pi N(x|Z,{\tau }^2)N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b)+(1-\pi )U(x)N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b) \end{gathered}$$
推导到上面的式子之后其实并不算完成，因为Beta分布依旧是原来的参数，这时我们可以看到一种极端的情况是，如果当前的测量是一个好的测量，那么加号前面的部分就会当做后验，反之，后面就会作为后验，所以我们需要将前面的Beta(a,b)分布变为Beta(a+1,b)，后面的Beta(a,b)变为Beta(a, b+1)。
根据Beta分布的性质，很容易得到转换公式：
$$Beta(a+1,b)=\frac{(a+b)}{a}\pi Beta(a,b)andBeta(a,b+1)=\frac{(a+b)}{b}(1-\pi )Beta(a,b)$$
带入上面公式之后再经过一个特别繁琐的化简之后，就能得到如下的迭代公式
$$\begin{gathered} q(Z,\pi )=\frac{a}{(a+b)}N(x|\mu ,{\tau }^2+{\sigma }^2)N(Z|m,s^2)Beta(a+1,b)+\frac{b}{(a+b)}U(x)N(Z|\mu ,{\sigma }^2)Beta(a,b+1) \\ =>N(Z|{\mu }^{\prime },{\tau }^{\prime 2})Beta(a^{\prime },b^{\prime })=C_{1}N(Z|m,s^2)Beta(a+1,b)+C_{2}N(Z|\mu ,{\sigma }^2)Beta(a,b+1) \end{gathered}$$
其实剩下的就是艰苦卓绝的化简工作，这里我只推了一下高斯分布的，beta分布实在太麻烦了，因此直接上结论
$$\{\begin{array}{l}\frac{1}{s^2}=\frac{1}{{\sigma }^2}+\frac{1}{{\tau }^2}\\ m=s^2(\frac{\mu }{{\sigma }^2}+\frac{x}{{\tau }^2})\\ {\mu }^{\prime }=C_{1}m+C_2\mu \\ {\sigma }^{\prime 2}+{\tau }^{\prime 2}=C_1(s^2+m^2)+C_2({\sigma }^2+{\mu }^2)\\ \frac{a^{\prime }}{a^{\prime }+b^{\prime }}=C_1\frac{a+1}{a+b+1}+C_2\frac{a}{a+b+1}\\ \frac{a^{\prime }(a^{\prime }+1)}{(a^{\prime }+b^{\prime })(a^{\prime }+b^{\prime }+1)}=C_1\frac{(a+1)(a+2)}{(a+b+1)(a+b+2)}+C_2\frac{a(a+1)}{(a+b+1)(a+b+2)}\end{array}$$
OK，上面的公式就是新旧状态之间的更新公式了，其中a，b是内点外点的检测次数(这个有待看代码是如何判断的)，$\mu ,\sigma$是深度滤波器初始化时候的参数，而$\tau$论文中说是根据根据测量时刻的两个相机的相对位置产生的，个人感觉是位置越远，$\tau$越小，因为基线长。

---

总结

可以从论文看出来整个SVO的框架还是很清晰的，上述有一些参数或者实现具体还要在代码中查看，以后有时间看完代码之后再来进行补充～加油加油