Kameraposition in Weltkoordinate von cv :: solvePnP

Question

Ich habe eine kalibrierte Kamera (intrinsische Matrix und Verzerrungskoeffizienten) und ich möchte die Kameraposition wissen, einige 3D-Punkte und ihre entsprechenden Punkte im Bild (2d Punkte).

Ich weiß, dass cv::solvePnP mir helfen könnte, und nach this und this Lesen verstehe ich, dass ich die Ausgänge der solvePnP rvec und tvec sind die Rotation und Translation des Objekts im Kamerakoordinatensystem.

Also muss ich die Kamera Rotation/Übersetzung im Weltkoordinatensystem herausfinden.

Von den Links oben scheint es, dass der Code einfach ist, in Python:

found,rvec,tvec = cv2.solvePnP(object_3d_points, object_2d_points, camera_matrix, dist_coefs) 
rotM = cv2.Rodrigues(rvec)[0] 
cameraPosition = -np.matrix(rotM).T * np.matrix(tvec) 

Ich weiß nicht, Python/numpy stopft (Ich verwende C++), aber das macht nicht viel Sinn für mich:

• rvec, TVEC Ausgabe von solvePnP sind 3x1 Matrix, 3 Elementvektoren
• cv2.Rodrigues (rvec) ist eine 3x3-Matrix
• cv2.Rodrigues (rvec) [0] ist ein 3x1 Matrix, 3 Elemente Vektoren
• cameraPosition ist eine 3x1 * 1x3 Matrix Multiplikation, die eine .. 3x3 Matrix ist. wie kann ich das in opengl mit einfachen glTranslatef und glRotate aufrufen?

Answer 1

Wenn Sie mit "Weltkoordinaten" "Objektkoordinaten" meinen, müssen Sie die inverse Transformation des vom pnp-Algorithmus angegebenen Ergebnisses erhalten.

Es gibt einen Trick zum Umwandeln von Transformationsmatrizen, mit dem Sie die Inversionsoperation speichern können, die normalerweise teuer ist und die den Code in Python erklärt. Bei einer Transformation [R|t] haben wir das inv([R|t]) = [R'|-R'*t], wobei R' die Transponierte R ist. So können Sie Code (nicht getestet):

cv::Mat rvec, tvec; 
solvePnP(..., rvec, tvec, ...); 
// rvec is 3x1, tvec is 3x1 

cv::Mat R; 
cv::Rodrigues(rvec, R); // R is 3x3 

R = R.t(); // rotation of inverse 
tvec = -R * tvec; // translation of inverse 

cv::Mat T(4, 4, R.type()); // T is 4x4 
T(cv::Range(0,3), cv::Range(0,3)) = R * 1; // copies R into T 
T(cv::Range(0,3), cv::Range(3,4)) = tvec * 1; // copies tvec into T 
// fill the last row of T (NOTE: depending on your types, use float or double) 
double *p = T.ptr<double>(3); 
p[0] = p[1] = p[2] = 0; p[3] = 1; 

// T is a 4x4 matrix with the pose of the camera in the object frame 

Update: Später T mit OpenGL zu verwenden, die Sie im Auge behalten, dass die Achsen des Kamerarahmens unterscheiden zwischen OpenCV und OpenGL.

OpenCV verwendet die Referenz, die normalerweise in Computer Vision verwendet wird: X zeigt nach rechts, Y nach unten, Z nach vorne (wie in this image). Der Rahmen der Kamera in OpenGL ist: X zeigt nach rechts, Y nach oben, Z nach hinten (wie in). Sie müssen also eine Drehung um die X-Achse von 180 Grad anwenden. Die Formel dieser Rotationsmatrix ist in wikipedia.

Diese Transformationen sind immer verwirrend und ich könnte auf einem bestimmten Punkt falsch liegen, nehmen Sie dies mit einem Körnchen Salz.

Schließlich, berücksichtigen Sie, dass Matrizen in OpenCV in Zeile-Haupt-Reihenfolge im Speicher und OpenGL-Dateien in der Reihenfolge der Spalten-Haupt gespeichert werden.

Answer 2

Wenn Sie es in eine Standard-4x4-Pose-Matrix umwandeln möchten, die die Position Ihrer Kamera angibt. Verwenden rotm wie oben links 3x3 Quadrat, TVEC wie die 3 Elemente auf der rechten Seite, und 0,0,0,1 als die untere Reihe

pose = [rotation tvec(0) 
     matrix  tvec(1) 
     here  tvec(2) 
     0 , 0, 0, 1] 

dann invertieren (erhalten von Kamerapose anstelle von Pose Welt)