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

深度传感器与深度估计算法

深度传感器概念

获得空间中目标位置或距离的传感器，按接收的媒介波来源可分为主动式和被动式两大范畴，主动式包括激光雷达、雷达、超声波传感器等，被动式主要为单目、多目相机等，同时两大类可组合为混合类型传感器，深度（即距离）测量机制可分为基于（被动）视差原理的机制和（主动）ToF(Time Of Flight)机制两种，使用结构光的RGBD相机，尽管其媒介波源于自身，但其工作原理实为双目视差。市面上已有面向室内应用的RGBD相机、面向自动驾驶应用的激光雷达+相机融合一体的传感器，其既有“被动测量深度”的单目或多目相机，又有“主动”测量深度的器件。由于环境信息规律的统一、不同种的媒介波的属性的统一，各式各样传感器的模型可再抽象为统一的深度传感器模型，进一步便引出了深度估计问题。

深度估计问题最早起源于双目立体视觉，后面扩展到多视图立体视觉并得到了详尽的研究可供借鉴，现在我们推广到一般的深度估计问题。

问题

自动驾驶应用为估计空间状态而使用多种具体的传感器，数据的具体处理方式不同，而此类硬件可被统一抽象为深度传感器，软件估计算法是否可以据此完成统一抽象？

首先总结问题的一般形式：已知传感器数据参考帧的位姿参数，帧上一个兴趣点的深度值和深度值方差的先验，提供帧序列和其位姿数据，通过一定方法获取相应同名点，估计该点的深度及其方差。此处深度指参考帧的坐标原点到空间点的距离。此问题需要设定深度分布的概率模型，然后给出一些参考思路。

深度传感器的概率模型

常用的概率模型有两种，第一种是高斯分布模型，较简单而易求解；第二种是高斯分布加均匀分布的混合分布，对现实传感器的属性描述更真实。高斯分布一般作为理想环境与传感器理想工作条件下传感器测量输出的分布，均匀分布则是对传感器运行异常或环境异常如遮挡问题等条件下假设的测量分布，两种分布的结合更能逼近描述真实世界。
深度求解的具体方法因特定传感器而异，下面首先以多视图图像估计深度为例给出具体估计方法，然后重点关注第二种概率模型的一般估计问题。

高斯分布模型的求解

深度期望估计

法I Triangulation求空间点坐标得出深度
$x_i$为图像点齐次坐标，$P_i$为投影矩阵，$X$为空间点齐次坐标，投影方程如下，
$$x_i=P_{i}X$$
已知2帧以上的投影矩阵$P_i$和同名点，i=1，2……，求同名点的空间点估计X。

设$P^{jT}$是$P_i$的第j行，则有
$$y{p}^{3T}X-p^{2T}X=0$$

$$x{p}^{3T}X-p^{1T}X=0$$

$$x{p}^{2T}X-y{p}^{1T}X=0$$

第三行与前两行线性相关，利用多帧构造方程，
$$AX=0$$
A的第i块为，
$$A_i=\left[\begin{array}{c}y{p}^{3T}-p^{2T}\\ x{p}^{3T}-p^{1T}\end{array}\right]$$
采用SVD求之。帧间运动不明显时易出现数值结果不稳定问题，实际测试中发现空间点坐标z经常为负数（以参考帧相机坐标系为参考坐标系，z应为正值），工程实现不考虑采用此法。

法II 射线法
设参考帧图像点对应一空间点的深度为$d_r$，当前帧同一点的深度为$d_c$，$f_r$，$f_c$分为参考帧、当前帧光心到空间点的射线单位矢量在相应帧坐标系下的表达，则有
$$d_r{f}_r=d_c(R_{rc}{f}_c)+t_{rc}$$
令$f_2=R_{rc}{f}_c$，$t=t_{rc}$，可构造如下方程，
$$\left[\begin{array}{cc}{f}_r^T{f}_r& -f_r^T{f}_2\\ f_2^T{f}_r& -f_2^T{f}_2\end{array}\right]\left[\begin{array}{c}{d}_r\\ d_c\end{array}\right]=\left[\begin{array}{c}{f}_r^{T}t\\ f_2^{T}t\end{array}\right]$$
克莱默法则求解$d_r$，$d_c$，获取参考帧下的光心到空间点下的矢量的两个表达式，
$$x_m=d_r{f}_r$$

$$x_n=d_c{f}_2+t$$

取二者平均求结果模长即为新观测到的深度。

深度方差估计

考虑深度的不确定性，针对图像数据可考虑光度不确定性或几何不确定性，本文暂时只考虑几何关系造成的不确定性，假设角点观测误差为1个像素，以参考帧坐标系为参考系，设参考帧光心到当前帧光心的矢量为$t$，参考帧光心到目标点的矢量为$p$，当前帧光心到目标点的矢量为$a$，
$$\alpha =arccos(\frac{p\ast t}{||p||\ast ||t||})$$

$$\beta =arccos(\frac{a\ast (-t)}{||a||\ast ||t||})$$

$${\beta }^{\prime }=\beta +arctan(1/f)$$

$$\gamma =\pi -\alpha -{\beta }^{\prime }$$

$$||p^{\prime }||=||t||\frac{sin{\beta }^{\prime }}{sin\gamma }$$

$${\sigma }_{obs}=||p||-||p^{\prime }||$$

$${\sigma }_{obs}^2={\sigma }_{obs}\ast {\sigma }_{obs}$$