IPM逆透视变换问题(2):Image --> Ground

IPM逆透视变换问题(2):Image --> Ground

1. 如果前置条件如下:

  1. 坐标系坐标系定义为:世界(右-前-上),相机(右-下-前),像素(右-下);
  2. 旋转角度表示为:绕 z z z轴为 y a w yaw yaw偏航角,绕 y y y轴为 r o l l roll roll滚转角,绕 x x x轴负方向为 p i t c h pitch pitch俯仰角,正负按照右手定则;
  3. 实际上在表达 y a w , p i t c h , r o l l yaw,pitch,roll yaw,pitch,roll姿态角时默认在(前-左-上)坐标系上进行,因此在表达这些角度时也可以增加一个(前-左-上)坐标系,从乘一次旋转矩阵。

2. 则点从像素坐标系到世界坐标系逆透视变换如下:

世界坐标系(右-前-上)沿着自身 Z Z Z轴平移 h h h,得到 T w t T_{wt} Twt:
T w t = [ 1 0 0 0 0 1 0 0 0 0 1 h 0 0 0 1 ] ( 1 ) \begin{aligned} T_{wt} &= \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&h\\ 0&0&0&1\end{bmatrix} \qquad &(1) \\ \end{aligned} Twt=10000100001000h1(1)

相机的姿态角度(滚转,俯仰,偏航 ( r o l l , p i t c h , y a w ) (roll,pitch,yaw) (roll,pitch,yaw)对应在世界坐标系(右前上)是 ( α = − p i t c h , β = r o l l , γ = y a w ) (\alpha = -pitch, \beta = roll, \gamma = yaw) (α=pitch,β=roll,γ=yaw),计算消失点时滚转角无影响即 r o l l = 0 roll=0 roll=0,则绕着固定坐标系 X , Y , Z X,Y,Z X,Y,Z三个坐标轴旋转后得到的变换矩阵 T w r T_{wr} Twr:
T w r = T w z ⋅ T w y ⋅ T w x = [ c o s ( γ ) − s i n ( γ ) 0 0 s i n ( γ ) c o s ( γ ) 0 0 0 0 1 0 0 0 0 1 ] [ c o s ( β ) 0 sin ⁡ ( β ) 0 0 1 0 0 − sin ⁡ ( β ) 0 c o s ( β ) 0 0 0 0 1 ] [ 1 0 0 0 0 cos ⁡ ( α ) − sin ⁡ ( α ) 0 0 sin ⁡ ( α ) cos ⁡ ( α ) 0 0 0 0 1 ] = [ c o s ( γ ) − s i n ( γ ) 0 0 s i n ( γ ) c o s ( γ ) 0 0 0 0 1 0 0 0 0 1 ] [ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ] [ 1 0 0 0 0 cos ⁡ ( α ) − sin ⁡ ( α ) 0 0 sin ⁡ ( α ) cos ⁡ ( α ) 0 0 0 0 1 ] = [ cos ⁡ ( γ ) − sin ⁡ ( γ ) cos ⁡ ( α ) sin ⁡ ( γ ) sin ⁡ ( α ) 0 sin ⁡ ( γ ) cos ⁡ ( γ ) cos ⁡ ( α ) − cos ⁡ ( γ ) sin ⁡ ( α ) 0 0 sin ⁡ ( α ) cos ⁡ ( α ) 0 0 0 0 1 ] ( 2 ) \begin{aligned} T_{wr} &= T_{wz} \cdot T_{wy} \cdot T_{wx} \\ &= \begin{bmatrix}cos\left(\gamma \right)&-sin\left(\gamma \right)&0&0\\ sin\left(\gamma \right)&cos\left(\gamma \right)&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}cos\left(\beta \right)&0&\sin \left(\beta \right)&0\\ 0&1&0&0\\ -\sin \left(\beta \right)&0&cos\left(\beta \right)&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&\cos \left(α\right)&-\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}cos\left(\gamma \right)&-sin\left(\gamma \right)&0&0\\ sin\left(\gamma \right)&cos\left(\gamma \right)&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&\cos \left(α\right)&-\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\cos \left(α\right)&\sin \left(γ\right)\sin \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\cos \left(α\right)&-\cos \left(γ\right)\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&0\\ 0&0&0&1\end{bmatrix} \qquad &(2) \\ \end{aligned} Twr=TwzTwyTwx=cos(γ)sin(γ)00sin(γ)cos(γ)0000100001cos(β)0sin(β)00100sin(β)0cos(β)0000110000cos(α)sin(α)00sin(α)cos(α)00001=cos(γ)sin(γ)00sin(γ)cos(γ)0000100001100001000010000110000cos(α)sin(α)00sin(α)cos(α)00001=cos(γ)sin(γ)00sin(γ)cos(α)cos(γ)cos(α)sin(α)0sin(γ)sin(α)cos(γ)sin(α)cos(α)00001(2)

