c++ - Posición de la cámara en coordenada mundial desde cv:: solvePnP

opengl opencv

Question

Tengo una cámara calibrada (matriz intrínseca y coeficientes de distorsión) y quiero saber la posición de la cámara conociendo algunos puntos 3d y sus puntos correspondientes en la imagen (2d puntos).

Sé que cv::solvePnP podría ayudarme, y después de leer this y this , entiendo que los resultados de solvePnP rvec y tvec son la rotación y la traducción del objeto en el sistema de coordenadas de la cámara.

Entonces necesito averiguar la rotación / traducción de la cámara en el sistema de coordenadas del mundo.

De los enlaces de arriba parece que el código es directo, en 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)

No sé cosas de python / numpy (estoy usando C ++) pero esto no tiene mucho sentido para mí:

• La salida rvec, tvec de solvePnP son matrices 3x1, 3 vectores de elementos
• cv2.Rodrigues (rvec) es una matriz de 3x3
• cv2.Rodrigues (rvec) [0] es una matriz 3x1, 3 vectores de elementos
• cameraPosition es una multiplicación de matrices 3x1 * 1x3 que es una matriz .. 3x3. ¿Cómo puedo usar esto en glRotate llamadas simples glTranslatef y glRotate ?

Answer 1

Si con "coordenadas mundiales" te refieres a "coordenadas del objeto", tienes que obtener la transformación inversa del resultado dado por el algoritmo pnp.

Hay un truco para invertir matrices de transformación que le permite guardar la operación de inversión, que generalmente es costosa, y eso explica el código en Python. Dada una transformación [R|t] , tenemos esa inv([R|t]) = [R''|-R''*t] , donde R'' es la transposición de R Entonces, puedes codificar (no probado):

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 = cv::Mat::eye(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
// T is a 4x4 matrix with the pose of the camera in the object frame

Actualización: Más tarde, para usar T con OpenGL, debe tener en cuenta que los ejes del marco de la cámara difieren entre OpenCV y OpenGL.

OpenCV usa la referencia generalmente utilizada en la visión por computadora: X señala hacia la derecha, Y hacia abajo, Z hacia adelante (como en esta imagen ). El marco de la cámara en OpenGL es: X puntos a la derecha, Y arriba, Z a la parte posterior (como en esta imagen ). Por lo tanto, debe aplicar una rotación alrededor del eje X de 180 grados. La fórmula de esta matriz de rotación está en wikipedia .

// T is your 4x4 matrix in the OpenCV frame
cv::Mat RotX = ...; // 4x4 matrix with a 180 deg rotation around X
cv::Mat Tgl = T * RotX; // OpenGL camera in the object frame

Estas transformaciones siempre son confusas y puedo estar equivocado en algún momento, así que tómenlo con un grano de sal.

Finalmente, tenga en cuenta que las matrices en OpenCV se almacenan en orden mayor de filas en la memoria, y las de OpenGL, en orden de columnas principales.

Answer 2

Si quieres convertirlo en una matriz de pose 4x4 estándar, especifica la posición de tu cámara. Utilice rotM como la esquina superior izquierda 3x3, tvec como los 3 elementos a la derecha y 0,0,0,1 como la fila inferior

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

luego inviértalo (para posar de la cámara en lugar de posar del mundo)