双目测距方案

Written On 2023-01-26.

双目测距方案流程为：

立体校正得出重投影矩阵
识别得到左右目中，待测物体的中心像素点坐标
计算距离（深度信息）

立体校正得出重投影矩阵

通过立体校正，可用求得重投影矩阵 \(Q\)。

重投影矩阵 \(Q\) 实现了世界坐标系到图像像素坐标系之间的转换。

\[Q = \begin{bmatrix} 1 & 0 & 0 & -c_x \\ 0 & 1 & 0 & -c_y \\ 0 & 0 & 0 & f \\ 0 & 0 & -1/T_x & (c_x - c^{'}_x)/T_x \end{bmatrix} \tag{1} \]

\(c_x, c_y\)：左相机主点在图像中的坐标
- 相机主光轴与像平面的交点即是主点
\(f\)：重投影后的焦距
\(T_x\)：两台相机投影中心间的平移（负值）
\(c^{'}_x\)：是右相机主点在图像中的坐标
- 因为经过校正后，每个像素点是行对齐的，也就是右相机主点在图像中的坐标与左相机的相等

\(T_x\) 相反数等于基线，即基线 \(b\)：

\[ b = -T_x \tag{2} \]

识别得到左右目中，待测物体的中心像素点坐标

两个线程分别进行识别，获得在各自图像的中心点像素点坐标；
设待测物体在左目图像中，所识别到的为 \((x_l, y_l)\)，在右目图像为 \((x_r, y_r)\)；

同时，水平装载的双目相机，经过立体校正，左目图像中的任意像素点与其在右目图像中的像素点，行号是相同的；

那么基于左相机，视差 \(disparity\)：

\[ disparity = x_l - x_r \tag{3} \]

且

\[\begin{cases} x = x_l \\ y = y_l = y_r \end{cases} \tag{4} \]

基于立体校正后的理想模型

\[ depth = \frac{fb}{disparity} = \frac{-f \cdot T_x}{disparity} \]

\(depth\)：深度信息
\(f\)：相机焦距
\(b\)：左、右目镜头中心之间的距离(基线)
\(disparity\)：视差

上式与 \((2)(3)\) 联立，即可求得 \(depth\)。

基于重投影矩阵

重投影矩阵计算深度信息过程

重投影矩阵 \(Q\) 对于世界坐标系到图像像素坐标系之间的转换，其关系为：

\[Q \begin{bmatrix} x \\ y \\ disparity \\ 1 \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 & -c_x \\ 0 & 1 & 0 & -c_y \\ 0 & 0 & 0 & f \\ 0 & 0 & -\frac{1}{T_x} & \frac{c_x - c^{'}_x}{T_x} \end{bmatrix}\begin{bmatrix} x \\ y \\ disparity \\ 1 \end{bmatrix} = \begin{bmatrix} x - c_x \\ y - c_y \\ f \\ (-disparity + c_x - c^{'}_x)/T_x \end{bmatrix} = \begin{bmatrix} X \\ Y \\ Z \\ W \end{bmatrix} \]

\(x,y\)：左目相机中，待测物体中心的像素点坐标
\(disparity\)：视差

根据此转换关系，可以得到待测物体中心的三维坐标为 \((\frac{X}{W}, \frac{Y}{W}, \frac{Z}{W})\)

展开矩阵可得三维坐标为：

\[\begin{cases} \frac{X}{W} = \frac{x - c_x}{-disparity + c_x - c^{'}_x}T_x\\ \frac{Y}{W} = \frac{y - c_y}{-disparity + c_x - c^{'}_x}T_x\\ \frac{Z}{W} = \frac{f}{-disparity + c_x - c^{'}_x}T_x\\ \end{cases} \]

当立体校正正确时，有 \(c_x = c^{'}_x\)；
立体校正正确时，上式转化为：

\[\begin{cases} \frac{X}{W} = \frac{c_x - x}{disparity}T_x\\ \frac{Y}{W} = \frac{c_y - y}{disparity}T_x\\ \frac{Z}{W} = \frac{-f}{disparity}T_x\\ \end{cases} \]