世界坐标系(右前上)相机自身的(右前上)坐标系到相机的变换 T w o T_{wo} Two
T w o = T w t ⋅ T w r = [ 1 0 0 0 0 1 0 0 0 0 1 h 0 0 0 1 ] [ cos ⁡ ( γ ) − sin ⁡ ( γ ) cos ⁡ ( α ) sin ⁡ ( γ ) sin ⁡ ( α ) 0 sin ⁡ ( γ ) cos ⁡ ( γ ) cos ⁡ ( α ) − cos ⁡ ( γ ) sin ⁡ ( α ) 0 0 sin ⁡ ( α ) cos ⁡ ( α ) 0 0 0 0 1 ] = [ cos ⁡ ( γ ) − sin ⁡ ( γ ) cos ⁡ ( α ) sin ⁡ ( γ ) sin ⁡ ( α ) 0 sin ⁡ ( γ ) cos ⁡ ( γ ) cos ⁡ ( α ) − cos ⁡ ( γ ) sin ⁡ ( α ) 0 0 sin ⁡ ( α ) cos ⁡ ( α ) h 0 0 0 1 ] ( 3 ) \begin{aligned} T_{wo} &=T_{wt} \cdot T_{wr}\\ &= \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&h\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\cos \left(α\right)&\sin \left(γ\right)\sin \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\cos \left(α\right)&-\cos \left(γ\right)\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\cos \left(α\right)&\sin \left(γ\right)\sin \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\cos \left(α\right)&-\cos \left(γ\right)\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&h\\ 0&0&0&1\end{bmatrix} \qquad &(3) \end{aligned} Two=TwtTwr=10000100001000h1cos(γ)sin(γ)00sin(γ)cos(α)cos(γ)cos(α)sin(α)0sin(γ)sin(α)cos(γ)sin(α)cos(α)00001=cos(γ)sin(γ)00sin(γ)cos(α)cos(γ)cos(α)sin(α)0sin(γ)sin(α)cos(γ)sin(α)cos(α)000h1(3)

相机自身的(右前上)坐标系相机坐标系的变换 T c o T_{co} Tco
T o c = [ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ] [ 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1 ] [ 1 0 0 0 0 0 1 0 0 − 1 1 0 0 0 0 1 ] = [ 1 0 0 0 0 0 1 0 0 − 1 1 0 0 0 0 1 ] ( 4 ) \begin{aligned} T_{oc} &= \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&0&1&0\\ 0&-1&1&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}1&0&0&0\\ 0&0&1&0\\ 0&-1&1&0\\ 0&0&0&1\end{bmatrix} \qquad &(4) \end{aligned} Toc=100001000010000110000100001000011000001001100001=1000001001100001(4)

世界坐标系相机坐标系的变换为:
T w c = T w o ⋅ T o c = [ cos ⁡ ( γ ) − sin ⁡ ( γ ) cos ⁡ ( α ) sin ⁡ ( γ ) sin ⁡ ( α ) 0 sin ⁡ ( γ ) cos ⁡ ( γ ) cos ⁡ ( α ) − cos ⁡ ( γ ) sin ⁡ ( α ) 0 0 sin ⁡ ( α ) cos ⁡ ( α ) h 0 0 0 1 ] [ 1 0 0 0 0 0 1 0 0 − 1 1 0 0 0 0 1 ] = [ cos ⁡ ( γ ) − sin ⁡ ( γ ) sin ⁡ ( α ) − sin ⁡ ( γ ) cos ⁡ ( α ) 0 sin ⁡ ( γ ) cos ⁡ ( γ ) sin ⁡ ( α ) cos ⁡ ( γ ) cos ⁡ ( α ) 0 0 − cos ⁡ ( α ) sin ⁡ ( α ) h 0 0 0 1 ] ( 5 ) \begin{aligned} T_{wc} &= T_{wo} \cdot T_{oc} \\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\cos \left(α\right)&\sin \left(γ\right)\sin \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\cos \left(α\right)&-\cos \left(γ\right)\sin \left(α\right)&0\\ 0&\sin \left(α\right)&\cos \left(α\right)&h\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0&0\\ 0&0&1&0\\ 0&-1&1&0\\ 0&0&0&1\end{bmatrix}\\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\sin \left(α\right)&-\sin \left(γ\right)\cos \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\sin \left(α\right)&\cos \left(γ\right)\cos \left(α\right)&0\\ 0&-\cos \left(α\right)&\sin \left(α\right)&h\\ 0&0&0&1\end{bmatrix} \qquad &(5) \end{aligned} Twc=TwoToc=cos(γ)sin(γ)00sin(γ)cos(α)cos(γ)cos(α)sin(α)0sin(γ)sin(α)cos(γ)sin(α)cos(α)000h11000001001100001=cos(γ)sin(γ)00sin(γ)sin(α)cos(γ)sin(α)cos(α)0sin(γ)cos(α)cos(γ)cos(α)sin(α)000h1(5)

