Obtenga coordenadas 3D del píxel de la imagen 2D si se conocen parámetros intrínsecos y extrínsecos

Estoy haciendo la calibración de la cámara desde tsai algo. Obtuve una matriz intrínseca y extrínseca, pero ¿cómo puedo reconstruir las coordenadas 3D de esa información?

enter image description here

1) Puedo usar Eliminación Gaussiana para encontrar X, Y, Z, W y luego los puntos serán X / W, Y / W, Z / W como un sistema homogéneo.

2) Puedo usar el enfoque de documentación de OpenCV :

enter image description here

como sé, v , R , t , puedo calcular X,Y,Z

Sin embargo, ambos métodos terminan en resultados diferentes que no son correctos.

¿Qué estoy haciendo mal?

Si tienes parámetros extrínsecos, entonces tienes todo. Eso significa que puede obtener una Homografía de los extrínsecos (también llamada CameraPose). Pose es una matriz de 3×4, la homografía es una matriz de 3×3, H definida como

  H = K*[r1, r2, t], //eqn 8.1, Hartley and Zisserman 

siendo K la matriz intrínseca de la cámara, siendo r1 y r2 las dos primeras columnas de la matriz de rotación, R ; t es el vector de traducción.

Luego normaliza dividiendo todo por t3 .

¿Qué pasa con la columna r3 , no la usamos? No, porque es redundante ya que es el producto cruzado de las 2 primeras columnas de pose.

Ahora que tienes homografía, proyecta los puntos. Tus puntos 2d son x, y. Agréguelos az = 1, por lo que ahora son 3d. Proyectarlos de la siguiente manera:

  p = [xy 1]; projection = H * p; //project projnorm = projection / p(z); //normalize 

Espero que esto ayude.