视觉SLAM总结——SVO中关键知识点总结

视觉SLAM总结——SVO中关键知识点总结

SVO是我接触的第一个视觉SLAM框架（按照高博的说法，这个是VO，算不上SLAM），当时和几个小伙伴一起读了源码相互交流下，但是当时没有及时总结，又忘得差不多了，这段时间正在复习，就借此机会一起总结下，复习的时候参看了几位SLAM大佬的笔记，觉得数学这个东西挖深了搞明白了真的很有意思，搞得我甚至有点想读个博…先放上几个SVO的参考博客：
svo： semi-direct visual odometry 论文解析
SVO原理解析
svo的Supplementary matterial 推导过程
能否具体解释下svo的运动估计与深度估计两方面？
下面就是我的总结，按照自问自答的形式总结，开始

1. 为什么叫半直接法视觉里程计？

所谓半直接指的是通过图像中的特征点图像块进行直接匹配来获取相机位姿，而不像直接法那样对整个图像（根据灰度梯度大小筛选需要匹配的点）使用直接匹配

2. SVO的优缺点是什么？

优点：
（1）速度非常快，在PC上能达到300帧，这也是直接法高效的最佳例证；
（2）关键点分布均匀。

缺点：
（1）没有重定位功能，追踪丢失后整个系统无法恢复，而且由于前端是通过比较前后两帧确定初始位姿，因此受单帧影响较大，在有遮挡模糊的情况下容易丢失；
（2）由于直接法算法本身所导致的缺点是：模糊、大运动、光照变化大的情况下容易丢失；
（3）深度滤波器收敛比较慢，而且VO的结果严重依赖于准确的位姿估计。

3. SVO的整体框架是怎样的？

如下图，SVO一共有两个线程，分别是追踪和建图：
追踪线程：
（1）Sparse Model-based Image Aligment：把当前帧和上一个追踪的帧进行稀疏直接法，获取粗略的位姿。
（2）Feature Aligment：把关键帧地图点投影到当前帧，并利用光流法来找到地图中特征块在new frame中应该出现的位置进行像素块的匹配。
（3）Pose & Structure Refinement：通过匹配好的像素块，计算当前帧和地图点对应的关键帧之间的光度误差，通过BA同时优化位姿和地图点坐标。

建图线程：
建图过程最主要的任务就是估计特征点深度，判断追踪线程传来的帧是否为关键帧，如果为关键帧就提取新的特征点，把这些点作为地图的种子点，放入优化线程。否则，不为关键帧的时候，就用此帧的信息更新地图中种子点的深度估计值。

4. 分别介绍下直接法和光流法，有什么区别？

区别：光流法是通过灰度不变假设跟踪特征点的运动，节省了计算特征描述子的时间，但是计算三维变换仍需要通过对极几何、PnP或者ICP，分为稀疏光流法和稠密光流法，而直接法是通过最小化光度误差直接计算三维变换，分为稀疏直接法（特征点），半稠密直接法（梯度大的点），稠密直接法（需要GPU）。

