Semi-direct Viusal Odometry学习笔记(二)

创建地图点

在SVO中，后端的特征点是只在关键帧上提取的，用FAST加金字塔。而上一个关键帧的特征点在这一个关键帧上找匹配点的方法，是用极限搜索，寻找亮度差最小的点。最后再用depthfilter深度滤波器把这个地图点准确地过滤出来。VINS中对特征点的跟踪是使用的光流的方法，对每个特征位置的误差没有进行优化处理，优化的只是特征对应的空间点深度。

在SVO中，选取30个地图点，如果这30个地图点在当前帧和最近一个关键帧的视差的中位数大于40，或者与窗口中的关键帧的距离大于一定阈值，就认为需要一个新的关键帧。然后把当前帧设置为关键帧，对当前帧进行操作。

2.1 初始化种子
当关键帧过来的时候，对关键帧进行处理。在当前图像上，划分出$25pixel\times 25pixel$的网格。
首先，当前帧上的这些已有的特征点，占据住网格。

在当前帧的5层金字塔上，每层提取fast点，首先用$3\times 3$范围的非极大值抑制。然后，对剩下的点，全部都计算shiTomasi分数。再全部映射到第0层的网格上，每个网格只保留分数最大的，且大于阈值的那个点。

然后，对于所有的新的特征点，初始化成种子点。用高斯分布表示逆深度，均值为最近的那个点的深度的倒数。深度范围为当前帧的最近的深度的倒数，即1.0/depth_min。高斯分布的标准差为1/6*1.0/depth_min。

2.2 更新种子，深度滤波器
如果新来一个关键帧，或者是当前的普通帧，或者之前的关键帧，用于更新种子点。对于每个种子点，通过正负1倍标准差，确定逆深度的搜索范围。这些参数都是对应种子点在它自己被初始化的那一帧。
然后把深度射线上的最短和最长的深度，映射到当前帧的单位深度平面上，其实就得到的在单位平面上的极限线段。然后，再把逆深度的均值对应的深度，映射到当前帧，就是跟1.3中的同样的方法，得到图块放射矩阵，和最佳搜索层数。

把极线线段投影到对应的层数上，如果两个端点间的像素距离小于2个像素，就直接进行优化位置。用的是1.3中的找图块匹配的方法，把对应的图块映射过来。找到最佳匹配位置后，进行三角定位。
$$\begin{gathered} s_{cur}{x}_{cur}=s_{ref}{R}_{c\to ref}{x}_{ref}+t_{cur\to ref} \\ {s}_r{R}_{c,r}{x}_r-s_c{x}_c=-t_{c,r} \\ \left[\begin{array}{cc}{R}_{c,r}{x}_r& -x_c\end{array}\right]\left[\begin{array}{c}{s}_r\\ s_c\end{array}\right]=-t_{c,r} \\ \left[\begin{array}{c}{s}_r\\ s_c\end{array}\right]=-({\left[\begin{array}{cc}{R}_{c,r}{x}_r& -x_c\end{array}\right]}^T\left[\begin{array}{cc}{R}_{c,r}{x}_r& -x_c\end{array}\right]{)}^{-1}{\left[\begin{array}{cc}{R}_{c,r}{x}_r& -x_c\end{array}\right]}^T{t}_{c,r} \end{gathered}$$
如果两个端点间像素距离大于2个像素，就在极线上进行搜索。首先，确定总步长数，以两端点间的距离除以0.7，得到总步长数n_steps。然后，把单位深度平面上的极线线段分n_steps段，从一个端点开始往另外一个端点走，每走一步，就把位置投影到对应层数的图像上，坐标取整后，获取图块。然后，计算投影过来的图块与投影位置图块的相似度。如果分数小于阈值，就认为两个图块是相似的。在当前位置，再进行优化位置。然后进行三角定位。

