OpenCV相机姿态估计：畸变与无畸变图像结果为何不一致？

问题 #

我正尝试使用OpenCV的solvePnp()和Rodrigues()方法估计相机位置，相关参数说明：

• obj_points：三维真实世界点
• img_points：从原始（未畸变）图像中提取的二维相机像素点
• mtx：内参矩阵
• dist：畸变系数

首次使用原始图像点时，代码如下：

_, rvecs, tvecs = cv.solvePnP(
        obj_points,
        img_points,
        mtx,
        dist,
        flags=cv.SOLVEPNP_ITERATIVE,
    )

rotMat, _ = cv.Rodrigues(rvecs)

camera_position = -np.matrix(rotMat).T * np.matrix(tvecs)

此时得到的相机位置正确，但使用无畸变图像点重复操作时结果变化，代码如下：

_, rvecs, tvecs = cv.solvePnp(
        obj_points,
        undistorted_img_points,
        mtx,
        0,
        flags=cv.SOLVEPNP_ITERATIVE,
    )

rotMat, _ = cv.Rodrigues(rvecs)

camera_position = -np.matrix(rotMat).T * np.matrix(tvecs)

已验证无畸变点正确（在无畸变图像上绘制测试通过），且畸变系数设为0，尝试传入np.eye(3)替代mtx也无效。

疑问：为何两种情况下相机位置不一致？本该一致吗？还是遗漏了什么？

分析与解答 #

两种操作的结果本该一致，出现差异的核心原因是对无畸变图像点对应的内参理解有误，以下是具体分析和修正方案：

1. 无畸变图像对应的内参已发生变化 #

对原始图像做去畸变处理时，OpenCV的去畸变流程会输出新的无畸变图像，同时原始内参矩阵mtx不再适配这张无畸变图像——去畸变过程会计算出一个新的内参矩阵（通常称为newCameraMatrix），这个矩阵才是无畸变图像对应的正确内参。

直接沿用原始mtx传入solvePnP，相当于告诉算法无畸变图像的像素坐标系和原始图像内参完全一致，这和实际情况不符，必然导致位姿估计错误。

2. 修正步骤 #

生成无畸变图像点时，必须同步获取去畸变后的新内参矩阵，再传入solvePnP：

# 先获取去畸变后的新内参矩阵
newCameraMatrix, roi = cv.getOptimalNewCameraMatrix(mtx, dist, (width, height), 1, (width, height))
# 生成无畸变图像（如果之前没做这一步）
undistorted_img = cv.undistort(original_img, mtx, dist, None, newCameraMatrix)

# 提取无畸变图像点后，用newCameraMatrix替代原始mtx传入solvePnP
_, rvecs, tvecs = cv.solvePnP(
        obj_points,
        undistorted_img_points,
        newCameraMatrix,  # 使用去畸变后的新内参
        0,  # 无畸变图像的畸变系数为0
        flags=cv.SOLVEPNP_ITERATIVE,
    )

rotMat, _ = cv.Rodrigues(rvecs)
camera_position = -np.matrix(rotMat).T * np.matrix(tvecs)

3. 传入np.eye(3)无效的原因 #

np.eye(3)是单位矩阵，完全不符合无畸变图像的实际内参（内参包含焦距、主点偏移等关键信息），自然无法得到正确的位姿结果。

4. 验证方案 #

可以用第一次得到的正确位姿，反向投影3D点到无畸变图像，验证是否和提取的undistorted_img_points重合：

# 用原始流程得到的rvecs、tvecs，结合newCameraMatrix投影
projected_points, _ = cv.projectPoints(obj_points, rvecs, tvecs, newCameraMatrix, 0)
# 比较projected_points和undistorted_img_points，误差应极小

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