tomas789 / kitti2bag

Convert KITTI dataset to ROS bag file the easy way!
MIT License
702 stars 256 forks source link

How to convert topic "/tf" to kitti calib metrix like r_velo_to_cam、R0_rect、P2? #73

Open HXB-1997 opened 9 months ago

HXB-1997 commented 9 months ago

I am using this code: def project_velo_to_image(pts_3d, rect, Trv2c, P2): #(N,3)

T=np.zeros([4,4],dtype=np.float32)

# T[:3,:]=Trv2c
# T[3,3]=1
T = Trv2c
# R=np.zeros([4,4],dtype=np.float32)
# R[:3,:3]=rect
# R[3,3]=1
R = rect
M=np.dot(P2,R)
M=np.dot(M,T)
pts_3d = cart2hom(pts_3d) # cartesian to homogeneous
pts_2d = np.dot(pts_3d, M.T)  # nx3 x,y,scale
pts_2d[:, 0] /= pts_2d[:, 2] #divide by scale factor
pts_2d[:, 1] /= pts_2d[:, 2]
return pts_2d[:, 0:2]