如何针对鱼眼相机正确使用solvePnP求解位姿？

问题背景 #

我已耗时数周尝试正确求解鱼眼相机相对于板式网球场的位姿，试过相机矩阵、鱼眼校正、归一化坐标的各种组合，但均未得到正确结果。

场景说明 #

板式网球场尺寸为10×20米，四周有3米高的玻璃墙。相机置于玻璃墙后方、短边中点处，高度约3米，俯视球场；借助鱼眼镜头可覆盖球场四个角落。

参考坐标系 #

将球场左上角标记为(0,0,0)，x轴沿图像向右递增，y轴沿图像向下递增，理论上相机应位于(5,20,3)位置。

问题现象 #

使用以下代码时，得到的三轴方向与朝向正确，但轴的实际位置约为(0,6,0)而非设定原点；通过rvec、tvec计算的相机位置显示y=25，与理论值不符。

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.]
])

# Manually clicked on the original (distorted) image
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
)

# Draws the X (Red), Y (Green), and Z (Blue) axes in undistorted image
undistorted_image = cv2.fisheye.undistortImage(img, K, D, Knew=K)
cv2.drawFrameAxes(undistorted_image, K, None, rvec, tvec, 5)


# Draw the original points and their reprojection

projected_points, _ = cv2.fisheye.projectPoints(
    objective_points.reshape(-1, 1, 3), 
    rvec, 
    tvec, 
    K, 
    D
)

# Reshape arrays for easy looping
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()

结果 #

我尝试过更换相机矩阵（含虚构焦距等参数）、增减标记点、修改solvePnP的flags参数、使用归一化坐标等方法，结果仅出现微小朝向变化，轴位置仍约为(0,6,0)；手动标记的点经检查无误。

我还尝试过参考其他示例代码，在另一球场场景下从校正后图像取点求解，结果仍不正确。怀疑是solvePnP与鱼眼模块使用的相机模型差异导致问题，恳请帮助解决该问题。

Appendix 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)
    cv2.waitKey(0)
    cv2.destroyAllWindows()

Appendix 2 #

import cv2
import numpy as np

img = cv2.imread("bg_BO-0001.jpg")
size = img.shape
image_points_2D = np.array([
    [763, 226],
    [1123, 222],
    [1696, 601],
    [203, 616],
    [755, 112],
    [1134, 112]
    ],
    dtype="double",
)

figure_points_3D = np.array([
    [ 0.,  0., 0.],
    [10.,  0., 0.],
    [10., 17., 0.],
    [ 0., 17., 0.],
    # [ 0., 10., 0.],   # left of field
    # [ 5., 10., 0.],   # center of the field
    # [10., 10., 0.],   # right of field
    [ 0.,  0., 3.],
    [10.,  0., 3.],
])

distortion_coeffs = np.zeros((4, 1))
focal_length = size[1]
center = (size[1] / 2, size[0] / 2)
matrix_camera = np.array(
    [[focal_length, 0, center[0]], [0, focal_length, center[1]], [0, 0, 1]],
    dtype="double",
)
success, vector_rotation, vector_translation = cv2.solvePnP(
    figure_points_3D, image_points_2D, matrix_camera, distortion_coeffs, flags=0
)
nose_end_point2D, jacobian = cv2.projectPoints(
    np.array([(0.0, 0.0, 1000.0)]),
    vector_rotation,
    vector_translation,
    matrix_camera,
    distortion_coeffs,
)
for p in image_points_2D:
    cv2.circle(img, (int(p[0]), int(p[1])), 3, (0, 0, 255), -1)
point1 = (int(image_points_2D[0][0]), int(image_points_2D[0][1]))

point2 = (int(nose_end_point2D[0][0][0]), int(nose_end_point2D[0][0][1]))

cv2.line(img, point1, point2, (255, 255, 255), 2)
# Draws the X (Red), Y (Green), and Z (Blue) axes in undistorted image
cv2.drawFrameAxes(img, matrix_camera, None, vector_rotation, vector_translation, 3)

cv2.imshow("Final", img)
cv2.waitKey(0)
cv2.destroyAllWindows()

补充可复现信息 #

• 原始图片：
• 鱼眼相机参数文件fishcam-fisheye.txt内容：

fx = 1240.0
fy = 1366.0
cx = 950.0
cy = 496.0
k1 = -0.358
k2 = 0.046
k3 = -0.088
k4 = 0.102

• 参数加载函数：

def load_fisheye_params(path):
    parameters = {}
    
    with open(path, 'r') as file:
        for line in file:
            # Split the line by '=' to separate key and value
            key, value = map(str.strip, line.strip().split('='))
            parameters[key] = float(value)  # Convert value to float

    if not parameters:
        raise ValueError("Error: No parameters found in the file.")

    fx = parameters['fx']
    fy = parameters['fy']
    cx = parameters['cx']
    cy = parameters['cy']
    k1 = parameters['k1']
    k2 = parameters['k2']
    k3 = parameters['k3']
    k4 = parameters['k4']

    mtx = np.array([[fx, 0., cx],
                    [0., fy, cy],
                    [0., 0., 1.]])
    dist = np.array([[k1], [k2], [k3], [k4]])

    return mtx, dist

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