光流法：光流法是基于灰度不变假设（同一个空间点的像素灰度值，在各个图像中是固定不变的）进行泰勒展开，如下$$\mathbf{I}(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t)=\mathbf{I}(x,y,t)$$然后进行泰勒展开$$\mathbf{I}(x+\mathrm{d}x,y+\mathrm{d}y,t+\mathrm{d}t)\approx \mathbf{I}(x,y,t)+\frac{\partial \mathbf{I}}{\partial x}\mathrm{d}x+\frac{\partial \mathbf{I}}{\partial y}\mathrm{d}y+\frac{\partial \mathbf{I}}{\partial t}\mathrm{d}t$$灰度不变嘛，有$$\frac{\partial \mathbf{I}}{\partial x}\mathrm{d}x+\frac{\partial \mathbf{I}}{\partial y}\mathrm{d}y+\frac{\partial \mathbf{I}}{\partial t}\mathrm{d}t=0$$两边除以$dt$，有$$\frac{\partial \mathbf{I}}{\partial x}\frac{\mathrm{d}x}{\mathrm{d}t}+\frac{\partial \mathbf{I}}{\partial y}\frac{\mathrm{d}y}{\mathrm{d}t}=-\frac{\partial \mathbf{I}}{\partial t}$$$\mathrm{d}x/\mathrm{d}t$和$\mathrm{d}y/\mathrm{d}t$分别是像素在$x$和$y$方向上的速度$u,v$而$\partial \mathbf{I}/\partial x$和$\partial \mathbf{I}/\partial y$分别是像素在$x$和$y$方向上的梯度${\mathbf{I}}_x,{\mathbf{I}}_y$，于是有$$\left[\begin{array}{ll}{\mathbf{I}}_x& {\mathbf{I}}_y\end{array}\right]\left[\begin{array}{l}u\\ v\end{array}\right]=-{\mathbf{I}}_t$$我们已知的是${\mathbf{I}}_x,{\mathbf{I}}_y$，而要求的是$u,v$，一个像素点当然求不了，因此，光流法考虑的是一个$w\times w$大小的窗口，因此总共有$w^2$个方程，$${\left[\begin{array}{ll}{\mathbf{I}}_x& {\mathbf{I}}_y\end{array}\right]}_k\left[\begin{array}{c}u\\ v\end{array}\right]=-{\mathbf{I}}_{tk},k=1,\dots ,w^2$$记$$A=\left[\begin{array}{c}{\left[{\mathbf{I}}_x,{\mathbf{I}}_y\right]}_1\\ ⋮\\ {\left[{\mathbf{I}}_x,{\mathbf{I}}_y\right]}_k\end{array}\right],\mathbf{b}=\left[\begin{array}{c}{\mathbf{I}}_{t1}\\ ⋮\\ {\mathbf{I}}_{tk}\end{array}\right]$$采用最小二乘法求解$${\left[\begin{array}{c}u\\ v\end{array}\right]}^{\ast }=-{\left({\mathbf{A}}^T\mathbf{A}\right)}^{-1}{\mathbf{A}}^T\mathbf{b}$$ 这样就求得了像素块在下一帧的位置

