Stima di posa della telecamera (OpenCV PnP)

Question

Sto cercando di ottenere una stima di posa globale da un'immagine di quattro fiduciali con posizioni globali note utilizzando la mia webcam.

Ho controllato molte domande di stackexchange e alcuni documenti e non riesco a trovare una soluzione corretta. I numeri di posizione che ottengo sono ripetibili ma in nessun modo linearmente proporzionali al movimento della fotocamera. Per tua informazione sto usando C++ OpenCV 2.1.

At this link is pictured i miei sistemi di coordinate e i dati di test utilizzati di seguito.

% 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] 

Finora, qualcuno può vedere niente di sbagliato con questi numeri? Lo apprezzerei se qualcuno li controllasse ad esempio MatLAB (il codice sopra è m-file friendly).

Da questo punto, non sono sicuro di come ottenere la posa globale da rMat e tVec. Da quello che ho letto in this question, per ottenere la posa da RMAT e TVEC è semplicemente:

position = transpose(rMat) * tVec % matrix multiplication 

Tuttavia ho il sospetto da altre fonti che ho letto non è così semplice.

Per ottenere la posizione della telecamera in coordinate del mondo reale, cosa devo fare? Siccome non sono sicuro che questo sia un problema di implementazione (comunque molto probabilmente un problema di teoria) mi piacerebbe per qualcuno che ha usato con successo la funzione solvePnP in OpenCV per rispondere a questa domanda, anche se qualsiasi idea è ben accetta!

Grazie mille per il vostro tempo.

Answer 1

Ho risolto il problema un po 'di tempo fa, mi scuso per il ritardo di un anno.

Nel pitone OpenCV 2.1 usavo, e la nuova versione 3.0.0-dev, ho verificato che per ottenere la posa della fotocamera nella cornice globale è necessario:

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

Ora pos è la posizione della telecamera espressa nel frame globale (lo stesso frame in cui sono espressi gli objectPoint). R è una matrice di atteggiamenti DCM che è una buona forma per immagazzinare l'atteggiamento. Se avete bisogno di angoli di Eulero, allora è possibile convertire il DCM per angoli di Eulero data una sequenza di rotazione XYZ utilizzando:

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

Answer 2

posizione della fotocamera sarebbe {- transpose (r) * t}. Questo è tutto.

E hai fatto tutto correttamente eccetto, cv :: solvePnp fornisce (4 x 1) vettore per la traduzione se ricordo bene, dovresti dividere x, y, z con la coordinata w.

Answer 3

Se vuoi dire con posa globale un 4x4 fotocamera pongono a matrice, che può essere utilizzato in OpenGL, lo faccio in questo modo

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; 
} 

Penso che lanciando il sistema di coordinate è necessario, perché OpenCV e OpenGL/VTK/etc. utilizzare diversi sistemi di coordinate, come illustrato in questa immagine OpenGL and OpenCV Coordinate Systems

Bene, funziona in questo modo ma qualcuno potrebbe avere una spiegazione migliore.