相机坐标系图像坐标系的变换(对齐过程)为:
T c i = [ 1 0 0 0 1 0 0 0 1 0 0 1 z c ] ( 6 ) \begin{aligned} T_{ci} &= \begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&1\\ 0&0&\frac{1}{z_c}\end{bmatrix} \qquad &(6) \end{aligned} Tci=10000100001zc1(6)

图像坐标系像素坐标系的变换为:
T i p = [ 1 f x 0 − c x f x 0 1 f y − c y f y 0 0 1 ] ( 7 ) \begin{aligned} T_{ip} &= \begin{bmatrix}\frac{1}{f_x}&0&-\frac{c_x}{f_x}\\ 0&\frac{1}{f_y}&-\frac{c_y}{f_y}\\ 0&0&1\end{bmatrix} \qquad &(7) \end{aligned} Tip=fx1000fy10fxcxfycy1(7)

世界坐标系图像像素坐标系的变换为:
T w p = T w c ⋅ T c i ⋅ T i p = [ cos ⁡ ( γ ) − sin ⁡ ( γ ) sin ⁡ ( α ) − sin ⁡ ( γ ) cos ⁡ ( α ) 0 sin ⁡ ( γ ) cos ⁡ ( γ ) sin ⁡ ( α ) cos ⁡ ( γ ) cos ⁡ ( α ) 0 0 − cos ⁡ ( α ) sin ⁡ ( α ) h 0 0 0 1 ] [ 1 0 0 0 1 0 0 0 1 0 0 1 z c ] [ 1 f x 0 − c x f x 0 1 f y − c y f y 0 0 1 ] = [ cos ⁡ ( γ ) f x − sin ⁡ ( γ ) sin ⁡ ( α ) f y − cos ⁡ ( γ ) c x f y + sin ⁡ ( γ ) sin ⁡ ( α ) f x c y − sin ⁡ ( γ ) cos ⁡ ( α ) f x f y f x f y sin ⁡ ( γ ) f x cos ⁡ ( γ ) sin ⁡ ( α ) f y − sin ⁡ ( γ ) c x f y − cos ⁡ ( γ ) sin ⁡ ( α ) f x c y + cos ⁡ ( γ ) cos ⁡ ( α ) f x f y f x f y 0 − cos ⁡ ( α ) f y cos ⁡ ( α ) c y z c + h f y + sin ⁡ ( α ) f y z c z c f y 0 0 1 z c ] ( 8 ) \begin{aligned} T_{wp} &= T_{wc} \cdot T_{ci} \cdot T_{ip}\\ &= \begin{bmatrix}\cos \left(γ\right)&-\sin \left(γ\right)\sin \left(α\right)&-\sin \left(γ\right)\cos \left(α\right)&0\\ \sin \left(γ\right)&\cos \left(γ\right)\sin \left(α\right)&\cos \left(γ\right)\cos \left(α\right)&0\\ 0&-\cos \left(α\right)&\sin \left(α\right)&h\\ 0&0&0&1\end{bmatrix} \begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&1\\ 0&0&\frac{1}{z_c}\end{bmatrix} \begin{bmatrix}\frac{1}{f_x}&0&-\frac{c_x}{f_x}\\ 0&\frac{1}{f_y}&-\frac{c_y}{f_y}\\ 0&0&1\end{bmatrix}\\ &= \begin{bmatrix}\frac{\cos \left(γ\right)}{f_x}&-\frac{\sin \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\cos \left(γ\right)c_xf_y+\sin \left(γ\right)\sin \left(α\right)f_xc_y-\sin \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\sin \left(γ\right)}{f_x}&\frac{\cos \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\sin \left(γ\right)c_xf_y-\cos \left(γ\right)\sin \left(α\right)f_xc_y+\cos \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ 0&-\frac{\cos \left(α\right)}{f_y}&\frac{\cos \left(α\right)c_yz_c+hf_y+\sin \left(α\right)f_yz_c}{z_cf_y}\\ 0&0&\frac{1}{z_c}\end{bmatrix} \qquad &(8) \end{aligned} Twp=TwcTciTip=cos(γ)sin(γ)00sin(γ)sin(α)cos(γ)sin(α)cos(α)0sin(γ)cos(α)cos(γ)cos(α)sin(α)000h110000100001zc1fx1000fy10fxcxfycy1=fxcos(γ)fxsin(γ)00fysin(γ)sin(α)fycos(γ)sin(α)fycos(α)0fxfycos(γ)cxfy+sin(γ)sin(α)fxcysin(γ)cos(α)fxfyfxfysin(γ)cxfycos(γ)sin(α)fxcy+cos(γ)cos(α)fxfyzcfycos(α)cyzc+hfy+sin(α)fyzczc1(8)

图像像素坐标系的点变换到世界坐标系的点的过程为:
1 z c [ x w y w z w 1 ] = T w p [ u v 1 ] [ x w z c y w z c z w z c 1 z c ] = [ cos ⁡ ( γ ) f x − sin ⁡ ( γ ) sin ⁡ ( α ) f y − cos ⁡ ( γ ) c x f y + sin ⁡ ( γ ) sin ⁡ ( α ) f x c y − sin ⁡ ( γ ) cos ⁡ ( α ) f x f y f x f y sin ⁡ ( γ ) f x cos ⁡ ( γ ) sin ⁡ ( α ) f y − sin ⁡ ( γ ) c x f y − cos ⁡ ( γ ) sin ⁡ ( α ) f x c y + cos ⁡ ( γ ) cos ⁡ ( α ) f x f y f x f y 0 − cos ⁡ ( α ) f y cos ⁡ ( α ) c y z c + h f y + sin ⁡ ( α ) f y z c z c f y 0 0 1 z c ] [ u v 1 ] = [ cos ⁡ ( γ ) u f y − sin ⁡ ( γ ) sin ⁡ ( α ) f x v − cos ⁡ ( γ ) c x f y + sin ⁡ ( γ ) sin ⁡ ( α ) f x c y − sin ⁡ ( γ ) cos ⁡ ( α ) f x f y f x f y sin ⁡ ( γ ) u f y + cos ⁡ ( γ ) sin ⁡ ( α ) f x v − sin ⁡ ( γ ) c x f y − cos ⁡ ( γ ) sin ⁡ ( α ) f x c y + cos ⁡ ( γ ) cos ⁡ ( α ) f x f y f x f y cos ⁡ ( α ) c y z c + sin ⁡ ( α ) z c f y − cos ⁡ ( α ) z c v + h f y z c f y 1 z c ] ( 9 ) \begin{aligned} \frac{1}{z_c}\begin{bmatrix}x_w\\ y_w\\ z_w\\ 1\end{bmatrix} &= T_{wp} \begin{bmatrix}u\\ v\\ 1\end{bmatrix} \\ \begin{bmatrix}\frac{x_w}{z_c}\\ \frac{y_w}{z_c}\\ \frac{z_w}{z_c}\\ \frac{1}{z_c}\end{bmatrix} &= \begin{bmatrix}\frac{\cos \left(γ\right)}{f_x}&-\frac{\sin \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\cos \left(γ\right)c_xf_y+\sin \left(γ\right)\sin \left(α\right)f_xc_y-\sin \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\sin \left(γ\right)}{f_x}&\frac{\cos \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\sin \left(γ\right)c_xf_y-\cos \left(γ\right)\sin \left(α\right)f_xc_y+\cos \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ 0&-\frac{\cos \left(α\right)}{f_y}&\frac{\cos \left(α\right)c_yz_c+hf_y+\sin \left(α\right)f_yz_c}{z_cf_y}\\ 0&0&\frac{1}{z_c}\end{bmatrix} \begin{bmatrix}u\\ v\\ 1\end{bmatrix} \\ &= \begin{bmatrix}\frac{\cos \left(γ\right)uf_y-\sin \left(γ\right)\sin \left(α\right)f_xv-\cos \left(γ\right)c_xf_y+\sin \left(γ\right)\sin \left(α\right)f_xc_y-\sin \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\sin \left(γ\right)uf_y+\cos \left(γ\right)\sin \left(α\right)f_xv-\sin \left(γ\right)c_xf_y-\cos \left(γ\right)\sin \left(α\right)f_xc_y+\cos \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\cos \left(α\right)c_yz_c+\sin \left(α\right)z_cf_y-\cos \left(α\right)z_cv+hf_y}{z_cf_y}\\ \frac{1}{z_c}\end{bmatrix} \qquad &(9) \end{aligned} zc1xwywzw1zcxwzcywzczwzc1=Twpuv1=fxcos(γ)fxsin(γ)00fysin(γ)sin(α)fycos(γ)sin(α)fycos(α)0fxfycos(γ)cxfy+sin(γ)sin(α)fxcysin(γ)cos(α)fxfyfxfysin(γ)cxfycos(γ)sin(α)fxcy+cos(γ)cos(α)fxfyzcfycos(α)cyzc+hfy+sin(α)fyzczc1uv1=fxfycos(γ)ufysin(γ)sin(α)fxvcos(γ)cxfy+sin(γ)sin(α)fxcysin(γ)cos(α)fxfyfxfysin(γ)ufy+cos(γ)sin(α)fxvsin(γ)cxfycos(γ)sin(α)fxcy+cos(γ)cos(α)fxfyzcfycos(α)cyzc+sin(α)zcfycos(α)zcv+hfyzc1(9)

由于消失点是处于地面上的点,其Z坐标为0,即 z w = = 0 z_w == 0 zw==0,所以有:

cos ⁡ ( α ) c y z c + sin ⁡ ( α ) z c f y − cos ⁡ ( α ) z c v + h f y z c f y = 0 ( 10 ) \frac{\cos \left(α\right)c_yz_c+\sin \left(α\right)z_cf_y-\cos \left(α\right)z_cv+hf_y}{z_cf_y}=0 \qquad (10) zcfycos(α)cyzc+sin(α)zcfycos(α)zcv+hfy=0(10)
则可以得到:
z c = h f y cos ⁡ ( α ) v − cos ⁡ ( α ) c y − sin ⁡ ( α ) f y ( 11 ) 1 z c = cos ⁡ ( α ) v − cos ⁡ ( α ) c y − sin ⁡ ( α ) f y h f y = v cos ⁡ ( α ) h f y + − cos ⁡ ( α ) c y − sin ⁡ ( α ) f y h f y ( 12 ) \begin{aligned} z_c&=\frac{hf_y}{\cos \left(α\right)v - \cos \left(α\right)c_y - \sin \left(α\right)f_y} \qquad &(11) \\ \frac{1}{z_c}&= \frac{\cos \left(α\right)v - \cos \left(α\right)c_y - \sin \left(α\right)f_y}{hf_y}\\ &=v\frac{\cos \left(α\right)}{hf_y}+ \frac{-\cos \left(α\right)c_y -\sin \left(α\right)f_y}{hf_y} \qquad &(12) \end{aligned} zczc1=cos(α)vcos(α)cysin(α)fyhfy=hfycos(α)vcos(α)cysin(α)fy=vhfycos(α)+hfycos(α)cysin(α)fy(11)(12)

z c z_c zc带入上式即有:

T w p = [ cos ⁡ ( γ ) f x − sin ⁡ ( γ ) sin ⁡ ( α ) f y − cos ⁡ ( γ ) c x f y + sin ⁡ ( γ ) sin ⁡ ( α ) f x c y − sin ⁡ ( γ ) cos ⁡ ( α ) f x f y f x f y sin ⁡ ( γ ) f x cos ⁡ ( γ ) sin ⁡ ( α ) f y − sin ⁡ ( γ ) c x f y − cos ⁡ ( γ ) sin ⁡ ( α ) f x c y + cos ⁡ ( γ ) cos ⁡ ( α ) f x f y f x f y 0 − cos ⁡ ( α ) f y v cos ⁡ ( α ) f y 0 0 v cos ⁡ ( α ) h f y + − cos ⁡ ( α ) c y − sin ⁡ ( α ) f y h f y ] ( 13 ) \begin{aligned} T_{wp}&= \begin{bmatrix}\frac{\cos \left(γ\right)}{f_x}&-\frac{\sin \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\cos \left(γ\right)c_xf_y+\sin \left(γ\right)\sin \left(α\right)f_xc_y-\sin \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ \frac{\sin \left(γ\right)}{f_x}&\frac{\cos \left(γ\right)\sin \left(α\right)}{f_y}&\frac{-\sin \left(γ\right)c_xf_y-\cos \left(γ\right)\sin \left(α\right)f_xc_y+\cos \left(γ\right)\cos \left(α\right)f_xf_y}{f_xf_y}\\ 0&-\frac{\cos \left(α\right)}{f_y}&v\frac{\cos \left(α\right)}{f_y}\\ 0&0&v\frac{\cos \left(α\right)}{hf_y}+ \frac{-\cos \left(α\right)c_y -\sin \left(α\right)f_y}{hf_y}\end{bmatrix} \qquad &(13) \end{aligned} Twp=fxcos(γ)fxsin(γ)00fysin(γ)sin(α)fycos(γ)sin(α)fycos(α)0fxfycos(γ)cxfy+sin(γ)sin(α)fxcysin(γ)cos(α)fxfyfxfysin(γ)cxfycos(γ)sin(α)fxcy+cos(γ)cos(α)fxfyvfycos(α)vhfycos(α)+hfycos(α)cysin(α)fy(13)

具体计算时要分离(u, v)的 影响, T w p T_{wp} Twp第三行中的 v v v难以分离,但是第三行用于计算 z w z_w zw,且地面上的 z w z_w zw恒为零,所以可以全部设为0; T w p T_{wp} Twp的第四行则比较容易分离。最后得到计算IPM时从像素到世界的变换 T 4 × 3 T_{4\times 3} T4×3

T 4 × 3 = [ cos ⁡ ( γ ) f x − sin ⁡ ( γ ) sin ⁡ ( α ) f y − c x f y cos ⁡ ( γ ) + c y f x sin ⁡ ( γ ) sin ⁡ ( α ) − f x f y sin ⁡ ( γ ) cos ⁡ ( α ) f x f y sin ⁡ ( γ ) f x cos ⁡ ( γ ) sin ⁡ ( α ) f y − c x f y sin ⁡ ( γ ) − c y f x cos ⁡ ( γ ) sin ⁡ ( α ) + f x f y cos ⁡ ( γ ) cos ⁡ ( α ) f x f y 0 0 0 0 cos ⁡ ( α ) h f y − c y cos ⁡ ( α ) − f y sin ⁡ ( α ) h f y ] = [ cos ⁡ ( γ ) f x − sin ⁡ ( γ ) sin ⁡ ( α ) f y − c x cos ⁡ ( γ ) f x + c y sin ⁡ ( γ ) sin ⁡ ( α ) f y − sin ⁡ ( γ ) cos ⁡ ( α ) sin ⁡ ( γ ) f x cos ⁡ ( γ ) sin ⁡ ( α ) f y − c x sin ⁡ ( γ ) f x − c y cos ⁡ ( γ ) sin ⁡ ( α ) f y + cos ⁡ ( γ ) cos ⁡ ( α ) 0 0 0 0 cos ⁡ ( α ) h f y − c y cos ⁡ ( α ) h f y − sin ⁡ ( α ) h ] ( 14 ) \begin{aligned}T_{4\times 3} &= \begin{bmatrix} \frac{\cos \left(\gamma \right)}{f_x} &\frac{-\sin \left(\gamma \right)\sin \left(\alpha \right)}{f_y} &\frac{-c_x f_y \cos \left(\gamma \right)+c_y f_x\sin \left(\gamma \right)\sin \left(\alpha \right)-f_x f_y \sin \left(\gamma \right)\cos \left(\alpha \right)}{f_x f_y}\\ \frac{\sin \left(\gamma \right)}{f_x} &\frac{\cos \left(\gamma \right)\sin \left(\alpha \right)}{f_y} &\frac{-c_x f_y\sin \left(\gamma \right)-c_y f_x\cos \left(\gamma \right)\sin \left(\alpha \right)+f_x f_y\cos \left(\gamma \right)\cos \left(\alpha \right)}{f_x f_y}\\ 0&0&0\\ 0&\frac{\cos \left(\alpha \right)}{h f_y} &\frac{-c_y\cos \left(\alpha \right)-f_y \sin \left(\alpha \right)}{h f_y} \end{bmatrix} \\ &= \begin{bmatrix} \frac{\cos(\gamma)}{f_x} &-\frac{\sin(\gamma) \sin(\alpha)}{f_y} &-\frac{c_x \cos(\gamma)}{f_x} + \frac{c_y \sin(\gamma) \sin(\alpha)}{f_y} - \sin(\gamma) \cos(\alpha)\\ \frac{\sin(\gamma)}{f_x} &\frac{\cos(\gamma) \sin(\alpha)}{f_y} &-\frac{c_x \sin(\gamma)}{f_x} - \frac{c_y \cos(\gamma) \sin(\alpha)}{f_y} + \cos(\gamma) \cos(\alpha)\\ 0&0&0\\ 0 &\frac{\cos(\alpha)}{h f_y} &-\frac{c_y \cos(\alpha)}{h f_y} - \frac{\sin(\alpha)}{h} \end{bmatrix} \qquad &(14) \end{aligned} T4×3=fxcos(γ)fxsin(γ)00fysin(γ)sin(α)fycos(γ)sin(α)0hfycos(α)fxfycxfycos(γ)+cyfxsin(γ)sin(α)fxfysin(γ)cos(α)fxfycxfysin(γ)cyfxcos(γ)sin(α)+fxfycos(γ)cos(α)0hfycycos(α)fysin(α)=fxcos(γ)fxsin(γ)00fysin(γ)sin(α)fycos(γ)sin(α)0hfycos(α)fxcxcos(γ)+fycysin(γ)sin(α)sin(γ)cos(α)fxcxsin(γ)fycycos(γ)sin(α)+cos(γ)cos(α)0hfycycos(α)hsin(α)(14)


如果式(14)中的角度 α , γ \alpha,\gamma α,γ取负值,同时矩阵乘 − 1 -1 1(不影响结果)。
则所有的 s i n sin sin值取反,同时矩阵的第三行与式(14)中的第四行乘 h h h(设定成什么都可以,不影响结果),此时即得到:

T G I = [ cos ⁡ ( − γ ) f x − sin ⁡ ( − γ ) sin ⁡ ( − α ) f y − c x f y cos ⁡ ( − γ ) + c y f x sin ⁡ ( − γ ) sin ⁡ ( − α ) − f x f y sin ⁡ ( − γ ) cos ⁡ ( − α ) f x f y sin ⁡ ( − γ ) f x cos ⁡ ( − γ ) sin ⁡ ( − α ) f y − c x f y sin ⁡ ( − γ ) − c y f x cos ⁡ ( − γ ) sin ⁡ ( − α ) + f x f y cos ⁡ ( − γ ) cos ⁡ ( − α ) f x f y 0 − cos ⁡ ( − α ) f y − − c y cos ⁡ ( − α ) − f y sin ⁡ ( − α ) f y 0 cos ⁡ ( − α ) h f y − c y cos ⁡ ( − α ) − f y sin ⁡ ( − α ) h f y ] ⋅ − 1 = [ − cos ⁡ ( γ ) f x sin ⁡ ( γ ) sin ⁡ ( α ) f y c x cos ⁡ ( γ ) f x − c y sin ⁡ ( γ ) sin ⁡ ( α ) f y − sin ⁡ ( γ ) cos ⁡ ( α ) sin ⁡ ( γ ) f x cos ⁡ ( γ ) sin ⁡ ( α ) f y − c x sin ⁡ ( γ ) f x − c y cos ⁡ ( γ ) sin ⁡ ( α ) f y − cos ⁡ ( γ ) cos ⁡ ( α ) 0 cos ⁡ ( α ) f y − c y cos ⁡ ( α ) f y + sin ⁡ ( α ) 0 − cos ⁡ ( α ) h f y c y cos ⁡ ( α ) h f y − sin ⁡ ( α ) h ] = [ − h cos ⁡ ( γ ) f x h sin ⁡ ( γ ) sin ⁡ ( α ) f y h c x cos ⁡ ( γ ) f x − h c y sin ⁡ ( γ ) sin ⁡ ( α ) f y − h sin ⁡ ( γ ) cos ⁡ ( α ) h sin ⁡ ( γ ) f x h cos ⁡ ( γ ) sin ⁡ ( α ) f y − h c x sin ⁡ ( γ ) f x − h c y cos ⁡ ( γ ) sin ⁡ ( α ) f y − h cos ⁡ ( γ ) cos ⁡ ( α ) 0 h cos ⁡ ( α ) f y − h c y cos ⁡ ( α ) f y + h sin ⁡ ( α ) 0 − cos ⁡ ( α ) f y c y cos ⁡ ( α ) f y − sin ⁡ ( α ) ] 1 h ( 15 ) {\color{darkorange} \begin{aligned} T_{GI} &=\begin{bmatrix} \frac{\cos(-\gamma)}{f_x} &\frac{-\sin(-\gamma) \sin(-\alpha)}{f_y} &\frac{-c_x f_y \cos(-\gamma) + c_y f_x \sin(-\gamma) \sin(-\alpha) - f_x f_y \sin(-\gamma) \cos(-\alpha)}{f_x f_y}\\ \frac{\sin(-\gamma)}{f_x} &\frac{\cos(-\gamma) \sin(-\alpha)}{f_y} &\frac{-c_x f_y \sin(-\gamma) - c_y f_x \cos(-\gamma) \sin(-\alpha) + f_x f_y \cos(-\gamma) \cos(-\alpha)}{f_x f_y}\\ 0 &-\frac{\cos(-\alpha)}{f_y}&-\frac{- c_y \cos(-\alpha) - f_y \sin(-\alpha)}{f_y}\\ 0 &\frac{\cos(-\alpha)}{h f_y} &\frac{-c_y \cos(-\alpha) - f_y \sin(-\alpha)}{h f_y}\end{bmatrix} \cdot -1\\ &=\begin{bmatrix} -\frac{\cos(\gamma)}{f_x} &\frac{\sin(\gamma) \sin(\alpha)}{f_y} &\frac{c_x \cos(\gamma)}{f_x} - \frac{c_y \sin(\gamma) \sin(\alpha)}{f_y} - \sin(\gamma) \cos(\alpha)\\ \frac{\sin(\gamma)}{f_x} &\frac{\cos(\gamma) \sin(\alpha)}{f_y} &-\frac{c_x \sin(\gamma)}{f_x} - \frac{c_y \cos(\gamma) \sin(\alpha)}{f_y} - \cos(\gamma) \cos(\alpha)\\ 0 &\frac{\cos(\alpha)}{f_y} &- \frac{c_y \cos(\alpha)}{f_y} + \sin(\alpha) \\ 0 &-\frac{\cos(\alpha)}{h f_y} &\frac{c_y \cos(\alpha)}{h f_y} - \frac{\sin(\alpha)}{h} \end{bmatrix} \\ &=\begin{bmatrix} -h\frac{\cos(\gamma)}{f_x} &h\frac{\sin(\gamma) \sin(\alpha)}{f_y} &h\frac{c_x \cos(\gamma)}{f_x} - h\frac{c_y \sin(\gamma) \sin(\alpha)}{f_y} - h\sin(\gamma) \cos(\alpha)\\ h\frac{\sin(\gamma)}{f_x} &h\frac{\cos(\gamma) \sin(\alpha)}{f_y} &-h\frac{c_x \sin(\gamma)}{f_x} - h\frac{c_y \cos(\gamma) \sin(\alpha)}{f_y} - h\cos(\gamma) \cos(\alpha)\\ 0 &h\frac{\cos(\alpha)}{f_y} &-h \frac{c_y \cos(\alpha)}{f_y} + h\sin(\alpha) \\ 0 &-\frac{\cos(\alpha)}{f_y} &\frac{c_y \cos(\alpha)}{f_y} - \sin(\alpha) \end{bmatrix} \frac{1}{h} \qquad&(15) \end{aligned} } TGI=fxcos(γ)fxsin(γ)00fysin(γ)sin(α)fycos(γ)sin(α)fycos(α)hfycos(α)fxfycxfycos(γ)+cyfxsin(γ)sin(α)fxfysin(γ)cos(α)fxfycxfysin(γ)cyfxcos(γ)sin(α)+fxfycos(γ)cos(α)fycycos(α)fysin(α)hfycycos(α)fysin(α)1=fxcos(γ)fxsin(γ)00fysin(γ)sin(α)fycos(γ)sin(α)fycos(α)hfycos(α)fxcxcos(γ)fycysin(γ)sin(α)sin(γ)cos(α)fxcxsin(γ)fycycos(γ)sin(α)cos(γ)cos(α)fycycos(α)+sin(α)hfycycos(α)hsin(α)=hfxcos(γ)hfxsin(γ)00hfysin(γ)sin(α)hfycos(γ)sin(α)hfycos(α)fycos(α)hfxcxcos(γ)hfycysin(γ)sin(α)hsin(γ)cos(α)hfxcxsin(γ)hfycycos(γ)sin(α)hcos(γ)cos(α)hfycycos(α)+hsin(α)fycycos(α)sin(α)h1(15)

之所进行这样的对比,主要是网络上大部分关于IPM的代码都是使用式(15)进行代码编写,而且在没有多少优化的情况下,导致代码阅读性较差,极不友好。


你可能感兴趣的:(slam,线性代数,几何学,人工智能)