直接法：直接法是基于灰度不变假设建立一个优化问题，优化目标是最小化光度误差：$$\underset{\xi }{\min }J(\xi )=\Vert e{\Vert }^2$$其中$$e={\mathbf{I}}_1\left({\mathbf{p}}_1\right)-{\mathbf{I}}_2\left({\mathbf{p}}_2\right)$$而$$\begin{array}{rl}{\mathbf{p}}_1& ={\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]}_1=\frac{1}{Z_1}\mathbf{K}\mathbf{P}\\ {\mathbf{p}}_2=& {\left[\begin{array}{c}u\\ v\\ v\\ 1\end{array}\right]}_2=\frac{1}{Z_2}\mathbf{K}(\mathbf{R}\mathbf{P}+\mathbf{t})=\frac{1}{Z_2}\mathbf{K}{\left(\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}\right)}_{1:3}\end{array}$$可以由下图直观理解
对于非线性的最优化问题，我们最关心的当然是，残差相对于位姿的导数，这里使用李代数上的扰动模型：$$\begin{array}{rl}e(\mathbf{\xi }\oplus \delta \mathbf{\xi })& ={\mathbf{I}}_1\left(\frac{1}{Z_1}\mathbf{K}\mathbf{P}\right)-{\mathbf{I}}_2\left(\frac{1}{Z_2}\mathbf{K}\exp \left(\delta {\xi }^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}\right)\\ & \approx {\mathbf{I}}_1\left(\frac{1}{Z_1}\mathbf{K}\mathbf{P}\right)-{\mathbf{I}}_2\left(\frac{1}{Z_2}\mathbf{K}\left(1+\delta {\xi }^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}\right)\\ & ={\mathbf{I}}_1\left(\frac{1}{Z_1}\mathbf{K}\mathbf{P}\right)-{\mathbf{I}}_2\left(\frac{1}{Z_2}\mathbf{K}\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}+\frac{1}{Z_2}\mathbf{K}\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}\right)\end{array}$$记$$\begin{array}{rl}\mathbf{q}& =\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}\\ \mathbf{u}& =\frac{1}{Z_2}\mathbf{K}\mathbf{q}\end{array}$$$q$为空间点在绕动后位于第二个相机坐标系下的坐标，$u$是它在第二个相机下的像素坐标，我们通过一阶泰勒展开获得其雅可比矩阵$$\begin{array}{rl}e(\mathbf{\xi }\oplus \delta \mathbf{\xi })& ={\mathbf{I}}_1\left(\frac{1}{Z_1}\mathbf{K}\mathbf{P}\right)-{\mathbf{I}}_2\left(\frac{1}{Z_2}\mathbf{K}\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}+\mathbf{u}\right)\\ & \approx {\mathbf{I}}_1\left(\frac{1}{Z_1}\mathbf{K}\mathbf{P}\right)-{\mathbf{I}}_2\left(\frac{1}{Z_2}\mathbf{K}\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{P}\right)-\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \mathbf{q}}\frac{\partial \mathbf{q}}{\partial \delta \xi }\delta \mathbf{\xi }\\ & =e(\mathbf{\xi })-\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \mathbf{q}}\frac{\partial \mathbf{q}}{\partial \delta \xi }\delta \mathbf{\xi }\end{array}$$其中$\frac{\partial I_2}{\partial u}\frac{\partial u}{\partial \mathbf{q}}\frac{\partial \mathbf{q}}{\partial \delta \mathbf{\xi }}$就是我们需要求得的雅可比矩阵$\mathbf{J}$的链式求导形式，我们分别分析其中的没一项就可以获得其最终形式
（1）$\partial I_2/{\partial }_u$为像素$u$处的像素梯度;
（2）$\partial u/\partial q$为投影方程关于相机坐标系下的三位点的导数$$\frac{\partial \mathbf{u}}{\partial \mathbf{q}}=\left[\begin{array}{ccc}\frac{\partial u}{\partial X}& \frac{\partial u}{\partial Y}& \frac{\partial u}{\partial Z}\\ \frac{\partial v}{\partial X}& \frac{\partial v}{\partial Y}& \frac{\partial v}{\partial Z}\end{array}\right]=\left[\begin{array}{ccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}\end{array}\right]$$（3）$\partial \mathbf{q}/\partial \delta \mathbf{\xi }$为三维点对位姿变换的导数$$\frac{\partial {\mathbf{P}}^{\prime }}{\partial \delta \mathbf{\xi }}=\left[\mathbf{I},-{\mathbf{P}}^{\prime \prime }\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& Z^{\prime }& -Y^{\prime }\\ 0& 1& 0& Z^{\prime }& 0& X^{\prime }\\ 0& 0& 1& Y^{\prime }& -X^{\prime }& 0\end{array}\right]$$因此有雅可比矩阵$$\mathbf{J}=-\frac{\partial {\mathbf{I}}_2}{\partial \mathbf{u}}\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}$$而$$\frac{\partial \mathbf{u}}{\partial \delta \mathbf{\xi }}=\left[\begin{array}{cccccc}\frac{f_x}{Z}& 0& -\frac{f_{x}X}{Z^2}& -\frac{f_{x}XY}{Z^2}& f_x+\frac{f_x{X}^2}{Z^2}& -\frac{f_{x}Y}{Z}\\ 0& \frac{f_y}{Z}& -\frac{f_{y}Y}{Z^2}& -f_y-\frac{f_y{Y}^2}{Z^2}& \frac{f_{y}XY}{Z^2}& \frac{f_{y}X}{Z}\end{array}\right]$$然后用高斯牛顿或者列文伯格进行迭代求解就好

5. 介绍下SVO追踪线程中用到的算法

SVO的追踪线程中应用的是稀疏直接法（Sparse Model based Image Alignment）和光流法（Feature Alignment），还有BA优化（Pose & Structure Refinement），
（1）在Sparse Model based Image Alignment步骤中，假设相邻帧之间的位姿已知（初始化为上一帧位姿或者单位矩阵），通过直接法求得帧间变换矩阵

（2）在Feature Alignment步骤中，因为通过直接法估计的位姿是有误差的，因此会投影像素会存在下图误差
灰框表示真实匹配的像素块位置，而蓝框表示通过变换矩阵投影获得的像素快预测位置，两者之间是存在误差的，此步骤就是通过光流法将像素位置调整到真实皮匹配的像素块位置。注意这里和上面讲的光流法有一点不同的是，因为这里的像素块不是帧间的像素块，而是当前帧和地图点对应帧的像素块，在空间上他们可能距离很远，因此需要在这个光流法中考虑一个仿射变换，如下$$u_i^{\prime }=\arg \underset{u_i^{\prime }}{\min }\frac{1}{2}{\Vert I_k\left(u_i^{\prime }\right)-A_i{I}_r\left(u_i\right)\Vert }^2\forall i$$其中$A_i$就是那个仿射变换

