深度滤波器

SLAM

---

3 深度滤波器(Depth Filter)

  SVO深度滤波器的思想最初是在文献[2]中提到，然后SVO的作者那它做了单目VO中的深度估计，文献[3]中SVO的作者是二作，REMODE的位姿估计使用的就是SVO，做了单目的重建，深度估计也用的是深度滤波器的思想。深度滤波器的算法步骤比较简单，但是中间涉及到的概率模型或者说是概率模型的推导较为复杂(可能是概率论学的不好。。。)。整个算法的步骤参考别人的博客可以简单的总结为以下流程图。

图3 深度滤波器流程图

1. 建立深度滤波器的概率模型

  设有一系列的深度测量值$d_{k}$ ($k=r,\cdots,r+n$ )。假设深度测量的模型是由一个服从高斯分布的好模型和一个服从均匀分布的块模型组成的混合模型，将深度测量模型表示为，

$$\nonumber p(d_{k}|\hat{d},\rho)=\rho N(d_{k}|\hat{d},\tau^{2}_{k})+(1-\rho)U(d_{k}|d_{min},d_{max}) \tag{3.1}$$
其中$N(d_{k}|\hat{d},\tau^{2}_{k})$为以真值$\hat{d}$为中心的高斯分布，$\tau^{2}_{k}$ 为其方差。$\rho$ 为该测量值属于好模型的概率。$d_{min}、d_{max}$为均匀分布的上下限。其中，认为$\hat{d}$ 和 $\rho$ 是独立分布的，即满足，
$$\nonumber p(\hat{d},\rho)=p(\hat{d})p(\rho) \tag{3.2}$$
对每次的深度测量打上标签$y_{k}$ ($k=r,\cdots,r+n$ )，则将式(1)中的概率模型改写为，
$$\nonumber p(d_{k}|\hat{d},\rho,y_{k})=N(d_{k}|\hat{d},\tau^{2}_{k})^{y_{k}}U(d_{k})^{1-y_{k}} \tag{3.3}$$
当$y_{k}=1$ 是，则第$k$ 次测量是一个内点，反之则为一个外点。其中，
$$\nonumber p(y_{k}|\rho)=\rho^{y_{k}}(1-\rho)^{1-y_{k}} \tag{3.4}$$
依据概率论中的乘法定理，进一步将式(3)写为，
$$\nonumber p(XY,\hat{d},\rho)=[\prod^{n}_{k=r}p(d_{k}|\hat{d},\rho,y_{k})p(y_{k}|\rho)]p(\hat{d})p(\rho) \tag{3.5}$$
对上式的简要推导，

这里默认式(3.5)中的右半部分满足贝叶斯的条件独立性假设，参考统计学方法48页，公式(4.3)

$$\begin{align} \nonumber &[\prod^{n}_{k=r}p(d_{k}|\hat{d},\rho,y_{k})p(y_{k}|\rho)]p(\hat{d})p(\rho) \\ \nonumber &=\prod^{n}_{k=r}p(d_{k}|\hat{d},\rho,y_{k})\prod^{n}_{k=r}p(y_{k}|\rho)p(\rho)p(\hat{d}) \\ \nonumber &=\prod^{n}_{k=r}p(d_{k}|\hat{d},\rho,y_{k})p(Y,\rho)p(\hat{d}) \\ \nonumber &=p(XY,\rho|\hat{d})p(\hat{d}) \\ \nonumber &=p(XY,\hat{d},\rho) \end{align}$$

  设存在后验估计$p(Y,\hat{d},\rho|X)$的一个近似解$q(Y,\hat{d},\rho)$，且满足

$$\nonumber q(Y,\hat{d},\rho)=q_{Y}(Y)q_{\hat{d},\rho}(\hat{d},\rho) \tag{3.6}$$
当$p(Y,\hat{d},\rho|X)$ 与$q(Y,\hat{d},\rho)$ 的KL散度(Kullback Leibler divergence)最小的时候，满足以下条件：
$$\nonumber ln\,q_{\hat{d},\rho}(\hat{d},\rho)=E_{y}[ln\,p(X,Y,\hat{d},\rho)]+const \tag{3.7}$$
$$\nonumber ln\,q_{Y}(\hat{d},\rho)=E_{\hat{d},\rho}[ln\,p(X,Y,\hat{d},\rho)]+const \tag{3.8}$$
其中上式中的两个期望值是针对$q_{Y}(Y)$ 和$q(Y,\hat{d},\rho)$ 来说的。
  对式(3.7)进行进一步的变换，
