Estimación de posición con openCV y Python

En el post anterior os hablé sobre como empezar con OpenCV y Python. Como punto de partida esta bien, pero se nos queda un poco corto, muy bonito todo lo de dibujar los bordes y tal… Pero todo eso ¿para qué? Esto es lo que os vengo a explicar hoy, desde mi punto de vista, una de las cosas que más me gustan de la visión artificial es la de estimar la posición de los objetos, en función de lo que se en la imagen. Y casualidad… Es lo que vamos a hacer hoy.

Para poder calcular la posiciones, distancias, y todo tipo de parámetros de una imagen necesitamos, por lo menos, tener la cámara calibrada y conocer la forma física de alguno de los objetos que vamos a detectar. Como este campo es muy amplio, vamos a ver como hacer esto aplicado a un ejemplo práctico.

A lo largo de este post vamos a ver como podemos calcular la distancia entre dos objetos, que van a estar sobre la misma superficie. Para ello vamos a seguir,grosso modo , los siguientes pasos:

1. Obtenemos la imagen
2. Reconocemos y analizamos un patrón conocido
3. Calculamos la distancia entre otros dos objetos

Introducción y objetivo

Todas las matemáticas que hay en torno a la estimación de posición de la cámara, y la relación entre los píxeles y la posición real en el mundo, se reducen más o menos a (parece mucho pero no le tengáis miedo, todavía):

Todo este locurón de matrices se puede entender si se entienden los cambios de base… Básicamente, lo que hace es pasar del mundo real a las coordenadas en pixeles. Toda la explicación con esquemas y teniendo en cuenta todas las variables, la podéis encontrar en la web oficial de openCV (aquí).

Sin embargo, nosotros vamos a intentar entenderlo un poco más por encima, pero sabiendo lo que está pasando aquí. Para verlo más fácil vamos a coger la forma reducida de la ecuación anterior, donde vamos a poner nombre a las matrices.

Vamos a suponer que somos un pixel de la camara y que vamos hacía el punto del mundo real. Lo primero que nos encontramos al salir del sensor, es la lente de la cámara. Esta lente nos va a modificar hacia donde vamos. Además, la modificación que nos da, va a ser igual siempre que usemos la misma cámara, da igual donde la coloquemos y hacia donde miremos. Por ello, dado que esta “deformación” es constante y propia de la cámara, vamos a llamar a esta matriz, la matriz intrínseca (A)

Una vez que abandonamos la cámara y vamos directos al objeto del mundo real, es cuando nos damos cuenta, de que no somos el centro del universo, y que en realidad estamos colocados en un lugar y con una orientación concreta. Y al estar colocado de una manera u otra, también condiciona lo que vemos, es decir nos “modifica”. Estos parámetros, aún usando la misma cámara, dependen de la colocación, y es muy fácil cambiarlos. Como ya no dependen de la cámara, todos los parámetros que nos dicen dónde miramos, y dónde estamos, los vamos a colocar en una matriz que llamaremos matriz extrínseca (R|t).

Ya casi estamos, solo nos queda la última puntilla. Como salimos de la cámara en línea recta y tenemos muy poca memoria, cuando nos topamos con un objeto del mundo real, con sus coordenadas X,Y,Z, no sabemos cuanta distancia hemos recorrido, así que la manera que han tenido los estudiosos de arreglar esto es poniendo una constante llamada factor de escala (s) multiplicando. De esta manera, calculas cual es tu s para cada caso, y de ahí sacas el resto de cuentas.

Definimos u, v como las coordenadas de un pixel en la imagen, y X, Y, Z las del mundo real. Con esto, si unimos todo lo anterior y decimos que el vector de pixeles [u,v,1] es m y M la posición en el mundo [X,Y,Z,1] (ese 1 es para ajustar las multiplicaciones de matrices). La ecuación nos queda:

En resumen, tenemos que para resolver la ecucación de la posición de los pixeles (m) a partir de objeto en la realidad (M), tenemos que conocer la matriz de la cámara (A), la matriz de rotación (R) y el vector de translación (t) de la camara con respecto al origen del mundo.

Pero si queremos lo contrario!! Tranquilos, guardad las antorchas, que esto es fácil, solo tenemos que despejar M de la ecuación y se nos queda:

Pero… ¿Empezamos a hacer algo!?

Ufff, menuda chapa… Si has llegado hasta aquí, enhorabuena, pero no cantes victoria todavía, que aún no llega lo divertido. Como hemos visto en la sección anterior, uno de los parámetros que hay que calcular, y que recomiendo hacerlo lo primero, es sacar la matriz A.