（3）在Pose & Structure Refinement步骤中，因为像素块位置被调整了，因此Bundle Adjustment可以进一步优化相机位姿和空间的三维坐标，这一步优化的效果是有限的，因此程序中是否运行这一部分是可选的。

6. SVO判断是否为关键帧的准则是什么？

SVO的关键帧判断标准现对于ORB SLAM2要薄弱很多，如果当前帧和它相关联的所有关键帧之间的相对平移都超多了场景平均深度的12%就判定为关键帧

7. SVO中是否有重定位功能？

有的！不过很简单，基本没什么用，其重定位功能如下：
启动重定位操作的条件是（参考视觉SLAM——SVO代码(1)主要流程）：
（1）位姿变化过大，导致可用于跟踪的特征点数太少。
（2）重投影以后，利用多关键帧和当前帧的匹配点进行相机位姿优化的过程中，不断丢弃重投影误差大的点。优化完以后，如果inlier数目较少，跟踪失败，开启重定位。
重定位操作的实现很简单，就是在跟丢之后，仍然假设当前帧的位姿和前一帧一样，往这个位姿上投地图点，用光流法和BA`去优化计算，如果优化成功，就重定位回来，如果优化不成功，就继续下一帧。所以，在跟丢后，只能再回到跟丢时的位置，才能重定位回来。

8. SVO中初始化是怎样完成的？

图像刚进来的时候先获取其金字塔图像，然后对第一帧检测FAST特征点和边缘特征，如果图像中间的特征点数量超过50个，就把这张图像作为第一个关键帧，然后获取第一帧后面的连续图像，用于和第一帧进行三角初始化，规则是：如果后面帧中匹配点的数量大于阈值，并且视差的中位数大于阈值。如果视差的方差大的话，选择计算$E$矩阵，如果视差的方差小的话，选择计算$H$矩阵。如果计算完$H$和$E$矩阵之后，还有足够的内点，就认为这帧是合适三角化的帧，根据$H$或$E$矩阵恢复出来的位姿和种子点（深度还没有收敛的空间点），将这一帧作为关键帧并将种子点送入深度滤波器中，后面就开始执行正常的追踪线程。

9. 什么是深度滤波器？（重要！看这里！）

单目最基本的估计深度的方法就是三角化，而基于三角化，我们通过概率的方式表示这个问题，就得到了深度滤波器，关于三角化和基本深度滤波器的总结可以参看我的博客多视图几何总结——三角形法，SVO中不太一样的是，其深度概率模型是一个高斯分布和一个均匀分布$$p(x|Z,\pi )=\pi N\left(x|Z,{\tau }^2\right)+(1-\pi )U\left(x|Z_{\min },Z_{\max }\right)$$这也就导致了其更新方法没有我三角形法里写的那么简单，下面我根据几篇参考博客（主要参考SVO原理解析）对SVO的深度滤波器做一个分析总结
在SVO运行的过程中，凡是没有被选作关键帧的图像帧都会用来对空间点的深度进行更新，因此对于同一个空间点我们可以得到一些列的观测$x_1,x_2,\dots ,x_n$（这些观测可以理解为图像坐标），然后我们就需要根据这些观测估计我们深度概率模型的参数$$\arg \underset{Z,\pi }{\max }p\left(Z,\pi |x_1,\dots ,x_N\right)$$SVO中采用最大后验概率对求解上述问题（作者认为最大似然概率容易被极大值干扰）$$p\left(Z,\pi |x_1,\dots ,x_N\right)\propto p(Z,\pi )\prod _{n}p\left(x_n|Z,\pi \right)$$上式可以用Gaussian-Beta分布近似$$q\left(Z,\pi |a_n,b_n,{\mu }_n,{\sigma }_n\right)\triangleq \mathrm{Beta}\left(\pi |a_n,b_n\right)\cdot N\left(Z|{\mu }_n,{\sigma }_n\right)$$其迭代公式为$$q\left(Z,\pi |a_n,b_n,{\mu }_n,{\sigma }_n\right)\approx p\left(x_n|Z,\pi \right)q\left(Z,\pi |a_{n-1},b_{n-1},{\mu }_{n-1},{\sigma }_{n-1}\right)$$这里会有两个问题，为嘛是Gaussian-Beta分布近似？迭代公式具体操作是怎么样？
关于第一个问题，我没去细究，可以参考Supplementary matterial Parametric approximation to posterior这篇文章，我这里主要研究了下第二个问题，给了这样一个分布迭代公式，我们具体怎么操作，代码怎么写（多公式预警！！！）
定义Gaussian-Beta分布如下：$$q\left(Z,\pi |a,b,\mu ,{\sigma }^2\right)=N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b)$$其中$$N\left(x|\mu ,{\sigma }^2\right)=\frac{1}{{\left(2\pi {\sigma }^2\right)}^{1/2}}\exp \left\{-\frac{1}{2{\sigma }^2}(x-\mu {)}^2\right\}$$$$\mathrm{Beta}(\pi |a,b)=\frac{\Gamma (a+b)}{\Gamma (a)\Gamma (b)}{\pi }^{a-1}(1-\pi {)}^{b-1}$$其中$\Gamma (a)=(a-1)!$，这里我们需要先说明一下这两个分布的接下来推导要用到的两个性质：
关于Gaussian分布有：$$N\left(x|Z,{\tau }^2\right)N\left(Z|\mu ,{\sigma }^2\right)=N\left(x|\mu ,{\sigma }^2+{\tau }^2\right)N\left(Z|m,s^2\right)$$其中$$\frac{1}{s^2}=\frac{1}{{\sigma }^2}+\frac{1}{{\tau }^2}m=s^2\left(\frac{\mu }{{\sigma }^2}+\frac{x}{{\tau }^2}\right)$$这个推到有点复杂，可以参考svo的Supplementary matterial 推导过程
关于Beta分布有：$$\mathrm{Beta}(\pi |a,b)=\frac{1}{\pi }\frac{a}{a+b}\mathrm{Beta}(\pi |a+1,b)=\frac{1}{1-\pi }\frac{b}{a+b}\mathrm{Beta}(\pi |a,b+1)$$这个可以由$\Gamma (a)$的定义Beta分布的定义很容易推出（这里我和原本博客的公式不同，我觉得应该是这样）
--------我是华丽的分割线--------
上面说了各种分布的定义，下面我们来正式研究下递推过程是怎样的
由前文，我们的迭代公式如下$$p(x|Z,\pi )q\left(Z,\pi |a,b,\mu ,{\sigma }^2\right)$$其中$p(x|Z,\pi )$是高斯分布和均匀分布的组合，$q\left(Z,\pi |a,b,\mu ,{\sigma }^2\right)$是Gaussian-Beta分布，相乘之后不再是Gaussian-Beta分布，因此我们假定这样一个Gaussian-Beta分布$q\left(Z,\pi |a^{\prime },b^{\prime },{\mu }^{\prime },{\sigma }^{\prime 2}\right)$，我们分别求已知分布和假定分布的一阶矩和二阶矩，使其相等求得$a^{\prime },b^{\prime },{\mu }^{\prime },{\sigma }^{\prime 2}$就获得了一个更新后的Gaussian-Beta分布，好，顺着这个思路开始吧！

将$p(x|Z,\pi )$和$q\left(Z,\pi |a,b,\mu ,{\sigma }^2\right)$的公式带入得$$\begin{array}{rl}& \left(\pi N\left(x|Z,{\tau }^2\right)\right)+(1-\pi )U(x))N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b)\\ =& \pi N\left(x|Z,{\tau }^2\right)N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b)+(1-\pi )U(x)N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b)\\ =& \frac{a}{a+b}N\left(Z|\mu ,{\sigma }^2\right)N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a+1,b)+\frac{b}{a+b}U(x)N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b+1)\\ =& \frac{a}{a+b}N\left(x|\mu ,{\sigma }^2+{\tau }^2\right)N\left(Z|m,s^2\right)\mathrm{Beta}(\pi |a+1,b)+\frac{b}{a+b}U(x)N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b+1)\end{array}$$这里就用到了我们刚刚提到的关于Gaussian分布和Beta分布的两个性质。
现在我们等式左边（假定分布）是$$q\left(Z,\pi |a^{\prime },b^{\prime },{\mu }^{\prime },{\sigma }^2\right)$$等式右边（已知分布）是$$C_{1}N\left(Z|m,s^2\right)\mathrm{Beta}(\pi |a+1,b)+C_{2}N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b+1)$$其中$$\begin{array}{rl}{C}_1& =\frac{a}{a+b}N\left(x|\mu ,{\sigma }^2+{\tau }^2\right)\\ C_2& =\frac{b}{a+b}U(x)\end{array}$$
现在我们先来求左边（假定分布）的一、二节矩：
（1）关于$Z$的一阶矩：$$\begin{array}{rl}\int Zp\left(Z,\pi |a^{\prime },b^{\prime },{\mu }^{\prime },{\sigma }^{\prime }\right)dZd\pi & =\int ZN\left(Z|{\mu }^{\prime },{\sigma }^{\prime 2}\right)\mathrm{Beta}\left(\pi |a^{\prime },b^{\prime }\right)dZd\pi \\ & =\int \mathrm{ZN}\left(Z|{\mu }^{\prime },{\sigma }^{\prime 2}\right)dZ\\ & ={\mu }^{\prime }\end{array}$$（2）关于$Z$的二阶矩：$$\begin{array}{rl}\int Z^{2}p\left(Z,\pi |a^{\prime },b^{\prime },b^{\prime },{\sigma }^{\prime }\right)dZd\pi & =\int Z^{2}N\left(Z|{\mu }^{\prime },{\sigma }^{\prime 2}\right)\mathrm{Beta}\left(\pi |a^{\prime },b^{\prime }\right)dZd\pi \\ & =\int Z^{2}N\left(Z|{\mu }^{\prime },{\sigma }^{\prime 2}\right)dZ\\ & ={\mu }^{\prime 2}+{\sigma }^{\prime 2}\end{array}$$（3）关于$\pi$的一阶矩：$$\begin{array}{rl}\int \pi p\left(Z,\pi |a^{\prime },b^{\prime },{\mu }^{\prime },{\sigma }^{\prime }\right)dZd\pi & =\int \pi N\left(Z|{\mu }^{\prime },{\sigma }^{\prime 2}\right)\mathrm{Beta}\left(\pi |a^{\prime },b^{\prime }\right)dZd\pi \\ & =\int \pi \mathrm{Beta}\left(\pi |a^{\prime },b^{\prime }\right)d\pi \\ & =\frac{a^{\prime }}{a^{\prime }+b^{\prime }}\end{array}$$（4）关于$\pi$的二阶矩：$$\begin{array}{rl}\int {\pi }^{2}p\left(Z,\pi |a^{\prime },b^{\prime },{\mu }^{\prime },{\sigma }^{\prime }\right)dZd\pi & =\int {\pi }^{2}N\left(Z|{\mu }^{\prime },{\sigma }^{\prime 2}\right)\mathrm{Beta}\left(\pi |a^{\prime },b^{\prime }\right)dZd\pi \\ & =\int {\pi }^2\mathrm{Beta}\left(\pi |a^{\prime },b^{\prime }\right)d\pi \\ & =\frac{a^{\prime }\left(a^{\prime }+1\right)}{\left(a^{\prime }+b^{\prime }\right)\left(a^{\prime }+b^{\prime }+1\right)}\end{array}$$

然后我们再来求右边（已知分布）的一、二节矩：
首先对自己的概率密度积分：$$\begin{array}{rl}C& =\int \frac{a}{a+b}N\left(x|\mu ,{\sigma }^2+{\tau }^2\right)N\left(Z|m,s^2\right)\mathrm{Beta}(\pi |a+1,b)+\frac{b}{a+b}U(x)N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b+1)dZd\pi \\ & =\int C_{1}N\left(Z|m,s^2\right)\mathrm{Bet}a(\pi |a+1,b)+C_{2}N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b+1)dZd\pi \\ & =C_1+C_2\end{array}$$
（1）关于$Z$的一阶矩：$$\begin{array}{rl}& \frac{1}{C}\int Z\left\{C_{1}N\left(Z|m,s^2\right)\mathrm{Beta}(\pi |a+1,b)+C_{2}N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b+1)\right\}dZd\pi \\ =& \frac{1}{C}\int Z\left\{C_{1}N\left(Z|m,s^2\right)+C_{2}N\left(Z|\mu ,{\sigma }^2\right)\right\}dZd\pi \\ =& \frac{1}{C}\left\{C_{1}m+C_2\mu \right\}\end{array}$$（2）关于$Z$的二阶矩：$$\begin{array}{rl}& \frac{1}{C}\int Z^2\left\{C_{1}N\left(Z|m,s^2\right)\mathrm{Beta}(\pi |a+1,b)+C_{2}N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b+1)\right\}dZd\pi \\ =& \frac{1}{C}\int Z^2\left\{C_{1}N\left(Z|m,s^2\right)+C_{2}N\left(Z|\mu ,{\sigma }^2\right)\right\}dZd\pi \\ =& \frac{1}{C}\left\{C_1\left(m^2+s^2\right)+C_2\left({\mu }^2+{\sigma }^2\right)\right\}\end{array}$$（3）关于$\pi$的一阶矩：$$\begin{array}{rl}& \frac{1}{C}\int \pi \left\{C_{1}N\left(Z|m,s^2\right)\mathrm{Beta}(\pi |a+1,b)+C_{2}N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b+1)\right\}dZd\pi \\ =& \frac{1}{C}\int \pi \left\{C_1\mathrm{Beta}(\pi |a+1,b)+C_2\mathrm{Beta}(\pi |a,b+1)\right\}dZd\pi \\ =& \frac{1}{C}\left\{C_1\frac{a+1}{a+b+1}+C_2\frac{a}{a+b+1}\right\}\end{array}$$（4）关于$\pi$的二阶矩：$$\begin{array}{rl}& \frac{1}{C}\int {\pi }^2\left\{C_{1}N\left(Z|m,s^2\right)\mathrm{Beta}(\pi |a+1,b)+C_{2}N\left(Z|\mu ,{\sigma }^2\right)\mathrm{Beta}(\pi |a,b+1)\right\}dZd\pi \\ =& \frac{1}{C}\int {\pi }^2\left\{C_1\mathrm{Bet}a(\pi |a+1,b)+C_2\mathrm{Beta}(\pi |a,b+1)\right\}dZd\pi \\ =& \frac{1}{C}\left\{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)}\right\}\end{array}$$对$C_1$和$C_2$进行归一化：$$C_2^{\prime }=\frac{C_2}{C}$$$$C_2^{\prime }=\frac{C_2}{C}$$让上面的一二阶矩相等（上面的（1）（2）（3）（4）分别相等）有$$\begin{array}{rl}{\mu }^{\prime }& =C_1^{\prime }m+C_2^{\prime }\mu \\ {\mu }^{\prime 2}+{\sigma }^{\prime 2}& =C_1^{\prime }\left(m^2+s^2\right)+C_2^{\prime }\left({\mu }^2+{\sigma }^2\right)\\ \frac{a_n^{\prime }}{a_n^{\prime }+b_n^{\prime }}& =C_1^{\prime }\frac{a+1}{a+b+1}+C_2^{\prime }\frac{a}{a+b+1}\\ \frac{a_n^{\prime }\left(a_n^{\prime }+1\right)}{\left(a_n^{\prime }+b_n^{\prime }\right)\left(a_n^{\prime }+b_n^{\prime }+1\right)}& =C_1^{\prime }\frac{(a+1)(a+2)}{(a+b+1)(a+b+2)}+C_2^{\prime }\frac{a(a+1)}{(a+b+1)(a+b+2)}\end{array}$$然后联立求解就好了，根据第一个等式我们可以直接获得${\mu }^{\prime }$，然后带入第二个等式就可以解得${\sigma }^{\prime }$，联立后面两个等式就可以求得$a^{\prime }$和$b‘$，如下：
令$$e=C_1\frac{a+1}{a+b+1}+C_2\frac{a}{a+b+1}$$$$f=C_1^{\prime }\frac{(a+1)(a+2)}{(a+b+1)(a+b+2)}+C_2^{\prime }\frac{a(a+1)}{(a+b+1)(a+b+2)}$$那么$$a^{\prime }=\frac{e-f}{f-e/f}{b}^{\prime }=\frac{1-f}{f}{a}^{\prime }$$
在svo的Supplementary matterial 推导过程中还有这样一段源代码，我觉得看这个会更加促进对深度滤波器的理解，我就搬运至此了，需要看源码的最好还是回源博客看。

void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
{
  // 合成正态分布的标准差
  float norm_scale = sqrt(seed->sigma2 + tau2);
  if(std::isnan(norm_scale))
    return;
  // 正态分布
  boost::math::normal_distribution nd(seed->mu, norm_scale);
  // 公式(19)
  float s2 = 1./(1./seed->sigma2 + 1./tau2);
  // 公式(20)计算m.
  float m = s2*(seed->mu/seed->sigma2 + x/tau2);
  // 公式(21)计算C1.
  float C1 = seed->a/(seed->a+seed->b) * boost::math::pdf(nd, x);
  // 公式(22)计算C2。
  float C2 = seed->b/(seed->a+seed->b) * 1./seed->z_range;
  // 概率密度函数归一化
  float normalization_constant = C1 + C2;
  C1 /= normalization_constant;
  C2 /= normalization_constant;
  // 公式(25)
  float f = C1*(seed->a+1.)/(seed->a+seed->b+1.) + C2*seed->a/(seed->a+seed->b+1.);
  // 公式(26)
  float e = C1*(seed->a+1.)*(seed->a+2.)/((seed->a+seed->b+1.)*(seed->a+seed->b+2.))
          + C2*seed->a*(seed->a+1.0f)/((seed->a+seed->b+1.0f)*(seed->a+seed->b+2.0f));

  // update parameters.
  // 公式(23)
  float mu_new = C1*m+C2*seed->mu;
  // 公式(24)变形
  seed->sigma2 = C1*(s2 + m*m) + C2*(seed->sigma2 + seed->mu*seed->mu) - mu_new*mu_new;
  seed->mu = mu_new;
  // 公式(25)(26)联立求a'
  seed->a = (e-f)/(f-e/f);
  // 公式(25)求b'
  seed->b = seed->a*(1.0f-f)/f;
}
--------------------- 
作者：可爱的小蚂蚁 
来源：CSDN 
原文：https://blog.csdn.net/u013004597/article/details/52069741 
版权声明：本文为博主原创文章，转载请附上博文链接！

以为到这里就结束啦？还没有完呢，深度滤波器的输入是什么？是$x$和$\tau$，其中$x$指的是通过匹配的像素确定的深度的倒数（逆深度），$\tau$一个像素的偏差所导致的深度的偏差，如下图所示

这里的求解就是高中数学知识了，上图中这几个变量的关系是$$\begin{array}{rl}\mathbf{a}& =\mathbf{p}-\mathbf{t}\\ \alpha & =\arccos ⟨\mathbf{p},\mathbf{t}⟩\\ \beta & =\arccos ⟨\mathbf{a},-\mathbf{t}⟩\end{array}$$对$p_2$进行一个像素的扰动有$$\delta \beta =\arctan \frac{1}{f}$$因此有$$\begin{array}{l}{\beta }^{\prime }=\beta +\delta \beta \\ \gamma =\pi -\alpha -{\beta }^{\prime }\end{array}$$由正弦定理可以求得$\mathbf{p}’$的大小有$$\Vert {\mathbf{p}}^{\prime }\Vert =\Vert \mathbf{t}\Vert \frac{\sin {\beta }^{\prime }}{\sin \gamma }$$所以有$$\tau =\Vert \mathbf{p}\Vert -\Vert {\mathbf{p}}^{\prime }\Vert$$
到这里数学层面上的问题就都解决了，我们可以从整体上再来总结下，对于建图线程，工作逻辑如下：
（1）如果进来一个关键帧，就提取关键帧上的新特征点，作为种子点放进一个种子队列中。
（2）如果进来一个普通帧，就用普通帧的信息，更新所有种子点的概率分布。如果某个种子点的深度分布已经收敛，就把它放到地图中，供追踪线程使用。
而我们统购不同真的信息跟新种子点的概率分布用到的就是我们上面所推导的深度滤波器，注意哦，是一个点对应一个深度滤波器～。

如有错误，欢迎指出～

此外，对其他SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结