$$\begin{align} \nonumber ln\,q_{\hat{d},\rho}(\hat{d},\rho)&=E_{y}[ln\,([\prod^{n}_{k=r}p(d_{k}|\hat{d},\rho,y_{k})p(y_{k}|\rho)]p(\hat{d})p(\rho)]+const) \\ \nonumber &=E_{y}[ln\, \prod^{n}_{k=r}(N(d_{k}|\hat{d},\tau^{2}_{k})^{y_{k}})+ln\,\prod^{n}_{k=r}(U(d_{k})^{1-y_{k}}) \\ \nonumber &\quad+ln\,\prod^{n}_{k=r}(\rho^{y_{k}})+ln\,\prod^{n}_{k=r}((1-\rho)^{1-y_{k}})\\ \nonumber &\quad+ln\,p(\hat{d})+ln\,p(\rho)]+const \\ \nonumber &=\sum^{n}_{k=r}E_{y}[y{k}](ln\,N(d_{k}|\hat{d},\tau^{2}_{k})+\ln\,\rho) \\ \nonumber &\quad+\sum^{n}_{k=r}(1-E_{y}[y{k}])(ln\,U(d_{k})+ln\,(1-\rho)) \\ \nonumber &\quad+ln\,p(\hat{d})+ln\,p(\rho)]+const \end{align} \tag{3.9}$$
对式(3.9)两边去对数符号，得
$$q_{\hat{d},\rho}(\hat{d},\rho) = [\prod^{n}_{k=r}N(d_{k}|\hat{d},\tau^{2}_{k})^{r_{n}}]\rho^{S}(1-\rho)^{N-S}p(\hat{d})p(\rho) \tag{3.10}$$
其中，
$$r_{n}=E_{y}[y_{k}],\qquad S=\sum^{n}_{k=r}r_{n}$$
均匀分布的U是怎么消失的
共轭先验到近似到$Gaussion\times Beta$ 的过程

2.更新后验估计的参数

  将真实的后验分布近似表达为，

$$\nonumber q(\hat{d},\rho|a,b,\mu,\delta^{2}):=N(Z|\mu,\delta^{2})Beta(\rho|a,b) \tag{3.11}$$
其中，$Beta(\rho|a,b)$ 服从贝塔分布，即
$$\nonumber Beta(\rho|a,b)=\frac{\Gamma(a+b)}{\Gamma(a)+\Gamma(b)}\rho^{a-1}(1-\rho)^{b-1} \tag{3.12}$$
  当新的测量值$d_{k}$到来时，深度滤波器的更新可以写作，
