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

阿华AIGC实验室

2026-4-28

解决鱼眼相机位姿估计偏差问题的思路与修正方案

我完全理解你花了几周调试却没得到预期结果的挫败感——尤其是这还关系到考试，必须得把这个问题啃下来！咱们从你的场景设置、坐标系和代码细节入手，一步步排查问题：

核心问题排查：坐标系与鱼眼模块的适配

首先，你提到的坐标系设定和solvePnP、鱼眼模块的默认逻辑可能存在冲突：

你定义的是图像像素坐标系（x向右，y向下），但OpenCV的3D世界坐标系默认遵循右手定则，且solvePnP计算的是世界坐标系到相机坐标系的变换，这里很容易搞混相机位置的推导逻辑。
你的理论相机位置是(5,20,3)，但得到的y坐标是25，说明世界坐标系的轴方向或3D点定义可能有问题。

1. 修正相机位置的推导逻辑

首先明确：tvec是世界坐标系原点在相机坐标系下的平移向量，相机在世界坐标系中的位置应该是-R.T @ tvec（R是rvec转换的旋转矩阵），而不是直接用tvec。你之前直接用tvec判断相机位置，这是典型误区！

添加这段代码计算正确的相机世界坐标：

# 将rvec转换为旋转矩阵
R, _ = cv2.Rodrigues(rvec)
# 计算相机在世界坐标系中的位置：相机位置 = -旋转矩阵的转置 × 平移向量
camera_position_world = -R.T @ tvec
print("相机在世界坐标系中的位置：", camera_position_world.flatten())

2. 鱼眼模块与solvePnP的模型适配

你怀疑的solvePnP和鱼眼模块的模型差异是对的！cv2.solvePnP默认使用针孔相机模型，而鱼眼镜头的畸变模型完全不同——你不能直接用去畸变后的点和原始K矩阵传入solvePnP，正确流程有两种：

方案A：使用鱼眼专用的solvePnP（推荐）

如果你的OpenCV版本在4.5及以上，直接替换为鱼眼专用的位姿估计函数：

success, rvec, tvec = cv2.fisheye.solvePnP(
    objective_points, distorted_points, K, D, flags=cv2.SOLVEPNP_ITERATIVE
)

⚠️ 注意：这里直接传入原始畸变点，不需要提前做undistortPoints，鱼眼版本的solvePnP会直接处理畸变。

方案B：正确处理去畸变后的相机矩阵

如果必须用普通solvePnP，需要重新生成去畸变后的相机矩阵Knew，而非直接用原始K：

# 生成去畸变后的相机矩阵Knew，balance=1.0保留最大视野
h, w = original_img.shape[:2]
Knew = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(K, D, (w,h), np.eye(3), balance=1.0)
# 去畸变图像和点
undistorted_image = cv2.fisheye.undistortImage(original_img, K, D, Knew=Knew)
undistorted_points = cv2.fisheye.undistortPoints(distorted_points, K, D, P=Knew)
# 用Knew做solvePnP
success, rvec, tvec = cv2.solvePnP(
    objective_points, undistorted_points, Knew, None, flags=cv2.SOLVEPNP_ITERATIVE
)

3. 特征点3D坐标的致命错误

你提到距离越远的点重投影误差越大，核心原因可能是3D点的坐标写错了：
你定义场地是10×20米，但objective_points里的y轴只写到了17米，而你的理论相机位置y坐标是20——这直接导致位姿计算的y轴偏移！修正3D点如下：

objective_points = np.float32([
    [ 0., 0., 0.],   # 左上角
    [10., 0., 0.],   # 右上角
    [10., 20., 0.],  # 右下角（修正为20米）
    [ 0., 20., 0.],  # 左下角（修正为20米）
    [ 0., 0., 3.],   # 左上角墙顶
    [10., 0., 3.]    # 右上角墙顶
])

4. 坐标系方向的验证

OpenCV的drawFrameAxes显示的是相机坐标系在世界坐标系中的姿态：X轴向右，Y轴向下，Z轴向前（指向场景）。如果Z轴没有指向场地中心，说明你的世界坐标系和OpenCV右手定则冲突，需要调整3D点的轴方向（比如反转y轴）。

完整修正后的代码片段

整合所有修正点的代码如下：

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')
h, w = original_img.shape[:2]

# 修正场地y轴范围为0-20米
objective_points = np.float32([
    [ 0., 0., 0.],
    [10., 0., 0.],
    [10., 20., 0.],
    [ 0., 20., 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)

# 使用鱼眼专用solvePnP
success, rvec, tvec = cv2.fisheye.solvePnP(
    objective_points, distorted_points, K, D, flags=cv2.SOLVEPNP_ITERATIVE
)

# 计算正确的相机世界坐标
R, _ = cv2.Rodrigues(rvec)
camera_position_world = -R.T @ tvec
print("相机世界坐标：", camera_position_world.flatten())

# 生成去畸变图像用于可视化
Knew = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(K, D, (w,h), np.eye(3), balance=1.0)
undistorted_image = cv2.fisheye.undistortImage(original_img, K, D, Knew=Knew)

# 绘制坐标轴（用Knew适配去畸变图像）
cv2.drawFrameAxes(undistorted_image, Knew, None, rvec, tvec, 5)

# 重投影验证（鱼眼专用projectPoints）
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)

# 在原始畸变图像绘制对比点
img = original_img.copy()
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.imshow('Undistorted with Axes', undistorted_image)
cv2.waitKey(0)
cv2.destroyAllWindows()