1. 前言

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

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

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

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

2. 几何关系示意图

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

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

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

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

取路面上一点 P P P，其在路平面坐标系的坐标为 ( X P , Y P ) (X_P, Y_P) (XP​,YP​)， P P P点在图像平面内的对应点为 p p p, 其在像平面坐标系的坐标为 ( x p , y p ) (x_p, y_p) (xp​,yp​)。

3. 距离计算

3.1 参数

• 摄像头高度 h h h
• 摄像头水平视场角 2 β 0 2\beta_0 2β0​
• 摄像头垂直视场角 2 α 0 2\alpha_0 2α0​
• 摄像头俯仰角 γ 0 \gamma_0 γ0​

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

3.2 Y轴方向的成像模型²

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

根据三角函数关系和相似三角形可得，
tan ⁡ α = Z G G O tan ⁡ α 0 = M G G O Z G M G = p y g e g \begin{aligned} \tan\alpha &= \frac{ZG}{GO} \\ \tan\alpha_0 &= \frac{MG}{GO} \\ \frac{ZG}{MG} &= \frac{p_yg}{eg} \end{aligned} tanαtanα0​MGZG​​=GOZG​=GOMG​=egpy​g​​
所以，
tan ⁡ α = p y g e g ∗ tan ⁡ α 0 = p y g H / 2 ∗ tan ⁡ α 0 = y H / 2 ∗ tan ⁡ α 0 \begin{aligned} \tan\alpha &= \frac{p_yg}{eg}*\tan\alpha_0\\ &=\frac{p_yg}{H/2}*\tan\alpha_0\\ &=\frac{y}{H/2}*\tan\alpha_0 \end{aligned} tanα​=egpy​g​∗tanα0​=H/2py​g​∗tanα0​=H/2y​∗tanα0​​
那么，
P y I = O I tan ⁡ ( 9 0 ∘ − α − γ 0 ) = h tan ⁡ ( 9 0 ∘ − α − γ 0 ) \begin{aligned} P_yI &= \frac{OI}{\tan(90^{\circ}-\alpha-\gamma_0)}\\ &= \frac{h}{\tan(90^{\circ}-\alpha-\gamma_0)} \end{aligned} Py​I​=tan(90∘−α−γ0​)OI​=tan(90∘−α−γ0​)h​​

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

3.3 X轴方向的成像模型

P x Px Px为点P在X轴上的投影点，
在三角形 Δ O P y L \Delta OP_yL ΔOPy​L中，
tan ⁡ α 1 = P y P O P y = ∣ X p ∣ O P y = ∣ X p ∣ h 2 + P y I 2 \begin{aligned} \tan \alpha_1 &=\frac{P_yP}{OP_y} \\ &= \frac{|X_p|}{OP_y}\\ &= \frac{|X_p|}{\sqrt{h^2+P_yI^2}} \end{aligned} tanα1​​=OPy​Py​P​=OPy​∣Xp​∣​=h2+Py​I2 ​∣Xp​∣​​
根据P点成像的相似三角形可得，
O P y tan ⁡ α 1 O P y tan ⁡ β 0 = x W / 2 \begin{aligned} \frac{OP_y\tan\alpha_1}{OP_y\tan\beta_0} &=\frac{x}{W/2} \\ \end{aligned} OPy​tanβ0​OPy​tanα1​​​=W/2x​​
综上，
∣ X p ∣ = h 2 + P y I 2 ∗ x ∗ tan ⁡ β 0 W / 2 |X_p| = \frac{\sqrt{h^2+P_yI^2} *x *\tan\beta_0}{W/2} ∣Xp​∣=W/2h2+Py​I2 ​∗x∗tanβ0​​

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

4. Show me the code

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

Reference