OpenCV solvePnP位姿估计返回错误rvec/tvec结果问题

问题现象 #

尝试使用cv2.solvePnP在pyvista（VTK的Python封装库）中实现位姿估计时，函数输出的旋转向量、平移向量结果不符合预期。
为简化验证逻辑，设计了极简测试场景：仅对相机做平移变换，尝试通过solvePnP求解位姿抵消该平移，预期solvePnP返回对应平移的逆变换。

测试中设置的相机平移矩阵为沿X轴平移1000单位的变换：

Translation = np.array([[  1.,   0.,   0., 1000.],
                        [  0.,   1.,   0.,   0.],
                        [  0.,   0.,   1.,   0.],
                        [  0.,   0.,   0.,   1.]])

实际求解得到的rvec、tvec结果为：

rvec = array([ 0., 0., -3.142]),
tvec = array([ 707.107, 408.248, 8882.736])

该结果完全不符合预期：本次相机变换仅包含平移、无旋转分量，预期求解结果中rvec为全零向量，仅tvec存在非零值，方向与原平移相反以抵消位移，即预期结果为tvec= [-1000,0,0]、rvec=[0,0,0]。若将当前得到的错误结果应用到相机上，场景内所有点都会完全消失。

复现代码 #

import cv2
import pyvista as pv
from pyvista import examples
import pyvistaqt
from vtk import vtkMatrix4x4, vtkMatrix3x3, vtkTransform
from vtk.util.numpy_support import vtk_to_numpy  
import numpy as np

np.set_printoptions(suppress=True,precision=3)

def getCamMatrix():
    
    narray = np.eye(4)
    vmatrix = plotter.camera.GetModelViewTransformMatrix()
    vmatrix.DeepCopy(narray.ravel(), vmatrix)
    return narray    

def toVTK(m,n =4):
    if n == 4:
        newMatrixVTK = vtkMatrix4x4()
    else:
        newMatrixVTK = vtkMatrix3x3()    
    for i in range(n):
        for j in range(n):
            newMatrixVTK.SetElement(i,j, m[i,j])            
    return newMatrixVTK

def applyMatrixToCam(newMatrix):
    global plotter
    newMatrixVTK = toVTK(newMatrix)    
    transform = vtkTransform()
    transform.SetMatrix(newMatrixVTK)
    transform.Update()
    plotter.camera.ApplyTransform(transform)
    pass

print("Setting up points in world coordinates")
Points =  np.array([[ 2918.972,  -887.573,   416.331,1],
        [ 2338.002,  -702.07 ,  1039.864,1],
        [ 1458.473,  -707.246,  1005.19,1 ],
        [ 1219.4  ,  -890.161,   377.004,1],
        [ 1318.727, -1017.829,  -156.537,1],
        [ 2529.132, -1026.888,  -169.222,1]])


pMesh = pv.PolyData(Points[:,:3]) # vtk object to hold the six points


plotter = pyvistaqt.BackgroundPlotter() # setting up the plotting function
plotter.enable_trackball_style()
plotter.add_mesh(pMesh)



print("Transforming from World to Image Coordinates")
# Rotating the points towards a camera at the origin, i.e. applying the default camera transform
projected = (getCamMatrix() @ Points.T)[:3,:].T 

print("store original image points")
image_points = projected.copy()[:,:2]
print("Applying the perspective transform, i.e. division by the Z-coordinate")
image_points /= projected[:,-1].reshape(-1,1) 


print("Setting up a simple translation of the camera position")
Translation = np.array([[  1.,   0.,   0., 1000.],
                        [  0.,   1.,   0.,   0.],
                        [  0.,   0.,   1.,   0.],
                        [  0.,   0.,   0.,   1.]])

applyMatrixToCam(Translation)


print("Apply the new Camera Matrix to the six points")
projected_shift = (getCamMatrix() @ Points.T)[:3,:].T

retval, rvec, tvec = cv2.solvePnP(projected_shift, np.array(image_points), np.eye(3), None, None, None, False, cv2.SOLVEPNP_EPNP)

R = cv2.Rodrigues(rvec)[0]
extrinsicReal = np.vstack([np.hstack([R.T, -R.T@tvec]), [0,0,0,1]])
applyMatrixToCam(extrinsicReal)

错误原因与修正方案 #

代码存在三个核心问题，直接导致求解结果完全偏离预期：

• cv2.solvePnP参数顺序传反。函数接口要求第一个参数为世界坐标系下的3D目标点，第二个参数为对应点在图像平面的2D投影坐标。当前代码传入的第一个参数是相机平移后得到的相机坐标系3D点，第二个参数是初始状态下的2D投影点，输入逻辑完全颠倒。
• 相机内参设置错误。代码直接传入单位矩阵np.eye(3)作为相机内参，但pyvista/VTK默认透视相机有固定视场角、裁剪面参数，对应的内参矩阵并非单位矩阵，不符合实际投影模型的内参会直接导致PnP求解失效。
• 坐标系约定不匹配。OpenCV相机坐标系为X轴向右、Y轴向下、Z轴朝向场景；VTK相机坐标系为X轴向右、Y轴向上、Z轴朝向相机反向，二者Y、Z轴方向完全相反。未做坐标系对齐直接传值，会直接求解出绕Z轴旋转π的错误旋转量，和当前得到的rvecZ分量为-3.142（即-π）的现象完全吻合。

修正步骤：

1. 调整solvePnP输入参数顺序：3D点传入固定的世界坐标点集Points[:,:3]，2D点传入相机平移后经过透视除法得到的归一化图像坐标。
2. 根据pyvista相机的视场角、渲染窗口尺寸、近裁剪面距离计算正确的相机内参矩阵，禁止直接使用单位矩阵。
3. 传入坐标给solvePnP前，先将VTK坐标系下的点转换为OpenCV坐标系格式（Y、Z坐标取反），求解得到外参后再转换回VTK坐标系应用到相机。
4. 测试阶段可将PnP求解算法替换为cv2.SOLVEPNP_ITERATIVE，该方法在少量点场景下稳定性优于EPNP。

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