标签：mat Point 0.0 IPM vp expm cam np Vanish
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
标签：mat,
Point,
0.0,
IPM,
vp,
expm,
cam,
np,
Vanish
From： https://www.cnblogs.com/streamwill/p/17189565.html