カメラ姿勢推定（OpenCV PnP）

Question

私は、自分のウェブカメラを使用して、既知のグローバルポジションを持つ4つの基準点の画像からグローバルな姿勢推定値を取得しようとしています。

私は多くの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からグローバルポーズを取得する方法はわかりません。私はRMATとtVecからポーズを取得するには、this questionに読んだものから、 は単純です：

position = transpose(rMat) * tVec % matrix multiplication 

は、しかし、私はそれが簡単ではありません読んだ他のソースから疑います。

実際の座標でのカメラの位置を取得するには、何が必要ですか？ 私は、これは実装上の問題（ただしほとんどの場合、理論上の問題）である場合、私はすべてのアイデアがあまりにも歓迎されているものの、この質問に答えるためにOpenCVのに成功しsolvePnP機能を使用している誰かのために希望が不明だと！

ありがとうございます。

Answer 1

私は、しばらく前に年の遅延のための謝罪を、これを解決しました。

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

今POS：私が使っていたのpythonのOpenCV 2.1、および新しいバージョン3.0.0-devに

は、私はそれはあなたが必要グローバルフレームにカメラの姿勢を取得することを確認しましたグローバルフレーム（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]) 

Answer 2

カメラの位置は{ - transpose（r）* t}になります。それでおしまい。

あなたが正しく覚えていれば、cv :: solvePnpは（4 x 1）ベクトルを変換することを除いてすべて正しく行いました。x、y、zをw座標で除算する必要があります。

Answer 3

あなたはグローバルポーズで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/etc。この絵OpenGL and OpenCV Coordinate Systems

まあに示すように、異なる座標系を使用し、それがこのように動作しますが、誰かがより良い説明があるかもしれません。