33 lines
866 B
Python
33 lines
866 B
Python
# 将世界坐标系中的一个点投影到像素坐标系（世界坐标系->相机坐标系->图像坐标系->像素坐标系）
|
|
|
|
import numpy as np
|
|
|
|
intri = [[1111.0, 0.0, 400.0], [0.0, 1111.0, 400.0], [0.0, 0.0, 1.0]]
|
|
|
|
extri = np.array(
|
|
[
|
|
[-9.9990e-01, 4.1922e-03, -1.3346e-02, -5.3798e-02],
|
|
[-1.3989e-02, -2.9966e-01, 9.5394e-01, 3.8455e00],
|
|
[-4.6566e-10, 9.5404e-01, 2.9969e-01, 1.2081e00],
|
|
[0.0, 0.0, 0.0, 1.0],
|
|
]
|
|
)
|
|
|
|
|
|
def project_to_pixel(world_point):
|
|
R = extri[:3, :3]
|
|
T = extri[:3, -1]
|
|
cam_p1 = np.dot(world_point - T, R)
|
|
pixel_point = np.array(
|
|
[
|
|
-cam_p1[0] * intri[0][0] / cam_p1[2] + intri[0][2],
|
|
cam_p1[1] * intri[1][1] / cam_p1[2] + intri[1][2],
|
|
]
|
|
)
|
|
return pixel_point
|
|
|
|
|
|
p1 = np.array([0, 0, 0])
|
|
screen_p1 = project_to_pixel(p1)
|
|
print(screen_p1)
|