Posición de la cámara en coordenadas mundiales desde cv:: solvePnP

Question

Posición de la cámara en coordenadas mundiales desde cv:: solvePnP

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 correspondientes puntos en la imagen (puntos 2d).

Sé que cv::solvePnPpodría ayudarme, y después de leer estoy esto entiendo que las salidas de solvePnP rvec y tvec son la rotación y traducción del objeto en el sistema de coordenadas de la cámara.

Así que necesito averiguar la cámara rotación / traducción en el sistema mundial de coordenadas.

De los enlaces anteriores parece que el código es sencillo, 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 conozco python / numpy stuffs (estoy usando C++) pero esto no tiene mucho sentido para mí:

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

41

c++opencv computer-vision opengl pose-estimation

Author: Community, 2013-09-05

Source

2 answers

Si desea convertirlo en una matriz de pose estándar 4x4 especificando la posición de su cámara. Utilice rotM como el cuadrado superior izquierdo 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 invertirlo (para obtener la pose de la cámara en lugar de la pose del mundo)

3

Author: Hammer,
Warning: date(): Invalid date.timezone value 'Europe/Kyiv', we selected the timezone 'UTC' for now. in /var/www/agent_stack/data/www/ajaxhispano.com/template/agent.layouts/content.php on line 61
2013-09-05 19:14:56

score 45 · Accepted Answer

Si con "coordenadas del mundo" quieres decir "coordenadas del objeto", tienes que obtener la transformación inversa del resultado dado por el algoritmo pnp.

Existe un truco para invertir matrices de transformación que permite guardar la operación de inversión, que suele ser costosa, y que explica el código en Python. Dada una transformación [R|t], tenemos que inv([R|t]) = [R'|-R'*t], donde R' es la transposición de R. Por lo tanto, puede 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

Update: Later, to use T con OpenGL debe tener en cuenta que los ejes del marco de la cámara difieren entre OpenCV y OpenGL.

OpenCV utiliza la referencia generalmente utilizada en la visión por computadora: X apunta a la derecha, Y hacia abajo, Z al frente (como en esta imagen ). El marco de la cámara en OpenGL es: X apunta a la derecha, Y arriba, Z hacia atrás (como en esta imagen). Por lo tanto, es necesario 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 puede que me equivoque en algún paso, así que tome esto con un grano de sal.

Finalmente, tenga en cuenta que las matrices en OpenCV se almacenan en orden mayor de fila en la memoria, y las de OpenGL, en orden mayor de columna.