相机姿态估计(OpenCV PnP)

Question

我正试图从一个全球位置已知的四个受托者的图像中获得一个全球姿态估计，使用我的网络摄像头。

我检查了许多stackexchange问题和几篇论文，似乎无法找到正确的解决方案。我得到的位置数字是可重复的，但绝不与摄像机的运动成线性比例。我现在使用的是C++ OpenCV 2.1。

在这个链接图上我的坐标系和下面使用的测试数据。

% 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获得全球姿态。根据我在这个问题上读到的，从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，是一种很好的姿态存储形式。如果需要Euler角，则可以使用以下方法将DCM转换为给定XYZ旋转序列的Euler角：

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

Answer 2

如果你指的是全局姿态，一个4x4摄像机的姿态矩阵，可以在OpenGL中使用，我就是这样做的

CvMat* ToOpenGLCos( const CvMat* tVec, const CvMat* rVec )
{
    //** flip COS 180 degree around x-axis **//

    // Rodrigues to rotation matrix
    CvMat* extRotAsMatrix = cvCreateMat(3,3,CV_32FC1);
    cvRodrigues2(rVec,extRotAsMatrix);

    // Simply merge rotation matrix and translation vector to 4x4 matrix 
    CvMat* world2CameraTransformation = CreateTransformationMatrixH(tVec,
    extRotAsMatrix );

    // Create correction rotation matrix (180 deg x-axis)
    CvMat* correctionMatrix = cvCreateMat(4,4,CV_32FC1);
    /* 1.00000   0.00000   0.00000   0.00000
       0.00000  -1.00000  -0.00000   0.00000
       0.00000   0.00000  -1.00000   0.00000
       0.00000   0.00000   0.00000   1.00000 */
    cvmSet(correctionMatrix,0,0,1.0); cvmSet(correctionMatrix,0,1,0.0);
    ... 

    // Flip it
    CvMat* world2CameraTransformationOpenGL = cvCreateMat(4,4,CV_32FC1);
    cvMatMul(correctionMatrix,world2CameraTransformation,   world2CameraTransformationOpenGL);

    CvMat* camera2WorldTransformationOpenGL = cvCreateMat(4,4,CV_32FC1);
    cvInv(world2CameraTransformationOpenGL,camera2WorldTransformationOpenGL,
    CV_LU );

    cvReleaseMat( &world2CameraTransformationOpenGL );
    ...

    return camera2WorldTransformationOpenGL;
}

我认为翻转坐标系是必要的，因为OpenCV和OpenGL/VTK/等使用不同的坐标系，如图OpenGL和OpenCV坐标系所示

是这样的，但可能有人会有更好的解释。

Answer 3

摄像机的位置为{-转置(R)*t}。就这样。

除了cv::solvePnp给出(4x1)向量进行翻译，如果我还记得的话，你必须用w坐标除以x，y，z。