Lo recomiendo así, ya que una vez que la calculemos una vez, no tendremos que tocarla a no ser que cambiemos de cámara. Este proceso no es muy complicado, pero está explicado en infinidad de sitios. Solo tenéis que buscar en vuestro buscador de confianza: “Calibración cámara opencv python” y elegir el que más os guste. Si queréis que os haga un pequeño tutorial para calibrarla, podéis dejarme un comentario abajo.

Del proceso de calibración, se generan muchísimos datos. Generalmente la calibración termina con una llamada a una función tal que:

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(bla bla bla)

Lo que necesitamos para seguir son las variables mtx y dist. La primera, mtx, es la matriz que hemos llamado A, la intrínseca. Y el vector dist, es el vector que podemos pasar a nuestros algoritmos de openCV para corregir la distorsión.

No os lo vais a creer, pero lo “aburrido” ha terminado! Comenzamos con la parte divertida.

A trabajar

Conecta la WebCam!

Vamos a ir avanzando en dinamismo, llega el tiempo real. Vamos a conectar la webcam, y a partir de ahora voy a ir haciendo todo con la cámara. Es muy sencillo, solo necesitamos pasarle el número de cámara, es como un índice, generalmente si solo tenéis una cámara, poned el 0. Luego en el bucle principal vamos cogiendo cada fotograma (frame) para analizarlo.

import cv2

webcam = cv2.VideoCapture(0)

key = cv2.waitKey(1)
while not key == ord('q'):

    check, frame = webcam.read()
    if frame is None:
        break
        
    cv2.imshow("Imagen", frame)
    key = cv2.waitKey(1)
    
cv2.destroyAllWindows()

Aviso: como este código se puede ir de madre muy pronto, iré exponiendo solo los cambios, y al final del todo os dejaré el código entero, para que lo veáis.

Intenta detectar algo

Antes de empezar a detectar algo, tendremos que saber que queremos detectar… Lo más fácil para empezar es usar un rectángulo de papel con un círculo en cada esquina, cada uno de color diferente. De esta manera nos evitamos tener que hacer cuentas raras.

¿Qué me refiero con cuentas raras? En mi primer intento dibujé 2 círculos verdes en la parte de “delante” del folio, y 2 círculos azules en la parte de detrás. Os adelanto, que luego necesitamos especificar que coordenadas reales tienen los puntos entre ellos, para que luego el algoritmo intente aproximar lo que vemos en la imagen a lo que tenemos en la realidad.

Es decir, en mi caso de 2 puntos al frente y 2 detrás, separados 5cm a lo ancho, y 10cm a lo largo, como muestro en la imagen anterior. Tendríamos que si el primer punto es el (0,0,0), el punto 2 sería (5,0,0), el punto 3 (0,-10,0) y el punto 4 (5,-10,0).

Sin embargo, al detectar los colores y ver dos azules, como sabemos si el azul es el punto 3 o el 4? Para ello hay que hacer más matemáticas, y solo nos desvía del camino principal, así que haremos 4 puntos de colores diferentes y así no hay duda de cuál es cuál.

Bueno, que me lío y me voy por las ramas, tenemos nuestro mismo esquema pero cada esquina es un color diferente. Lo primero sería segmentar cada color y extraer las coordenadas de su centro. Para ello, usando el tester del post anterior tenemos que conseguir la máscara que nos extraiga solo el color que queremos.

Con los datos obtenidos, podemos generar la función que, a partir de una imagen, nos obtenga el centro de cada una de nuestras características. Por ejemplo, el siguiente código lo he comentado y creo que se entiende bastante fácil lo que hace.

import imutils

# La entrada es el frame en formato HSV
def getCentroRojo(frame_hsv):
    #Definimos los limites HSV para elegir el rojo
    limites_rojo = ((162,153,61),(193,255,178))

    #Generamos la mascara y obtenemos los contornos
    mask_rojo = cv2.inRange(frame_hsv, limites_rojo[0], limites_rojo[1])
    cnts, _ = cv2.findContours(mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)[-2:]
    cnts = imutils.grab_contours(cnts)
    
    # Si tenemos algún contorno
    if len(cnts) > 0:
       # Buscamos el que tenga más área
       c = max(cnts, key=cv2.contourArea)

       #Cogemos el círculo que más se aproxime. x,y serán las coordenadas de nuestra detección.
       ((x, y), radius) = cv2.minEnclosingCircle(c)
      
    return (x,y)

Si definimos todas nuestras funciones podríamos coger el centro de cada característica tal que:

