单目摄像头测距白水煮蝎子-

1. 前言

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

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

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

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

2. 几何关系示意图

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

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

平面
$ABU$

ABU代表路平面，
$ABCD$

ABCD为摄像头拍摄到的路平面上的梯形区域，
$O$

O为摄像机
$O$

O点为摄像机镜头中心点，
$OG$

OG为摄像机光轴，
$G$

G 点为摄像机光轴和路平面的交点（同时也是视野梯形的对角线交点），
$I$

I点为
$O$

O点在路平面上的垂直投影。在路面坐标系中， 将
$G$

G点定义为坐标系原点,，车辆前进方向定义为
$Y$

Y轴方向。


$G、A、B 、C、D$

G、A、B、C、D 各点在图像平面内的对应点上图所示，
$a、b、c、d$

a、b、c、d为像平面矩形的4个端点，
$H$

H和
$W$

W分别为像平面的高和宽。定义图像矩形的中点
$g$

g为像平面坐标系的坐标原点，y 轴代表车辆前进方向。

取路面上一点
$P$

P，其在路平面坐标系的坐标为
$(X_P, Y_P)$

(XP​,YP​)，
$P$

P点在图像平面内的对应点为
$p$

p, 其在像平面坐标系的坐标为
$(x_p, y_p)$

(xp​,yp​)。

3. 距离计算

3.1 参数

• 摄像头高度
  $h$

  h

• 摄像头水平视场角
  $2beta_0$

  2β0​

• 摄像头垂直视场角
  $2alpha_0$

  2α0​

• 摄像头俯仰角
  $gamma_0$

  γ0​

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

3.2 Y轴方向的成像模型²

摄像头光轴（
$OG$

OG）所在的与路面相垂直的平面为
$OEI$

OEI。图中各字符与几何关系示意图中对应字符含义相同。直线
$ML$

ML与直线
$OG$

OG垂直，与直线
$fF$

fF的延长线相交于点
$L$

L。点
$p_y$

py​表示像平面上的点
$p$

p在直线
$ef$

ef上的投影(
$y$

y轴方向)，
$P_y$

Py​点是路面上的点
$P$

P在视野纵向对称线上的投影，Z是直线
$p_yP_y$

py​Py​与直线
$线ML$

线ML的交点。

根据三角函数关系和相似三角形可得，

$begin{aligned} tanalpha &= frac{ZG}{GO} \ tanalpha_0 &= frac{MG}{GO} \ frac{ZG}{MG} &= frac{p_yg}{eg} end{aligned}$

tanαtanα0​MGZG​​=GOZG​=GOMG​=egpy​g​​
所以，

$begin{aligned} tanalpha &= frac{p_yg}{eg}*tanalpha_0\ &=frac{p_yg}{H/2}*tanalpha_0\ &=frac{y}{H/2}*tanalpha_0 end{aligned}$

tanα​=egpy​g​∗tanα0​=H/2py​g​∗tanα0​=H/2y​∗tanα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​​

注：计算角度
$alpha = arctan(frac{y}{H/2}*tanalpha_0)$

α=arctan(H/2y​∗tanα0​)时，y为空间点在投影面上的y轴坐标，是有符号的，H为图像高度。

3.3 X轴方向的成像模型

$Px$

Px为点P在X轴上的投影点，
在三角形
$Delta OP_yL$

ΔOPy​L中，

$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点成像的相似三角形可得，

$begin{aligned} frac{OP_ytanalpha_1}{OP_ytanbeta_0} &=frac{x}{W/2} \ end{aligned}$

OPy​tanβ0​OPy​tanα1​​​=W/2x​​
综上，

$|X_p| = frac{sqrt{h^2+P_yI^2} *x *tanbeta_0}{W/2}$

∣Xp​∣=W/2h2+Py​I2​∗x∗tanβ0​​

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

4. Show me the code

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

Reference