IPM逆透视变换问题1/3之：Vanish Point

IPM逆透视变换问题1/3之：Vanish Point 重要提醒：首先确定坐标系朝向，简化步骤须做出说明。

1. 坐标系

# world  coord [x-right,y-front,z-up]
# camera coord [x-right,y-down,z-front]
# image  coord [x-right,y-down,z-front(1)]
# pixel  coord [u-right,v-down]

2. 相机参数和姿态

# camera intrinsic parameter
cam_inpm = [640,640,640,512] #[f_x, f_y, c_x, c_y] [pixel,pixel,pixel,pixel] 
# camera extrinsic parameter
cam_expm = [0.0,0.0,3.0, 0.0,-8.0,6.0] # [x,y,z,roll,pitch,yaw] [m,m,m,deg,deg,deg]
mat_in = np.array([[cam_inpm[0],0,cam_inpm[2]],
                   [0,cam_inpm[1],cam_inpm[3]],
                   [0,0,1]])
                   
sr = np.sin(cam_expm[3]/180*np.pi)
cr = np.cos(cam_expm[3]/180*np.pi)
sp = np.sin(cam_expm[4]/180*np.pi)
cp = np.cos(cam_expm[4]/180*np.pi)
sy = np.sin(cam_expm[5]/180*np.pi)
cy = np.cos(cam_expm[5]/180*np.pi)

mat_wx = np.array([[1.0,0.0,0.0],[0.0,cr,-sr],[0.0,sr,cr]],dtype=float)
mat_wy = np.array([[cp,0.0,sp],[0.0,1.0,0.0],[-sp,0.0,cp]],dtype=float)
mat_wz = np.array([[cy,-sy,0.0],[sy,cy,0.0],[0.0,0.0,1.0]],dtype=float)
mat_ww = mat_wz @ mat_wy @ mat_wx   #extrinsic rotation z-y-x

mat_cc = np.array([[1.0,0.0,0.0],[0.0,0.0,1.0],[0.0,-1.0,0.0]],dtype=float)
mat_wc = mat_ww @ mat_cc

3. 设定消失点(sys_world)

# if only z_w=0 and x_w=0, so y_w=1/(cy*cr),             so vp_pp = [707.92777905 422.05386579]
# if only z_w=0,           so x_w=-sy/cr and y_w=cy/cr,  so vp_pp = [640.         422.05386579]
vp_ww = np.array([[0.0,1/(cy*cr),0.0]],dtype=float).T

4. 计算消失点(sys_pixel)

vp_cc = mat_wc.T @ vp_ww    
vp_ii = vp_cc / vp_cc[2,0]   #this line unnecessary, could be commented
vp_pp = mat_in @ vp_ii