如何为鱼眼相机正确使用solvePnP进行位姿估计？

我已经耗了好几周时间，尝试各种方法来正确估计鱼眼相机相对于板式网球场的位姿——相机矩阵、鱼眼畸变校正、归一化坐标，能试的组合都试了，但始终得不到理想结果，这可是关系到我考试的核心问题，求各位大佬帮忙！

实验设置 #

• 板式网球场规格：10×20米，四周环绕3米高的玻璃墙
• 相机安装位置：场地短边中间的玻璃墙后方，高度约3米，俯视角度，鱼眼镜头可以完整捕捉到场地四个角落

参考坐标系定义 #

我把场地左上角设为坐标原点(0,0,0)：

• x轴：沿图像向右延伸
• y轴：沿图像向下延伸
• 根据这个坐标系，相机的实际理论位置应该是(5,20,3)

代码实现与核心问题 #

我写了如下代码，但运行后出现两个关键问题：

1. 绘制出的X（红）、Y（绿）、Z（蓝）轴方向是对的，但坐标轴显示在(0,6,0)位置，而不是预期的原点(0,0,0)
2. 通过rvec和tvec计算出的相机y坐标为25，和实际值20偏差很大

import cv2
import numpy as np
from utils import load_fisheye_params, debug_image_with_points, undistort_image

K, D = load_fisheye_params('fishcam-fisheye.txt')
original_img = cv2.imread('bg_distorted_BO-2221.jpg')

objective_points = np.float32([
 [ 0., 0., 0.],
 [10., 0., 0.],
 [10., 17., 0.],
 [ 0., 17., 0.],
 [ 0., 0., 3.],
 [10., 0., 3.]
])

# 在原始畸变图像上手动点击的点
distorted_points = np.float32([
 [782., 299.],
 [1118., 283.],
 [1556., 585.],
 [376., 639.],
 [773., 204.],
 [1118., 187.]
])
distorted_points = distorted_points.reshape(-1, 1, 2)

undistorted_points = cv2.fisheye.undistortPoints(distorted_points, K, D, P=K)

success, rvec, tvec = cv2.solvePnP(
 objective_points, undistorted_points, K, None, flags=cv2.SOLVEPNP_ITERATIVE
)

# 在去畸变图像中绘制坐标轴
undistorted_image = cv2.fisheye.undistortImage(img, K, D, Knew=K)
cv2.drawFrameAxes(undistorted_image, K, None, rvec, tvec, 5)

# 绘制原始点与重投影点
projected_points, _ = cv2.fisheye.projectPoints(
 objective_points.reshape(-1, 1, 3), rvec, tvec, K, D
)

projected_points = projected_points.reshape(-1, 2)
clicked_points = distorted_points.reshape(-1, 2)

for i in range(len(objective_points)):
 clicked_px = (int(clicked_points[i][0]), int(clicked_points[i][1]))
 proj_px = (int(projected_points[i][0]), int(projected_points[i][1]))
 cv2.circle(img, clicked_px, 6, (0, 0, 255), -1)
 cv2.circle(img, proj_px, 6, (0, 255, 0), -1)
 cv2.line(img, clicked_px, proj_px, (0, 255, 255), 2)
 cv2.putText(img, str(i), clicked_px, cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 255), 2)

cv2.imshow('Debug', img)
cv2.waitKey(0)
cv2.destroyAllWindows()

已尝试的调试手段（均未解决问题） #

• 使用虚构的错误相机矩阵（随便编的焦距等参数），结果只是姿态轻微变化，坐标轴还是停在(0,6,0)位置
• 添加或更换特征点，结果没有明显改善，而且距离越远的点重投影误差越大
• 修改solvePnP的flags参数，结果反而比原来更差
• 反复验证了手动点击的特征点，确认没有点错（调试代码见附录1）
• 尝试归一化坐标流程：不设置cv2.fisheye.undistortPoints的P=K参数，后续solvePnP用单位矩阵np.eye(3)作为相机矩阵，结果依旧不对
• 在另一个板式网球场、不同相机位置的场景测试，基于预先保存的去畸变图像选取特征点，结果还是不正确

我的猜想 #

我怀疑问题出在solvePnP和OpenCV鱼眼模块采用的相机模型不匹配，但查了大量文献、官方文档，也试了各种在线脚本，都没找到解决办法。现在真的急坏了，求各位大佬给点思路或者解决方案！

附录1 特征点调试代码 #

def debug_image_with_points(img, K, D, undistorted_points, objective_points):
 map1, map2 = cv2.fisheye.initUndistortRectifyMap(
 K, D, np.eye(3), K, img.shape[1::-1], cv2.CV_16SC2
 )
 undistorted_img = cv2.remap(
 img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT
 )
 for i in range(len(undistorted_points)):
 color = tuple(np.random.randint(0, 255, 3).tolist())
 center = (int(undistorted_points[i][0][0]), int(undistorted_points[i][0][1]))
 string = f"({objective_points[i][0]}, {objective_points[i][1]}, {objective_points[i][2]})"
 cv2.circle(undistorted_img, center, 5, color, -1)
 cv2.putText(
 undistorted_img, string, (center[0] + 5, center[1] - 5),
 cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1
 )
 cv2.imshow('Undistorted Image with Points', undistorted_img)

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