接下来，计算三角定位出来的深度值的协方差。假设，在图像上的测量协方差为1个像素，则这个协方差的传递到深度上的过程如下。这个传递的，都是标准差。计算过程应用的是三角形的相关知识，参考《视觉SLAM十四讲》。
再把这个协方差传递到逆深度上。假设这时候三角定位出来的深度值为$z$，则在逆深度上的标准差${\delta }_{i}nv$为
$${\delta }_{i}nv=\frac{1}{2}(\frac{1}{z-{\sigma }_{obs}}-\frac{1}{z+{\sigma }_{obs}})$$
接下来就是更新种子点的深度分布了。下面进行具体的介绍。另外，如果种子点的方差，小于深度范围/200的时候，就认为收敛了，它就不再是种子点，而是candidate点。candidate点被成功观察到1次，就变成UNKNOW点。UNKNOW被成功观察到10次，就变成GOOD点。如果多次应该观察而没有被观察到，就变成DELETE点。

以下内容参考SVO原理解析，其来源于参考文献[1]：
$$\text{G. Vogiatzis and C. Hern´ andez, “Video-based, Real-Time Multi View Stereo,” Image and Vision Computing, vol. 29, no. 7, 2011.}$$
给定已知相对位姿的两个视角下的图像$I,I^{\prime }$。由两幅图像中的对应点及位姿可以计算得到一个深度值$x$。由于重建误差和无匹配的存在，考察实际情况中$x$的直方图分布，[1]认为，$x$的分布可以用高斯分布和均匀分布来联合表示
$$p(x|Z,\pi )=\pi N(x|Z,r^2)+(1-\pi )U(x|Z_{min},Z_{max})$$
其中，$\pi$表示$x$为有效测量的概率。下图是一个若干测量的直方图例子。$x$轴表示深度测量范围$[-5,5]$，$y$轴表示直方图统计

考虑同一个seed的一系列测量$x_1,x_2,\dots ,x_n$，假设这些测量都是独立的。我们想从(1)式求出$Z,\pi$。最直观的做法是通过最大似然估计求解。然而[1]认为最大似然估计容易被局部极大值干扰，其结果并不准确。[1]选择从最大后验概率求解，等价于
$$argma{x}_{Z,\pi }p(Z,\pi |x_1,\dots ,x_N)$$
有
$$p(Z,\pi |x_1,\dots ,x_N)\propto p(Z,\pi )\prod _{n}p(x_n|Z,\pi )$$
作者证明，上式可以用$Gaussian\times Beta$分布来近似
$$q(Z,\pi |a_n,b_n,{\mu }_n,{\sigma }_n)\approx Beta(\pi |a_n,b_n)\cdot N(Z|{\mu }_n,{\sigma }_n)$$
并给出一个迭代格式
$$q(Z,\pi |a_n,b_n,{\mu }_n,{\sigma }_n)\approx p(x_n|Z,\pi )q(Z,\pi |a_{n-1},b_{n-1},{\mu }_{n-1},{\sigma }_{n-1})\text{\hspace{1em}(5)}$$
这里约等于是因为上式子右端并不是$Gaussian\times Beta$的分布，而是用$q(Z,\pi |a_n,b_n,{\mu }_n,{\sigma }_n)$去近似右端项。[1]实际上利用一阶矩和二阶矩相等来更新参数。根据上式，在加入新的测量时，seed的近似后验概率分布也会得到更新。当${\sigma }_n$小于给定阈值时，认为seed的深度估计已经收敛，计算其三维坐标，并加入地图。

