单目摄像头测距

1. 前言

随着自动驾驶的发展，现代汽车的智能化程度逐步提高，基于视觉系统的自动驾驶也逐步趋于稳定——特斯拉、Mobileye、CommaAI等机构起着推波助澜的作用。

自动驾驶场景或者Adas场景中，当检测出前方车辆后通常需要进行距离估计，为车辆控制提供距离参考信息；而基于视觉的移动机器人也存在该需求。单目摄像头光学图像测距具有低成本和计算快的优点，主要有两种常用的测距方式¹。

• 使用目标物体大小和摄像头焦距
• 使用摄像头高度和俯仰角

本文将介绍第二种测距方式。

2. 几何关系示意图

该部分直接引用自参考文献2

投影关系图和投影平面²如下所示，

平面$ABU$代表路平面，$ABCD$为摄像头拍摄到的路平面上的梯形区域，$O$为摄像机$O$点为摄像机镜头中心点，$OG$为摄像机光轴，$G$ 点为摄像机光轴和路平面的交点（同时也是视野梯形的对角线交点），$I$点为$O$点在路平面上的垂直投影。在路面坐标系中， 将$G$点定义为坐标系原点,，车辆前进方向定义为$Y$轴方向。

$G、A、B、C、D$ 各点在图像平面内的对应点上图所示，$a、b、c、d$为像平面矩形的4个端点，$H$和$W$分别为像平面的高和宽。定义图像矩形的中点$g$为像平面坐标系的坐标原点，y 轴代表车辆前进方向。

取路面上一点$P$，其在路平面坐标系的坐标为$(X_P,Y_P)$，$P$点在图像平面内的对应点为$p$, 其在像平面坐标系的坐标为$(x_p,y_p)$。

3. 距离计算

3.1 参数

• 摄像头高度$h$
• 摄像头水平视场角$2{\beta }_0$
• 摄像头垂直视场角$2{\alpha }_0$
• 摄像头俯仰角${\gamma }_0$

相关概念请参见博客：相机焦距与视场角

3.2 Y轴方向的成像模型²

摄像头光轴（$OG$）所在的与路面相垂直的平面为$OEI$。图中各字符与几何关系示意图中对应字符含义相同。直线$ML$与直线$OG$垂直，与直线$fF$的延长线相交于点$L$。点$p_y$表示像平面上的点$p$在直线$ef$上的投影($y$轴方向)，$P_y$点是路面上的点$P$在视野纵向对称线上的投影，Z是直线$p_y{P}_y$与直线$线ML$的交点。

根据三角函数关系和相似三角形可得，
$$\begin{array}{rl}\tan \alpha & =\frac{ZG}{GO}\\ \tan {\alpha }_0& =\frac{MG}{GO}\\ \frac{ZG}{MG}& =\frac{p_{y}g}{eg}\end{array}$$
所以，
$$\begin{array}{rl}\tan \alpha & =\frac{p_{y}g}{eg}\ast \tan {\alpha }_0\\ & =\frac{p_{y}g}{H/2}\ast \tan {\alpha }_0\\ & =\frac{y}{H/2}\ast \tan {\alpha }_0\end{array}$$
那么，
$$\begin{array}{rl}{P}_{y}I& =\frac{OI}{\tan (90^{\circ }-\alpha -{\gamma }_0)}\\ & =\frac{h}{\tan (90^{\circ }-\alpha -{\gamma }_0)}\end{array}$$

注：计算角度$\alpha =\arctan (\frac{y}{H/2}\ast \tan {\alpha }_0)$时，y为空间点在投影面上的y轴坐标，是有符号的，H为图像高度。

3.3 X轴方向的成像模型

$Px$为点P在X轴上的投影点，
在三角形$\Delta O{P}_{y}L$中，
$$\begin{array}{rl}\tan {\alpha }_1& =\frac{P_{y}P}{O{P}_y}\\ & =\frac{|X_p|}{O{P}_y}\\ & =\frac{|X_p|}{\sqrt{h^2+P_y{I}^2}}\end{array}$$
根据P点成像的相似三角形可得，
$$\begin{array}{rl}\frac{O{P}_y\tan {\alpha }_1}{O{P}_y\tan {\beta }_0}& =\frac{x}{W/2}\end{array}$$
综上，
$$|X_p|=\frac{\sqrt{h^2+P_y{I}^2}\ast x\ast \tan {\beta }_0}{W/2}$$

x为空间点在投影面上的x轴坐标

4. Show me the code

代码很简单，后续补充上来。

Reference