黑芝麻智能 - 道路坡度估计
目录
记录在黑芝麻智能工作期间在道路坡度估计方面的工作。
Introduction
计算机视觉感知领域的一个问题就是测量目标的大小和距离，而对单目相机而言，传统的方法是基于地平假设并通过相机内外参将目标的尺寸、位置参数从二维空间变换（即逆透视变换）回三维空间。但道路并不总是平坦的，起伏的道路会使得测距变得不准确。如果有一种方法能够估计出道路的起伏情况，则可以弥补上述变换带来的测量误差。
我在黑芝麻时期的工作之一就是基于车道线进行实时的道路坡度建模。
Implementation
Road Slope Estimate Kernel
V2021.1026
方法
V2021.1111
方法
$$
\begin{align}
\left[
\begin{array}{c}
u \\
v \\
1
\end{array}
\right]
&=
\frac{1}{z_c}
\left[
\begin{array}{cccc}
f_x=\frac{f}{d_x} & 0 & c_x & 0 \\
0 & f_y=\frac{f}{d_y} & c_y & 0\\
0 & 0 & 1 & 0\\
\end{array}
\right]
\cdot
\left[
\begin{array}{cc}
R & T \\
0 & 1
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
x_w \\
y_w \\
z_w \\
1
\end{array}
\right] \notag \\
\left[
\begin{array}{c}
x_w \\
y_w \\
z_w \\
1
\end{array}
\right]
&=
z_c
\left[
\begin{array}{cc}
R' & T' \\
0 & 1
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\frac{x_c}{z_c}=x'_c \\
\frac{y_c}{z_c}=y'_c \\
1 \\
\frac{1}{z_c}
\end{array}
\right]
\Leftarrow
\left[
\begin{array}{cccc}
\frac{1}{f_x} & 0 & -\frac{c_x}{f_x} & 0 \\
0 & \frac{1}{f_y} & -\frac{c_y}{f_y} & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
u \\
v \\
1 \\
\frac{1}{z_c}
\end{array}
\right] \notag \\
&=
z_c
\left[
\begin{array}{cc}
R'_1 & T'_1 \\
R'_2 & T'_2 \\
R'_3 & T'_3 \\
0 & 1
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\frac{Coord_c}{z_c}=Coord'_c \\
\frac{1}{z_c}
\end{array}
\right] \notag \\
&=
\left[
\begin{array}{cc}
z_cR'_1\cdot Coord'_c + T'_1 \\
z_cR'_2\cdot Coord'_c + T'_2 \\
z_cR'_3\cdot Coord'_c + T'_3 \\
1
\end{array}
\right] \notag
\end{align}
$$
因此对于两点投影有：
$$
\begin{align}
x_{1w} &= z_{1c}R'_1\cdot Coord'_{1c} + T'_1 \tag{1} \\
y_{1w} &= z_{1c}R'_2\cdot Coord'_{1c} + T'_2 \tag{2} \\
z_{1w} &= z_{1c}R'_3\cdot Coord'_{1c} + T'_3 \tag{3} \\
x_{2w} &= z_{2c}R'_1\cdot Coord'_{2c} + T'_1 \tag{4} \\
y_{2w} &= z_{2c}R'_2\cdot Coord'_{2c} + T'_2 \tag{5} \\
z_{2w} &= z_{2c}R'_3\cdot Coord'_{2c} + T'_3 \tag{6} \\
z_{2w} &= z_{1w} \tag{7} \\
z_{2c}R'_3\cdot Coord'_{2c} + T'_3 = z_{2w} &= z_{1w} = z_{1c}R'_3\cdot Coord'_{1c} + T'_3 \notag \\
z_{2c}R'_3\cdot Coord'_{2c} &= z_{1c}R'_3\cdot Coord'_{1c} \notag \\
z_{2c} &= \frac{z_{1c}R'_3\cdot Coord'_{1c}}{R'_3\cdot Coord'_{2c}} \notag \\
distance^2 &= (x_{1w}-x_{2w})^2+(y_{1w}-y_{2w})^2 \tag{8} \\
&= (z_{1c}R'_1\cdot Coord'_{1c}-z_{2c}R'_1\cdot Coord'_{2c})^2+(z_{1c}R'_2\cdot Coord'_{1c}-z_{2c}R'_2\cdot Coord'_{2c})^2 \notag \\
&= (z_{1c}R'_1\cdot Coord'_{1c}-z_{1c}\frac{R'_3\cdot Coord'_{1c}R'_1\cdot Coord'_{2c}}{R'_3\cdot Coord'_{2c}})^2
+(z_{1c}R'_2\cdot Coord'_{1c}-z_{1c}\frac{R'_3\cdot Coord'_{1c}R'_2\cdot Coord'_{2c}}{R'_3\cdot Coord'_{2c}})^2 \notag \\
&= z_{1c}^2(R'_1\cdot Coord'_{1c}-\frac{R'_3\cdot Coord'_{1c}R'_1\cdot Coord'_{2c}}{R'_3\cdot Coord'_{2c}})^2
+z_{1c}^2(R'_2\cdot Coord'_{1c}-\frac{R'_3\cdot Coord'_{1c}R'_2\cdot Coord'_{2c}}{R'_3\cdot Coord'_{2c}})^2 \notag
\end{align}
$$
得到坡道感知结果，需进行多项式拟合，对该拟合问题- 以三次多项式进行拟合，$x$ 为纵向距离， $h$ 为坡道高度
- $x_0$ （含）以内坡道高度假设为零
- $x_0$ 处多项式一阶导为零 $$ \begin{align} h &= c_0+c_1x+c_2x^2+c_3x^3 \tag{1} \\ &= c_0+c_1x-(\frac{3c_0}{x_0^2}+\frac{2c_1}{x_0})x^2+(\frac{2c_0}{x_0^3}+\frac{c_1}{x_0^2})x^3 \notag \\ &= (1-\frac{3x^2}{x_0^2}+\frac{2x^3}{x_0^3})c_0+(x-\frac{2x^2}{x_0}+\frac{x^3}{x_0^2})c_1 \notag \\ 0 = h_{x_0} &= c_0+c_1x_0+c_2x_0^2+c_3x_0^3 \tag{2} \\ &= c_0+c_1x_0+c_2x_0^2-(\frac{c_1}{3x_0^2}+\frac{2c_2}{3x_0})x_0^3 \notag \\ 0 &= 3c_0+2c_1x_0+c_2x_0^2 \notag \\ c_2 &= -(\frac{3c_0}{x_0^2}+\frac{2c_1}{x_0}) \notag \\ 0 = h'_{x_0} &= c_1+2c_2x_0+3c_3x_0^2 \tag{3} \\ c_3 &= -(\frac{c_1}{3x_0^2}+\frac{2c_2}{3x_0}) \notag\\ &= -\frac{c_1}{3x_0^2}+\frac{2}{3x_0}(\frac{3c_0}{x_0^2}+\frac{2c_1}{x_0}) \notag \\ &= \frac{2c_0}{x_0^3}+\frac{c_1}{x_0^2} \notag \\ \end{align} $$
- 以二次多项式进行拟合，$x$ 为纵向距离， $h$ 为坡道高度
- $x_0$ （含）以内坡道高度假设为零 $$ \begin{align} h &= c_0+c_1x+c_2x^2 \tag{1} \\ &= c_0+c_1x-(\frac{c_0}{x_0^2}+\frac{c_1}{x_0})x^2 \notag \\ &= (1-\frac{x^2}{x_0^2})c_0+(x-\frac{x^2}{x_0})c_1 \notag \\ 0 = h_{x_0} &= c_0+c_1x_0+c_2x_0^2 \tag{2} \\ c_2 &= -(\frac{c_0}{x_0^2}+\frac{c_1}{x_0}) \notag \\ \end{align} $$