从2D像素位置生成世界坐标系下3D射线的问题排查

我正尝试从2D图像像素位置投射一条3D射线。已知该像素对应的实际3D坐标，期望相机发出的射线能与该点相交，但当前射线未命中目标点（见图1）。

我使用透视投影方程进行计算，可成功将3D点转换为像素坐标，但反转方程生成3D射线时失败。我预期从相机中心指向逆变换得到的3D点的射线能与原始3D点相交，但实际并未命中。

不确定问题出在哪里，恳请提供帮助，谢谢！

我的Python代码 #

import numpy as np
import matplotlib.pyplot as plt
from numpy.linalg import inv, pinv

#########################################################
#Important Constants
p_w = np.array([[-410, 0, 10, 1]]).T                            # world point (4x1), (x,y,z,1)
K = 1.0e+03*np.array(                                           # Camera intrinsics (3x3) 
                         [[1.5665, 0,1.0323],
                         [0, 1.5693, 0.7825],
                         [0, 0, 0.0010]], dtype = "double"
                         )
R = np.array(                                                   # R_world (3x3)
                [[-0.92232435, 0.00751749, 0.38634348],
                [-0.14904685, 0.9155222, 0.37363637],
                [0.35651484, 0.4021972, -0.84328796]], dtype = "double"
                )


t = np.array([ [-304.79755558],  [-980.59230844], [-1920.36050988]]) # T_world 
q = np.hstack((R,t))                                                 # (R|t) Transform (3x4)
C = -np.matrix(R).T @ np.matrix(t)                                   # Centre of Camera

#Convert 3D point to pixel coordinates 
p_c = np.matmul(q, p_w)                                              # world point in camera frame
p_homo = np.matmul(K, p_c)                                           # world point in image frame in homo coordinates 
p_img = p_homo/p_homo[2]                                             # world point in image frame in pixels

#Convert pixel coordinates to 3D ray 
p_c = np.matmul(inv(K), p_img)                                       #image point in camera frame
p_c = np.append(p_c, 1)                                               
pts = np.matmul(p_c, pinv(q))                                        #image point in world frame using psuedo inverse in homo coord
pt_w = pts / pts[2]                                                  #image point in world frame in mm


#plot results

fig = plt.figure()
ax = fig.add_subplot(projection='3d')
ax.scatter(p_w[0], p_w[1], p_w[2], color="g", label ="actual_3d_pt") # actual 3d point
ax.scatter(C[0], C[1], C[2], color="y", label="Camera") # camera centre

x = np.append([C[0]], [pt_w[0]])
y = np.append([C[1]], [pt_w[1]])
z = np.append([C[2]], [pt_w[2]])
ax.plot(x,y,z, label="3d-ray", color="r")               #plot line from camera origin to point on ray

ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
ax.legend()

plt.show()

内容的提问来源于stack exchange，提问作者Mattcc18