【状态估计】深度传感器与深度估计算法（2/3）

信息融合

设深度分布服从高斯分布，
$$P(d)=N(\mu ,{\sigma }^2)$$
设新观测到的深度也服从高斯分布，
$$P(d_{obs})=N({\mu }_{obs},{\sigma }_{obs}^2)$$
则融合后的深度估计，
$${\mu }_{fused}=\frac{{\sigma }_{obs}^2\mu +{\sigma }^2{\mu }_{obs}}{{\sigma }_{obs}^2+{\sigma }^2}$$

$${\sigma }_{fused}^2=\frac{{\sigma }_{obs}^2{\sigma }^2}{{\sigma }_{obs}^2+{\sigma }^2}$$

基本的深度估计算法

1. 当新的一帧到来，求出同名点坐标；
2. 更新兴趣点的深度估计和其不确定性；
3. 将当前估计与上一次融合；若深度方差小于设定阈值，停止更新；若否，转到1。

从帧序列找出兴趣点的同名点，一般使用射极约束作极线搜索和块匹配算法实现。

高斯分布+均匀分布的混合分布模型

概率模型

设深度真值为$Z$，传感器测量正常的概率为$\pi$（生成服从高斯分布的测量），异常的概率为$1-\pi$（如遮挡问题，生成服从均匀分布的异常测量），第$n$次测量$x_n$的概率模型，
$$p(x_n|Z,\pi )=\pi N(x_n|Z,{\tau }_n^2)+(1-\pi )U(x_n|Z_{min},Z_{max})$$

深度的后验概率

由Bayesian概率，深度测量的后验概率为
$$p(Z,\pi |x_1...x_n)\propto p(Z,\pi )\prod _{i=1}^{n}p(x_i|Z,\pi )$$

此分布较为复杂，可采取近似策略处理降低算法复杂度。