世界坐标系-相机坐标系-图像坐标系

import numpy as np
import cv2

import matplotlib.pyplot as plt

grama_=10
beita_=10
alpha_=10

src_=np.ones((1500,1500,3),np.uint8)*255

cv2.circle(src_,(50,100),15,(0,0,0),-1,8,0)

cv2.circle(src_,(200,120),15,(0,0,0),-1,8,0)

cv2.circle(src_,(180,180),15,(0,0,0),-1,8,0)

cv2.line(src_,(50,100),(200,120),(0,0,0),2,8,0)
#a、翻滚角：绕Z轴旋转
X_00=np.zeros((3,3),np.float32)
X_00[0][0]=np.cos(grama_)
X_00[1][1]=X_00[0][0]
X_00[0][1]=-np.sin(grama_)
X_00[1][0]=np.sin(grama_)
X_00[2][2]=1
Z_=X_00
#b、偏航角：绕Y轴旋转

Y_=np.zeros((3,3),np.float32)
Y_[0][0]=np.cos(beita_)
Y_[0][2]=-np.sin(beita_)
Y_[1][1]=1
Y_[2][0]=-Y_[0][2]
Y_[2][2]=Y_[0][0]

#c、俯仰角：绕X轴旋转

Z_0=np.zeros((3,3),np.float32)
Z_0[0][0]=1
Z_0[1][1]=np.cos(alpha_)
Z_0[1][2]=np.sin(alpha_)
Z_0[2][1]=-np.sin(alpha_)
Z_0[2][2]=Z_0[1][1]

X_=Z_0
#R=R1R2R3
R_=np.dot(Y_,Z_)
R_=np.dot(X_,R_)

t_=0#偏移量
R_0=np.zeros((4,4),np.float32)
R_0[0:3,0:3]=R_
R_0[-1][-1]=1
R_0[-1][0]=0
R_0[0][-1]=t_

#---> x,y,1,1
#
#投影关系
dis_jt=100
F_=np.zeros((3,4),np.float32)
F_[2][2]=1
F_[0][0]=dis_jt
F_[1][1]=F_[0][0]

scale_=80
F_R=np.dot(F_/scale_,R_0)

#从世界坐标系到像素坐标系关系
#其中dx和dy表示每一列和每一行分别代表多少mm，即1(pixel)=dx(mm)
d_x=0.9 
d_y=0.9
u_0=8
v_0=8
D_XY=np.zeros((3,3),np.float32)
D_XY[0][0]=1/d_x
D_XY[1][1]=1/d_y
D_XY[-1][-1]=1
D_XY[0][-1]=u_0
D_XY[1][-1]=v_0

room_xyz=[]
for i_ in range(0,300):
    for j_ in range(0,300):
        (x,y,z,q)=(j_,i_,1,1)
        X_0=[x,y,z,1]
        X_0=np.array([X_0])
        X_0=np.transpose(X_0)
        X_1=np.dot(F_R,X_0)
        X_1=np.dot(D_XY,X_1)
        #print("X_1:",X_1.shape)
        X_1=X_1.reshape(-1)
       # print("X_1:",X_1[0])
        a_,b_=int(X_1[0]),int(X_1[1])
        room_xyz.append([a_+500,b_+500,i_,j_])
room_xyz=np.array(room_xyz)
W_=1500#np.max(room_xyz[:,0])
H_=1500#np.max(room_xyz[:,1])
src_0=np.ones((W_,H_,3),np.uint8)*255
for i_ in range(0,len(room_xyz)):
    i_0,j_0,i_1,j_1=room_xyz[i_]
    i_0=int(i_0)
    j_0=int(j_0)
    j_1=int(j_1)
    i_1=int(i_1)
    #print("==>",i_0,j_0,i_1,j_1)
    if i_0>0 and j_0>=0:
        try:
            src_0[j_0][i_0]=src_[j_1][i_1]
        except:
            continue
print("X:",X_)
# cv2.imshow("cfg",src_)
# cv2.waitKey()
plt.figure("Image") # 图像窗口名称
plt.imshow(src_)
#plt.imshow(src_0)
plt.axis('on') # 关掉坐标轴为 off
plt.title('image') # 图像题目
plt.show()

plt.figure("Image2") # 图像窗口名称
#plt.imshow(src_)
plt.imshow(src_0)
plt.axis('on') # 关掉坐标轴为 off
plt.title('image2') # 图像题目
plt.show()

参考算法：

https://zhuanlan.zhihu.com/p/379269769