SLAM--深度滤波器

一、归一化积

设$p_1(x)$和$p_2(x)$是两个不同的概率密度函数，归一化积为：
$$p(x)=\eta \cdot p_1(x)p_2(x)$$
其中:
$$\eta =\int p_1(x)p_2(x)\mathrm{d}x$$
$\eta$是为了满足概率公理的归一化常数。

定理：对于$p(x|y_1,y_2)$，$y_1$和$y_2$是两次独立测量，有：
$$p(x|y_1,y_2)=\eta \cdot p(x|y_1)p(x|y_2)$$
推导，利用全概率公式我们可以知道：
$$p(x|y_1,y_2)=\frac{p(y_1,y_2|x)p(x)}{p(y_1,y_2)}$$
这里：
$$p(y_1,y_2|x)=p(y_1|x)p(y_2|x)=\frac{p(x|y_1)p(y_1)}{p(x)}\frac{p(x|y_2)p(y_2)}{p(x)}$$
则：
$$\eta =\frac{p(y_1)p(y_2)}{p(y_1,y_2)p(x)}$$
若$p(x|y_i),i=1,2$是高斯分布的概率密度函数；
$$\begin{gathered} p(x|y_1)=N({\mu }_1,{\sigma }_1^2) \\ p(x|y_2)=N({\mu }_2,{\sigma }_2^2) \end{gathered}$$
对于新的分布$p(x|y_1,y_2)=N(\mu ,{\sigma }^2)$:
$$\begin{gathered} \frac{1}{{\sigma }^2}=\frac{1}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2} \\ \frac{\mu }{{\sigma }^2}=\frac{{\mu }_1}{{\sigma }_1^2}+\frac{{\mu }_2}{{\sigma }_2^2} \\ ⟹{\sigma }^2=\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2} \end{gathered}$$

我们可以推导出K个归一化：
$$p(x|y_1,y_2,\cdots ,y_k)={\eta }_k\cdot p(x|y_1)p(x|y_2)\cdots p(x|y_k)$$
有：
$$\begin{gathered} ({\sigma }^2{)}^{-1}=\sum _{i=1}^k({\sigma }_i^2{)}^{-1} \\ ({\sigma }^2{)}^{-1}\cdot \mu =\sum _{i=1}^k({\sigma }_i^2{)}^{-1}\cdot {\mu }_k \end{gathered}$$

 
 

二、深度融合

我们假设所有的深度值都是服从高斯分布的，假设有两个状态${\xi }_1$、${\xi }_2$分别对相机坐标$T_{w{c}_1}$下的深度值进行观测，那么我们可以表示为：
$$\begin{gathered} p(d|{\xi }_1)=N({\mu }_1,{\sigma }_1^2) \\ p(d|{\xi }_2)=N({\mu }_2,{\sigma }_2^2) \end{gathered}$$
我们对这两个观测进行深度融合，即求出在共同观测${\xi }_1$、${\xi }_2$下的分布：
$$p(d|{\xi }_1,{\xi }_2)=N(\mu ,{\sigma }^2)$$
借助上述归一化积的方法，我们可以得到关系：
$$p(d|{\xi }_1,{\xi }_2)=\eta \cdot p(d|{\xi }_1)\cdot p(d|{\xi }_2)$$
所以深度融合可以看作两个高斯分布的乘积（归一化乘积）。

即：
$$\begin{gathered} \frac{1}{{\sigma }^2}=\frac{1}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2} \\ \frac{\mu }{{\sigma }^2}=\frac{{\mu }_1}{{\sigma }_1^2}+\frac{{\mu }_2}{{\sigma }_2^2} \end{gathered}$$
然后求出：
$$\begin{gathered} {\sigma }^2=\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2} \\ \mu =\frac{{\sigma }_2^2{\mu }_1+{\sigma }_1^2{\mu }_2}{{\sigma }_1^2+{\sigma }_2^2} \end{gathered}$$
这就是深度融合的过程。
 
 

三、不确定度的计算

这里不考虑光度误差带来的影响，即光度不变性，只考虑几何误差。

我们利用极线搜索的方式，首先考虑对极约束：
$$p_2^{T}F{p}_1=0$$
$p_1$、$p_2$是两个相机下的像素坐标；
假设在相机2的极线方程为：
$$a{u}_2+b{v}_2+c=0，即p_2^T\cdot \left[\begin{array}{c}a\\ b\\ c\end{array}\right]=0$$
根据对极约束，我们就可以得到：
$$F{p}_1=\left[\begin{array}{c}a\\ b\\ c\end{array}\right]$$
这样利用极线搜索的方式就可以找到最佳的像素匹配点（不考虑光度误差，认为是完全匹配）。

另外我们需要求出原匹配特征点三角计算的深度值 $p^{\prime }$和最佳像素点（极线搜索方式得到）的深度值$p$，如下图：

这样我们就可以计算第k个状态的深度${\mu }_k$和方差${\sigma }_k^2$：
$$\begin{gathered} {\mu }_k=p，{\sigma }_k=||p||-||p^{\prime }|| \\ p(d_k|{\xi }_k)=N({\mu }_k,{\sigma }_k^2) \end{gathered}$$
这样，之后就可以利用深度融合的方法进行更新，直到计算收敛。】
 
 

参考文献

1. 机器人学中的概率估计 — Timothy D. Barfoot
2. 视觉SLAM 十四讲 从理论到实践 ---- 高翔
3. Engel J, Sturm J, Cremers D. Semi-dense Visual Odometry for a Monocular Camera. 2013.
4. Engel J, Schps T, Cremers D. LSD-SLAM: Large-scale direct monocular SLAM[J]. Springer, Cham, 2014.