近似分布的推导
[1]作者提供了文档$\text{Supplementary matterial Parametrix approximation to posterior}$。这里首先假设$p(Z,\pi )$满足独立性公式
$$p(Z,\pi )=p(Z)p(\pi )$$
第一步：引入潜变量(latent variable) $y_n$。$y_n=1$表示$x_n$是内点，$y_n=0$表示$x_n$是外点。那么有
$$p(x_n|Z,\pi ,y_n)=N(x_n|Z,{\tau }_n^2{)}^{y_n}U(x_n{)}^{1-y_n}$$
和
$$p(y_n|\pi )={\pi }^{y_n}(1-\pi {)}^{1-y_n}$$
容易证明
$$\begin{gathered} p(x_n|Z,\pi )=\frac{1}{p(Z,\pi )}\sum _{y_n}p(x_n,Z,\pi ,y_n)=\sum _{y_n}p(y_n|Z,\pi )p(x_n|Z,\pi ,y_n) \\ =\sum _{y_n}p(y_n|\pi )p(x_n|Z,\pi ,y_n) \end{gathered}$$
第二步，估计后验概率。令KaTeX parse error: Expected '}', got '\y' at position 32: …},Y={y_1,\dots,\̲y̲_n}，$X,Y,Z,\pi$的联合概率密度函数为
$$\begin{gathered} p(X,Y,Z,\pi )=\prod _{n}p(x_n|Z,\pi ,y_n)p(y_n|Z,\pi )p(Z|\pi )p(\pi ) \\ =[\prod _{n}p(x_n|Z,\pi ,y_n)p(y_n|\pi )]p(Z)p(\pi ) \end{gathered}$$
由于并不知道$p(Y,Z,\pi |X)$的具体分布。令$q(Y,Z,\pi )$是$p(Y,Z,\pi |X)$的一个近似推断，且满足
$$q(Y,Z,\pi )=q_1(Y)q_2(Z,\pi )$$
根据变分推断，求解$p(Y,Z,\pi |X)$的最佳近似分布等价于最小化$p,q$的Kullback-Leibler散度，由此推出$q_1,q_2$需要满足
$$\begin{gathered} \ln q_2=E_Y[\ln p(X,Y,Z，\pi )]+const \\ \ln q_1=E_{Z,\pi }[\ln p(X,Y,Z,\pi )]+const \end{gathered}$$
综上，可以进一步证明$q_2$满足$Gaussian\times Beta$分布。

近似分布的迭代求解
由于(5)右边并不满足$Gaussian\times Beta$分布，[1]尝试用另一个$Gaussian\times Beta$分布来近似右端项。令(5)式两端相对于$Z,\pi$的一阶矩和二阶矩相等，建立起参数方程，联立求解得到新的参数。
定义$Gaussian\times Beta$分布为
$$q(Z,\pi |a,b,\mu ,{\sigma }^2)=N(Z|\mu ,sigm{a}^2)Beta(\pi |a,b)$$
其中，
$$Beta(\pi |a,b)=\frac{\Gamma (a+b)}{\Gamma (a)\Gamma (b)}{\pi }^{a-1}(1-\pi {)}^{b-1}$$
根据式(5),我们想找到$q{(}^{\prime })=q(Z,\pi |a^{\prime },b^{\prime },{\mu }^{\prime },{\sigma }^{\prime 2})$使得其一阶矩和二阶矩与
$$p(x|Z,\pi )q(Z,\pi |a,b,\mu ,{\sigma }^2)$$
相等。将$p$的表达式代入上式，有
KaTeX parse error: Expected 'EOF', got '\piN' at position 2: (\̲p̲i̲N̲(x|Z,\tau^2)+(1…
注意到
$$\begin{gathered} \Gamma (a,b)=\frac{1}{\pi }\frac{a}{a+b}\Gamma (a+1,b)=\frac{1}{1-\pi }\frac{b}{a+b}\Gamma (a,b+1) \\ N(x|Z,{\tau }^2)N(Z|\mu ,{\sigma }^2)=N(x|\mu ,{\sigma }^2+{\tau }^2)N(Z|m,s^2) \end{gathered}$$
其中
$$\frac{1}{s^2}=\frac{1}{{\sigma }^2}+\frac{1}{{\tau }^2},m=s^2(\frac{\mu }{{\sigma }^2}+\frac{x}{{\tau }^2})$$
将上式代入(10)，就可以得到[1]补充文档(18)式，即
$$C_{1}N(Z|m,s^2)Beta(\pi |a+1,b)+C_{2}N(Z|\mu ,{\sigma }^2)Beta(\pi |a,b+1)\text{\hspace{1em}(11)}$$
其中，
$$C_1=\frac{a}{a+b}N(x|\mu ,{\sigma }^2+{\sigma }^2),C_2=\frac{a}{a+b}U(x)$$
分别计算$q{(}^{\prime })$和（11）关于$Z,\pi$的一阶矩和二阶矩，通过其分别相等得到$a^{\prime },b^{\prime },{\mu }^{\prime },{\sigma }^{\prime }$四个方程。

按照上述方程利用新的信息对概率分布的参数进行更新。
更加具体的推导参见深度滤波器详细解读。

当然，我们可以对深度分布建立其他模型，只要明确每个概率模型的更新过程即可，但是客观上这些深度满足什么分布，是受到很多因素影响的。