Sensor de profundidad. Composición del escenario

Estaba yo pensando… ¿Cómo se compone el escenario completo?

Nuestro problema, ahora, es componer la imagen de un escenario completo de 180º contenida en un único fichero .pcd

Como ya vimos en su momento, los puntos de una nube de puntos representada por un fichero en formato pcd no necesitan mantener un orden establecido ni están sujetos a muchas restricciones. Yo diría que sólo a una: todos están referenciados al mismo sistema de coordenadas. Bueno, pues a esto se reduce nuestro problema: debemos cambiar el sistema de coordenadas referencia de los puntos de un fichero por el del otro. Es decir, los puntos del fichero source, que tienen como referencia sus propias coordenadas, debemos referenciarlos a las coordenadas del fichero target. Una vez hecho esto ya podremos añadir los puntos al fichero target.

Creo que es conveniente recordar que las coordenadas de los puntos en cada fichero tienen su origen en el propio sensor. Podéis revisarlo en este post. Por lo tanto, el problema se reduce a determinar el cambio de posición del objetivo del sensor de la foto source a la foto target.

En este momento, tuve que repasar mis olvidados conocimientos de geometría espacial. Me ayudó mucho un libro llamado “Fundamentos de robótica” de Antonio Barrientos, Luís Felipe Peñín, Carlos Balaguer y Rafael Aracil (ISBN: 84-481-0815-9). De estos cuatro autores, Barrientos, Balaguer y Aracil son viejos conocidos porque me dieron alguna clase en los cursos de la especialidad de Electrónica y Automática en la ETSII de la UPM. Además, tuve cierto contacto con ellos porque hice el proyecto fin de carrera en el Departamento de Automática, Ingeniería Electrónica e Informática Industrial (DISAM) de la UPM.

En el Capítulo 3 de este libro “Herramientas matemáticas para la localización espacial” se describen con claridad los recursos matemáticos necesarios para plantear y resolver el problema del cambio de coordenadas de los puntos.

El cambio de posición del sensor se caracteriza con una matriz de transformación que indica un giro alrededor de cada uno de sus ejes (X, Y, Z) y una traslación del punto origen de coordenadas. Una vez conocida esta matriz, podremos trasladar los puntos de un sistema de coordenadas a otro.

Para obtener esta matriz de traslación podríamos usar dos métodos:

– Si tuviéramos caracterizada con precisión la geometría de la plataforma  de pan & tilt y conociéramos con precisión la posición del objetivo del sensor sobre la plataforma y pudiéramos medir con precisión los giros de los servos podríamos obtener teóricamente la matriz de transformación de cada uno de los cambios de posición. Esto siempre y cuando la plataforma tuviera unos ajustes y tolerancias de reloj suizo.

– La librería PCL (de nuevo) y la clase pcl::IterativeClosestPoint. Mediante esta clase se implementa un algoritmo que permite minimizar la diferencia entre dos nubes de puntos de manera iterativa. Es decir, va aproximando de manera iterativa una nube source a una nube target y cuando el algoritmo finaliza se obtiene la matriz de transformación que ha llevado la nube source original a la nube más próxima a la target.

Como ninguno de los cuatro supuestos que harían factible el primer método se cumple, sólo nos queda la alternativa de la librería PCL. Éste es su valor: podemos resolver por software un problema que, de otra manera, hubiera necesitado una solución mecánica más compleja y, por supuesto, mucho más cara.

El uso de esta clase es muy sencillo:

  • Tras generar una instancia, se configura la nube de puntos source con el método setInputSource().
  • Se configura la nube de puntos target con el método setInputTarget().
  • Se configura la manera de considerar finalizada la aproximación. Hay distintas maneras de hacerlo. En este caso uso el método setMaximumIterations() para configurar el número de iteraciones que se deben hacer.
  • El algoritmo se lanza con el método align().
  • Se verifica que el algoritmo converge con el método hasConverged().
  • Se obtiene la matriz de transformación con el método getFinalTransformation().

Este recurso de la librería PCL es fundamental, pero no lo resuelve todo. Hay que trabajar en la ingeniería y generar un procedimiento de integración de las distintas instantáneas en un único escenario.

En algún momento ya he comentado que el campo de visión en horizontal del sensor es de 58º. He obtenido una toma cada 15º, lo que supone siete tomas por cuadrante tal y como se muestran a continuación. En realidad, así se cubre un ángulo de visión de 150º.

Para cubrir otro cuadrante necesitamos otras siete tomas. Una de ellas, la toma central numerada como la 6, es común a los dos cuadrantes.

Consideraré las coordenadas de la toma número 6 como la referencia a la que quiero trasladar todos los puntos de las demás tomas, convirtiéndose, de esta manera, en la instantánea central de la composición de las trece tomas que abarcarán una amplitud de 180º. Bueno, realmente, cubren 240º de visión.

He mantenido desvinculado el proceso de adquisición de las trece instantáneas y el de su integración sobre el sistema de coordenadas de la toma número 6. Por lo tanto, primero se obtienen todas las tomas grabando cada una a un fichero .pcd y después se comienza el proceso de integración. Esto permite depurar con comodidad el proceso de integración ya que las tomas están grabadas en ficheros y se puede repetir este proceso partiendo siempre de la misma información.

