opencv - 相机姿态估计(OpenCV PnP)

Question

我正在尝试使用我的网络摄像头从四个具有已知全局位置的基准的图像中获取全局姿态估计。

我检查了很多 stackexchange 问题和一些论文，但似乎无法找到正确的解决方案。我得到的位置数字是可重复的，但绝不与相机移动成线性比例。仅供引用，我正在使用 C++ OpenCV 2.1。

At this link is pictured我的坐标系和下面使用的测试数据。

% Input to solvePnP():
imagePoints =     [ 481, 831; % [x, y] format
                    520, 504;
                   1114, 828;
                   1106, 507]
objectPoints = [0.11, 1.15, 0; % [x, y, z] format
                0.11, 1.37, 0; 
                0.40, 1.15, 0;
                0.40, 1.37, 0]

% camera intrinsics for Logitech C910
cameraMat = [1913.71011, 0.00000,    1311.03556;
             0.00000,    1909.60756, 953.81594;
             0.00000,    0.00000,    1.00000]
distCoeffs = [0, 0, 0, 0, 0]

% output of solvePnP():
tVec = [-0.3515;
         0.8928; 
         0.1997]

rVec = [2.5279;
       -0.09793;
        0.2050]
% using Rodrigues to convert back to rotation matrix:

rMat = [0.9853, -0.1159,  0.1248;
       -0.0242, -0.8206, -0.5708;
        0.1686,  0.5594, -0.8114]

到目前为止，有人能看出这些数字有什么问题吗？如果有人愿意检查它们，例如 MatLAB(上面的代码是 m 文件友好的)，我将不胜感激。

从这一点来看，我不确定如何从 rMat 和 tVec 获取全局姿态。根据我在 this question 中阅读的内容，要从 rMat 和 tVec 获取姿势很简单:

position = transpose(rMat) * tVec   % matrix multiplication

但是我从其他来源怀疑我读到的不是那么简单。

要获取相机在真实世界坐标中的位置，我需要做什么？由于我不确定这是否是一个实现问题(但很可能是一个理论问题)我希望有人在 OpenCV 中成功使用 solvePnP 函数来回答这个问题，尽管也欢迎任何想法!

非常感谢您的宝贵时间。

Answer 1

我刚才解决了这个问题，很抱歉耽搁了一年。

在我使用的 python OpenCV 2.1 和更新版本 3.0.0-dev 中，我已经验证要在全局框架中获取相机的姿势，您必须:

_, rVec, tVec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)
Rt = cv2.Rodrigues(rvec)
R = Rt.transpose()
pos = -R * tVec

现在 pos 是在全局框架中表示的相机位置(表示 objectPoints 的同一框架)。R 是一个态度矩阵 DCM，它是一种很好的存储态度的形式。如果您需要欧拉角，则可以使用以下方法将 DCM 转换为给定 XYZ 旋转序列的欧拉角:

roll = atan2(-R[2][1], R[2][2])
pitch = asin(R[2][0])
yaw = atan2(-R[1][0], R[0][0])