63 lines
1.7 KiB
Python
63 lines
1.7 KiB
Python
# 将世界坐标系中从相机中心出发的一条射线上的多个点投影到像素坐标系，验证这些点的投影点相同
|
|
|
|
import matplotlib.pyplot as plt
|
|
import numpy as np
|
|
|
|
intri = np.array([[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
|
|
|
|
|
|
camera_origin = extri[:3, -1]
|
|
direction = extri[:3, :3].dot(np.array([1, 1, 10]))
|
|
direction /= np.linalg.norm(direction)
|
|
scale_factors = np.linspace(1, 10, 10)
|
|
for scale in scale_factors:
|
|
sampled_point = camera_origin + scale * direction
|
|
pixel_point = project_to_pixel(sampled_point)
|
|
print(f"Sampled Point: {sampled_point}, Pixel Coordinate: {pixel_point}")
|
|
sampled_points = np.array(
|
|
[camera_origin + scale * direction for scale in scale_factors]
|
|
)
|
|
fig = plt.figure()
|
|
ax = fig.add_subplot(111, projection="3d")
|
|
ax.scatter(
|
|
camera_origin[0],
|
|
camera_origin[1],
|
|
camera_origin[2],
|
|
color="red",
|
|
label="Camera Origin",
|
|
)
|
|
ax.text(camera_origin[0], camera_origin[1], camera_origin[2], "Camera", color="red")
|
|
ax.plot(
|
|
sampled_points[:, 0],
|
|
sampled_points[:, 1],
|
|
sampled_points[:, 2],
|
|
"bo-",
|
|
label="Sample Points on Ray",
|
|
)
|
|
ax.legend()
|
|
ax.set_xlabel("X")
|
|
ax.set_ylabel("Y")
|
|
ax.set_zlabel("Z")
|
|
plt.show()
|