网球场地相机标定问题：2D转3D坐标异常求助

背景 #

我通过球场检测模型提取了网球场地的所有特征点，在Unity中完成了这些点的3D位置映射，目前拥有约25组2D-3D点对对应关系。使用OpenCV相关函数生成相机投影矩阵和3D坐标时出现问题，输出结果不符合预期。

代码实现 #

相机投影矩阵生成函数 #

def process():
    C = cv.initCameraMatrix2D([objPts], [imgPts], (width, height))
    print("camera matrix:")
    print(C)
    fx = C[0,0]

    # fx * tan(hfov/2) == width/2
    hfov = np.arctan(width/2 / fx) * 2
    print(f"horizontal FoV:\n\t{hfov / np.pi * 180:.2f} °")

    # x? mm focal length -> 36 mm horizontal (24 vertical)?
    fd = 36 / (np.tan(hfov/2) * 2)
    print(f"focal length (35mm equivalent):\n\t{fd:.2f} mm")

    (rv, rvec, tvec) = cv.solvePnP(objPts, imgPts, C, distCoeffs=None)
    print("tvec [m]:")
    print(tvec)
    
    return C, tvec, rvec, rv

2D转3D坐标映射函数 #

def map2Dto3D(points_2D, tvec, rvec, C, R):
    # Convert rotation vector to rotation matrix
    R, _ = cv2.Rodrigues(rvec)

    # Invert the camera matrix
    C_inv = np.linalg.inv(C)

    # List to store 3D points
    points_3D = []

    # Iterate over each 2D point
    for point_2D in points_2D:
        # Convert 2D point to homogeneous coordinates
        point_2D_homo = np.array([point_2D[0], point_2D[1], 1])

        # Compute the intermediate 3D point
        intermediate_point = np.dot(np.dot(C_inv, R), point_2D_homo)

        # Compute the scale factor using the z-component of the translation vector
        scale_factor = tvec[2] / intermediate_point[2]

        # Compute the final 3D point
        point_3D = scale_factor * intermediate_point + tvec

        # Append the 3D point to the list
        points_3D.append(point_3D)

    # Write results to file
    with open("3dpoints.txt", "w") as file:
        for point_3D in points_3D:
            file.write(f"[{point_3D[0]}, {point_3D[1]}, {point_3D[2]}]\n")

    return points_3D

问题现象 #

生成的3D坐标不符合预期的(X,Y,Z)格式，且所有坐标值完全相同，无法实现正常的2D到3D映射，求技术帮助。

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