如何使用相机校准参数计算每个图像像素的3D世界坐标？

Question

我必须将图像坐标转换为世界坐标。 我可以访问下面给出的所有相机参数：

py=0.0036 # pixel width and height px py
px=0.0036
focal_length=1777.341982433
fx=focal_length
fy=focal_length

dist_offset=112.6882278 # distance offset.
prince_pt=[954.935281491,558.241833346] #pricipal offset

camera_matrix = [[fx,   0.00000000, prince_pt[0]], 
                 [  0.00000000, fy, prince_pt[1]],
                 [  0.00000000,   0.00000000,   1.00000000]]


dist_coeffs =   [[ 0.068732817,
                  0.139845334,
                  -0.11738526,
                  -0.001002139,
                   -0.001268871]]

roll = 0.0271091714
pitch =0.0188595763
yaw = 3.137739087
YPR=[roll,pitch,yaw]

我需要使用旋转矩阵和相机固有矩阵为每个像素计算世界坐标。我使用给定的偏航俯仰和横滚参数来计算旋转矩阵。我将这些欧拉角转换为旋转矩阵，如下所示

print ("roll = ", roll)
print ("pitch = ", pitch)
print ("yaw = ", yaw)
print( "")


def eulerAnglesToRotationMatrix(theta) :

    R_x = np.array([[1,         0,                  0                   ],
                    [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                    [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                    ])



    R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                    ])

    R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                    [math.sin(theta[2]),    math.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])


    RotationMat = np.dot(R_z, np.dot( R_y, R_x ))

    return  RotationMat

R=eulerAnglesToRotationMatrix(YPR)


print ("Rotation matrix \n", R)

获得旋转矩阵R后。令“ I”为像素中的图像坐标。 “ W”是世界坐标。 “ C”是相机固有矩阵。 然后，我通过C'R'I = W计算世界坐标W，其中C'=相机固有矩阵的逆矩阵，而R'=旋转矩阵的逆矩阵。 我的相机位于世界中心，因此我不考虑转换矩阵。

计算世界坐标的代码如下：

Cam_intrisic=np.asanyarray(camera_matrix)
print("Camera Intrinsic \n",Cam_intrisic)
iCam_intrinsic=inv(Cam_intrisic)
iR=inv(R)
print("Inverse of Rotation matrix \n",iR)
print(iCam_intrinsic)
print("Inverse Camera matrix \n",iCam_intrinsic)


def worldCordinate(a,b):
    pixelPt = np.ones((3, 1))
    pixelPt[0, 0] = a
    pixelPt[1, 0] = b
    tempMat=np.matmul (iR,np.matmul(iCam_intrinsic, pixelPt))

    return tempMat

a=1080
b=0
print("Input pixel: ",a,b )
world=worldCordinate(a,b)
print("\n World test co-ordinates ", world)

请让我知道我要去哪里。我没有关于相机的确切坐标。