$$\nonumber p(\hat{d},\rho|d_{r+1},\cdots,d_{k})\approx p(d_{k}|\hat{d},\rho) q(\hat{d},\rho|a,b,u,\sigma^{2}) \tag{3.13}$$
又因为在前面提到，$q(Y,\hat{d},\rho)$ 是 $p(\hat{d},\rho|d_{r+1},\cdots,d_{k})$ 的一个近似解，所以
$$q(\hat{d},\rho|a{'},b{’},u{'},\sigma{'}^{2})$$
和式(3.13)具有相同的一二阶矩。
  将式(3.1)代入式(3.13)，这里为了表述方便，令$Z=\hat{d},b=d_{max},a=d_{min}$ ，得
$$\nonumber (\rho N(d|Z,\tau^{2})+(1-\rho)U(d))N(Z|\mu,\sigma^{2})Beta(\rho|a,b) \tag{3.14}$$
根据$Beta$ 分布的性质，可以将式(3.14)进一步写为，
$$\begin{align} \nonumber &\frac{a}{a+b}N(d|\mu,\sigma^{2}+\tau^{2})N(Z|m,s^{2})Beta(\rho|a,b) \\ \nonumber &\qquad+\frac{b}{a+b}U(x)N(Z|\mu,\sigma^{2})Beta(\rho|a,b+1) \tag{3.15} \end{align}$$
其中，关于Beta分布的变换可以按照Beta分布的定义得到，关于两个高斯分布乘积之后，变量$s$ 和 $m$的由来，推导如下，

对比式(3.14)和式(3.15)可知，

$$\nonumber N(d|Z,\tau^{2})N(Z|\mu,\sigma^{2})=N(d|\mu,\sigma^{2}+\tau^{2})N(Z|m,s^{2})$$
将左边的两个高斯分布乘积可得，
$$\begin{align} \nonumber &N(d|Z,\tau^{2})N(Z|\mu,\sigma^{2})=\frac{1}{\sqrt{2\pi}\tau}e^{-\frac{(x-Z)^{2}}{2\tau^{2}}}\frac{1}{\sqrt{2\pi}\sigma}e^{-\frac{(Z-\mu)^{2}}{2\sigma^{2}}} \\ \nonumber &=\frac{1}{2\pi\tau\sigma}e^{-\frac{\sigma^{2}(x-Z)^{2}+\tau^{2}(Z-\mu)^{2}}{2\pi^{2}\sigma^{2}}} \\ \nonumber &=\frac{1}{\sqrt{2\pi(\sigma^{2}+\tau^{2})}}e^{-\frac{(x-\mu)^{2}}{2(\sigma^{2}+\tau^{2})}}\frac{1}{\frac{\sqrt{2\pi\sigma^{2}\tau^{2}}}{\sigma^{2}+\tau^{2}}}e^{-\frac{\sigma^{2}(x-Z)^{2}+\tau^{2}(Z-\mu)^{2}}{2\pi^{2}\sigma^{2}}+\frac{(x-\mu)^{2}}{2(\sigma^{2}+\tau^{2})}} \\ \nonumber &=N(d|\mu,\sigma^{2}+\pi^{2})\frac{1}{\sqrt{2\pi\frac{\sigma^{2}\pi^{2}}{\sigma^{2}+\pi^{2}}}}e^{\frac{-[(\sigma^{2}+\pi^{2})Z^{2}-2(d\sigma^{2}+\mu\tau^{2}Z+\sigma^{2}d^{2}+\tau^{2}\mu^{2})](\sigma^{2}+\tau^{2})+(x-\mu)^{2}\pi^{2}\sigma^{2}}{2\pi^{2}\sigma^{2}(\sigma^{2}+\tau^{2})}} \\ \nonumber &=N(d|\mu,\sigma^{2}+\pi^{2})\frac{1}{\sqrt{2\pi\frac{\sigma^{2}\pi^{2}}{\sigma^{2}+\pi^{2}}}}e^{-\frac{(\sigma^{2}+\tau^{2})^{2}-2(d\sigma^{2}+\mu\tau^{2})(\sigma^{2}+\tau^{2})Z+(d\sigma^{2}+\mu\tau^{2})^{2}}{2\pi^{2}\sigma^{2}(\sigma^{2}+\tau^{2})}} \\ \nonumber &=N(d|\mu,\sigma^{2}+\pi^{2})\frac{1}{\sqrt{2\pi\frac{\sigma^{2}\pi^{2}}{\sigma^{2}+\pi^{2}}}}e^{-\frac{[Z-\frac{d\sigma^{2}+\mu\tau^{2}}{\tau^{2}+\sigma^{2}}]^{2}}{2\frac{\pi^{2}\sigma^{2}}{\sigma^{2}+\tau^{2}}}} \\ \nonumber &=N(d|\mu,\sigma^{2}+\pi^{2})N(Z|\frac{d\sigma^{2}+\mu\tau^{2}}{\tau^{2}+\sigma^{2}},\frac{\pi^{2}\sigma^{2}}{\sigma^{2}+\tau^{2}}) \\ \nonumber &=N(d|\mu,\sigma^{2}+\pi^{2})N(Z|m,s^{2}) \end{align}$$