Ayuda con Visión por computador

Reconstrucción 3D a partir de dos imágenes

Intento realizar la reconstrucción 3D a partir de la visión estereo (dos imágenes o más) he llegado a un punto en el que me he quedado atascado. A ver si alguno de ustedes me puede orientar.

Aclaraciones:

La reconstrucción 3D lo que pretende es obtener las dimensiones 3D de un objeto a partir de varias imágenes tomadas del mismo desde diferentes posiciones(mínimo dos imágenes). Para ello yo estoy utilizando dos cámaras.

Pasos que estoy siguiendo:

1. Calibrar la cámara izquierda (Obtenemos la matriz de cámara KL)
2. Calibrar la cámara derecha (Obtenemos la matriz de cámara KR)
3. Sacar una imagen desde el lado izquierdo
4. Sacar una imagen desde el lado derecho
5. Detectar coordenadas en píxeles del objeto deseado de la imagen izquierda(mínimo 8 )
6. Detectar las mismas coordenadas en píxeles del objeto deseado en la imagen derecha(mínimo 8 )
7. Calcular la matriz fundamental a partir de los píxeles anteriormente calculados

   Para ello utilizo la función de OpenCV: cvFindFundamentalMatrix( points1, points2,8,0, fund_matrix); points1 corresponde a los puntos de la imagen de la izquierda, y points2 corresponde a los puntos de la imagen de la derecha, la matriz fundamental queda almacenada en fund_matrix.

8. calcular la matriz esencial a partir de la matriz fundamental y la matriz de cámara de ambas cámaras

   De la siguiente forma: ME=KLT x F x KR Siendo ME la matriz esencial. KLT la matriz transpuesta de KL(matriz de cámara izquierda). KR matriz de cámara derecha. F la matriz fundamental.

9. calcular la descomposición SVD de la matriz esencial

   Para ello hay que utilzar la siguiente funcion de OpenCV. void cvSVD( CvArr* A, CvArr* W, CvArr* U=0, CvArr* V=0, int flags=0 ); A es la matriz que queremos descomponer. U,W,V son la matrices en las que se descompone. Como vemos en la siguiente línea de abajo. A=U*W*VT

10. Calculamos la matriz de rotación(R)

    de la siguiente forma: U*P* VT donde P es : | 0 1 0 | | -1 0 0| | 0 0 1|

11. Calculamos la matriz de traslacion(T)

    de la siguiente forma: V*PP* VT donde PP es : | 0 -1 0 | | 1 0 0| | 0 0 1| A partir de aquí es cuando empiezan mis primeras dudas, a partir de la T de arriba, tengo que sacar un vector de 3 X 1. En uno de los documentos lo que hacen es formar esta matriz de 3 X 1, a partir del a02 y a21, de la siguiente forma: |a21| |a02| |a02|

12. Con todas las matrices halladas anteriormente resolver las siguientes dos ecuaciones independientes

    m1=[K1|0]M m2=[(K2*R)|(-K2*R*T)]*M m1 es una matriz 3X1 (coordenadas en pixeles de la primera imagen) m2 es una matriz de 3X1 (coordenadas en pixeles de la segunda imagen) K1 es una matriz de 3x3(matriz de cámara 1) K2 es una matriz de 3X3(matriz de cámara 2) R es una matriz de 3X3 (matriz de rotación) T es una matriz de 3X1 (vector de traslacion) M es el punto 3D que quiero obtener.

Mis dudas

Aplicando algebra lineal he conseguido hallar los puntos 3D M en ambas ecuaciones, el problema que he encontrado es que el eje Z, es decir, la tercera dimensión, la profundidad del objeto me la calcula de forma erronea. ¿Alguno de ustedes sabe si mi error radica en algún paso anterior? ¿Alguno de ustedes realiza la reconstrucción 3D de otra forma? Agradecería cualquier tipo de orientación. Un saludo