frame= cv2.cvtColor( ... , cv2.COLOR_BGR2HSV) 
centro_rojo = getCentroRojo(frame)
centro_verde = getCentroVerde(frame)
centro_azul = getCentroAzul(frame)
centro_amarillo = getCentroAmarillo(frame)

Usando solvePnP

Hemos detectado cada una de las características para ahora poder calcular la posición en el mundo real del patrón. La manera de convertir de píxeles a coordenadas (X,Y,Z) es usando la función solvePNP.

Esta función, trata de buscar la mejor solución para la combinación de posición y rotación de la cámara, que hace que las posiciones de las características sean las que se ven en la imagen.

Para ello la cámara de necesita:

• La matriz Intrínseca(A) que hemos obtenido de la parte de calibración.
• Los coeficientes de distorsión,  que también tenemos de la calibración.
• Las posiciones originales de las características. Es decir, las coordenadas de nuestro patrón, que luego relacionaremos con los píxeles de la imagen.
• Los píxeles de nuestras características en la imagen, en el mismo orden que los hemos definido en el punto anterior.

Para el patrón que hemos puesto antes (lo vuelvo a poner para tenerlo más cerca), tendríamos que definir nuestras características tal que así:

patron = np.array([[0, 10], # Punto 1: X = 0, Y = 10
                   [5, 10], # Punto 2: X = 5, Y = 10 
                   [0,  0], # Punto 3: X = 0, Y = 0 (Origen de coordenadas)
                   [5,  0]])# Ultimo punto

Con esta definición de “objeto” todas las coordenadas que obtengamos estarán calculadas con respecto a él. Es decir, si el “origen del mundo” está en el punto rojo, todas las coordenadas que calculemos con el solvePnP, van a estar referidas a este punto.

Así que solo nos quedaría usar la función de opencv cv2.solvePnP. Si tenemos los datos que hemos puesto hace un momento, solo nos queda ponerlo en el orden correcto, y ejecutarlo. El resultado de la función es la validez del resultado un dos vectores rvec y tvec que nos devuelve los vectores de rotación(rvec) y de translación(tvec) de la cámara con respecto a nuestro patrón.

centros_ordenados = [ centro_azul , centro_verde, centro_rojo, centro_amarillo ]
ret, rvec, tvec = cv2.solvePnP(patron ,
                         np.array(centros_ordenados),
                         Cam_Matrix, # Obtenida durante la calibración
                         self.dist)  # Obtenido durante la calibración

Calculando posiciones (solo en planos)

Si ya tenemos los vectores rvec y tvec, ya podemos usar todas las matemáticas que hemos visto al principio. Recuperando del inicio, la formula para calcular las coordenadas de un punto en el mundo, a partir de la posición en la pantalla es:

Además, si por lo que sea sabemos que lo que estamos detectando está en el mismo plano (o una altura Z conocida) que nuestro patrón o modelo. Si por ejemplo, sabemos que el eje Z tiene una altura 0, es decir, está a la misma altura, podemos obtener el valor de s. Sino fuera este el caso, habría que calcular el valor de s de otra manera, pero eso se va un poco de madre para este post.

def CalcularXYZ(u,v, A, rvec, tvec, s):
    # Generamos el vector m
    uv = np.array([[u,v,1]], dtype=np.float).T

    # Obtenemos R a partir de rvec
    R, _ = cv2.Rodrigues(rvec)
    Inv_R = np.linalg.inv(R)
    
    # Parte izquierda m*A^(-1)*R^(-1)
    Izda = Inv_R.dot(np.linalg.inv(A).dot(uv))

    # Parte derecha
    Drch = Inv_R.dot(tvec)

    # Calculamos S porque sabemos Z = 0
    s = 0 + Drch[2][0]/Izda[2][0]
    
    XYZ = Inv_R.dot( s * np.linalg.inv(A).dot(uv) - tvec)
   
    return XYZ

Conclusión

Con una cámara calibrada, un patrón y un poco de ganas podéis conseguir cosas muy chulas. Por ejemplo, aquí os dejo el algoritmo funcionando en el proyecto que presenté al concurso de MakeWithAda: Un camión bombero controlado con openCV, busca un objetivo y le echa agua para apagarlo :D. Si quereis más información lo podeis ver en mi canal de Youtube!

Si os fijais están superpuestas las detecciones de los cuadrados que hacen de patrón. Además, aunque no se ve porque está detrás de las letras, el algorimo también ha encontrado el objetivo, y como sabemos que tiene que estar en el suelo, podemos calcular todos nuestro datos muy facilmente.

Espero que os haya gustado y nos vemos en la próxima!