import cv2
import numpy as np

# 定义已知的三维坐标点和对应的二维坐标点
objectPoints = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1], [0, 1, 1], [0, 1, 1]],dtype=np.float32)
imagePoints = np.array([[10, 20], [30, 50], [20, 70], [50, 40],[20, 80], [30, 40]], dtype=np.float32)

# 定义相机的内参和畸变系数
cameraMatrix = np.array([[386, 0, 327], [0, 386, 244], [0, 0, 1]], dtype=np.float32)
distCoeffs = np.array([0, 0, 0, 0, 0], dtype=np.float32)

# 使用 solvePnP 函数计算相机的位姿
ret, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)

if ret:
    print("Rotation vector:\n", rvec)
    print("Translation vector:\n", tvec)
else:
    print("Failed to solve PnP.")