En este proceso de integración se pueden distinguir tres pasos, primero se acondicionan las nubes de puntos con:

  • Filtro para descartar los puntos que quedan fuera del rango del sensor.
  • Submuestreo con un voxel de 1 cm para ajustar la información a la necesaria y aligerar los tratamientos posteriores.
  • Filtro de outliers para descartar puntos aislados fruto de reflejos, errores o imprecisiones del sensor.

Después de esto, en un segundo paso, se obtienen  las matrices de transformación de una instantánea a otra de la siguiente manera:12_a_6 0_a_6

Se obtiene la matriz de transformación entre la foto 12 (source) y la foto 11 (target), después la matriz entre la foto 11 (source) y la 10 (target) y así hasta obtener la matriz de transformación entre la foto 7 (source) y la 6 (target).

En el otro cuadrante, se obtiene la matriz entre la foto 0 (source) y la 1 (target), después la matriz entre la foto 1 (source) y la 2 (target) y así hasta obtener la matriz de transformación entre la foto 5 (source) y la 6 (target).

¿Cómo se obtienen estas matrices de transformación de una instantánea a otra?

Como se puede ver en la siguiente figura dos tomas consecutivas tienen en común ¾ partes de los puntos, es decir 45º. Se usan estos puntos comunes para obtener la matriz de transformación que pasa los puntos de la foto 0 a las coordenadas de la foto 1. Para ello, le quito un slice de 15º con los puntos no comunes a cada una de las fotos. No hay una función que haga esto directamente en la librería PCL, he tenido que implementarla.

0_a_1

Ya tengo dos nubes de puntos que contienen solamente los puntos comunes pero en sus propias coordenadas.

Con la intención de mejorar las prestaciones del algoritmo iterativo, en lugar de partir de la foto 0 como source, parto de la foto 0 rotada 15º. Así, al estar más cercanas las nubes de puntos, he comprobado que el algoritmo necesita menos iteraciones para obtener resultados similares.

En un tercer paso, una vez que he calculado todas las matrices de transformación con las instantáneas originales, puedo ir aplicando estas transformaciones a cada foto para ir cambiando las coordenadas de sus puntos a las de la siguiente.

En este último paso se va haciendo:

  • Giro de 15º para colocar los puntos en la posición desde donde se calculó la matriz de transformación.
  • Se aplica la matriz de transformación mediante la función pcl::TransformPointCloud().
  • Se añaden los puntos de esta nube transformada a la nube de puntos target. Esto se hace con el operador + (suma).
  • Se aplica a la nube resultado un filtro de voxelización para volver a quedarnos con un punto por cada cm3 (centímetro cúbico).

El resultado de estos cuatro pasos se vuelve a tratar de manera iterativa girando, transformando, sumando al siguiente target y voxelizando hasta que se obtienen todos los puntos de un cuadrante en un fichero que tiene el origen de coordenadas de la instantánea 6. Se hace lo mismo con el otro cuadrante y finalmente obtenemos dos ficheros .pcl, uno por cuadrante, con las mismas coordenadas de la instantánea 6.

Como estos dos ficheros tienen el mismo sistema de coordenadas para obtener un único fichero de toda la escena solamente hay que sumarlos y pasarle un filtro de voxel de 1cm3.

Listo!! We did it!! Éste es el objetivo que me plantee hace unos cuantos posts. Con ésto me doy por satisfecho.

Podéis ver el resultado desde el visualizador online de PCL. Si queréis descargar el fichero para visualizarlo localmente, pinchad aqui.

Quiero hacer un comentario al proceso de integración que acabo de describir: Este que he descrito es el proceso “ingenuo”, he hecho todos los pasos y los he hecho uno a uno. Me parecía importante ir viendo cuales son los resultados y el rendimiento de cada paso para poder estudiar cada uno de ellos y determinar cómo se podrían mejorar. Alguna de las mejoras es evidente, como, por ejemplo, componer el giro de 15º y la transformación obtenida entre una instantánea y la siguiente en una única matriz de transformación en lugar de aplicarlas por separado a todos los puntos.

También, queda por hacer un trabajo de ajustar el procedimiento y tratar de mejorar los resultados y el consumo de CPU. De hecho, si os fijáis con cuidado en la composición de la escena se nota un poquito que no es completamente ortogonal, parece que las paredes de los lados no quedan completamente paralelas. Tengo dos hipótesis para tratar de explicar esto con las que habría que trabajar:

  • Que el propio sensor tenga un poquito de curvatura que sólo se notaría si se van componiendo fotos para ampliar la escena. Sería el mismo efecto que se observa en las fotos tomadas con un gran angular.
  • Que al haber registrado todas las fotos de un cuadrante en la misma dirección se hayan acumulado errores que provocan este efecto. Para descartar esta hipótesis se podría probar registrando las fotos al tresbolillo de manera que los efectos de los errores se vayan contrarrestando en lugar se sumarse.

En el siguiente post, y último de esta serie, me gustaría hacer algún comentario sobre qué se podría hacer a partir de aquí con este trabajo o alguna reflexión sobre cómo adaptar el hardware a un producto.

About Juan Serrano

Juan Serrano es especialista en ingeniería eléctrica y en el diseño y desarrollo de equipos de telecontrol
This entry was posted in Broadcasting, Depth sensors, Sensors and tagged , , , . Bookmark the permalink.

Leave a Reply