En este momento, tuve que repasar mis olvidados conocimientos de geometr\u00eda espacial. Me ayud\u00f3 mucho un libro llamado \u201cFundamentos de rob\u00f3tica\u201d de Antonio Barrientos, Lu\u00eds Felipe Pe\u00f1\u00edn, 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\u00f3nica y Autom\u00e1tica en la ETSII de la UPM. Adem\u00e1s, tuve cierto contacto con ellos porque hice el proyecto fin de carrera en el Departamento de Autom\u00e1tica, Ingenier\u00eda Electr\u00f3nica e Inform\u00e1tica Industrial (DISAM) de la UPM.<\/p>\n
En el Cap\u00edtulo 3 de este libro \u201cHerramientas matem\u00e1ticas para la localizaci\u00f3n espacial\u201d se describen con claridad los recursos matem\u00e1ticos necesarios para plantear y resolver el problema del cambio de coordenadas de los puntos.<\/p>\n
El cambio de posici\u00f3n del sensor se caracteriza con una matriz de transformaci\u00f3n que indica un giro alrededor de cada uno de sus ejes (X, Y, Z) y una traslaci\u00f3n del punto origen de coordenadas. Una vez conocida esta matriz, podremos trasladar los puntos de un sistema de coordenadas a otro.<\/p>\n
Para obtener esta matriz de traslaci\u00f3n podr\u00edamos usar dos m\u00e9todos:<\/p>\n
\u2013 Si tuvi\u00e9ramos caracterizada con precisi\u00f3n la geometr\u00eda de la plataforma\u00a0 de pan & tilt y conoci\u00e9ramos con precisi\u00f3n la posici\u00f3n del objetivo del sensor sobre la plataforma y pudi\u00e9ramos medir con precisi\u00f3n los giros de los servos podr\u00edamos obtener te\u00f3ricamente la matriz de transformaci\u00f3n de cada uno de los cambios de posici\u00f3n. Esto siempre y cuando la plataforma tuviera unos ajustes y tolerancias de reloj suizo.<\/p>\n
\u2013 La librer\u00eda PCL (de nuevo) y la clase pcl::IterativeClosestPoint<\/strong>. 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\u00f3n que ha llevado la nube source original a la nube m\u00e1s pr\u00f3xima a la target.<\/p>\n Como ninguno de los cuatro supuestos que har\u00edan factible el primer m\u00e9todo se cumple, s\u00f3lo nos queda la alternativa de la librer\u00eda PCL. \u00c9ste es su valor: podemos resolver por software un problema que, de otra manera, hubiera necesitado una soluci\u00f3n mec\u00e1nica m\u00e1s compleja y, por supuesto, mucho m\u00e1s cara.<\/p>\n El uso de esta clase es muy sencillo:<\/p>\n Este recurso de la librer\u00eda PCL es fundamental, pero no lo resuelve todo. Hay que trabajar en la ingenier\u00eda y generar un procedimiento de integraci\u00f3n de las distintas instant\u00e1neas en un \u00fanico escenario.<\/p>\n En alg\u00fan momento ya he comentado que el campo de visi\u00f3n en horizontal del sensor es de 58\u00ba. He obtenido una toma cada 15\u00ba, lo que supone siete tomas por cuadrante tal y como se muestran a continuaci\u00f3n. En realidad, as\u00ed se cubre un \u00e1ngulo de visi\u00f3n de 150\u00ba.<\/p>\n Para cubrir otro cuadrante necesitamos otras siete tomas. Una de ellas, la toma central numerada como la 6, es com\u00fan a los dos cuadrantes.<\/p>\n Considerar\u00e9 las coordenadas de la toma n\u00famero 6 como la referencia a la que quiero trasladar todos los puntos de las dem\u00e1s tomas, convirti\u00e9ndose, de esta manera, en la instant\u00e1nea central de la composici\u00f3n de las trece tomas que abarcar\u00e1n una amplitud de 180\u00ba. Bueno, realmente, cubren 240\u00ba de visi\u00f3n.<\/p>\n He mantenido desvinculado el proceso de adquisici\u00f3n de las trece instant\u00e1neas y el de su integraci\u00f3n sobre el sistema de coordenadas de la toma n\u00famero 6.\u00a0Por lo tanto, primero se obtienen todas las tomas grabando cada una a un fichero .pcd y despu\u00e9s se comienza el proceso de integraci\u00f3n. Esto permite depurar con comodidad el proceso de integraci\u00f3n ya que las tomas est\u00e1n grabadas en ficheros y se puede repetir este proceso partiendo siempre de la misma informaci\u00f3n.<\/p>\n En este proceso de integraci\u00f3n se pueden distinguir tres pasos, primero se acondicionan las nubes de puntos con:<\/p>\n Despu\u00e9s de esto, en un segundo paso, se obtienen \u00a0las matrices de transformaci\u00f3n de una instant\u00e1nea a otra de la siguiente manera: Se obtiene la matriz de transformaci\u00f3n entre la foto 12 (source) y la foto 11 (target), despu\u00e9s la matriz entre la foto 11 (source) y la 10 (target) y as\u00ed hasta obtener la matriz de transformaci\u00f3n entre la foto 7 (source) y la 6 (target).<\/p>\n En el otro cuadrante, se obtiene la matriz entre la foto 0 (source) y la 1 (target), despu\u00e9s la matriz entre la foto 1 (source) y la 2 (target) y as\u00ed hasta obtener la matriz de transformaci\u00f3n entre la foto 5 (source) y la 6 (target).<\/p>\n \u00bfC\u00f3mo se obtienen estas matrices de transformaci\u00f3n de una instant\u00e1nea a otra?<\/p>\n Como se puede ver en la siguiente figura dos tomas consecutivas tienen en com\u00fan \u00be partes de los puntos, es decir 45\u00ba. Se usan estos puntos comunes para obtener la matriz de transformaci\u00f3n que pasa los puntos de la foto 0 a las coordenadas de la foto 1. Para ello, le quito un slice de 15\u00ba con los puntos no comunes a cada una de las fotos. No hay una funci\u00f3n que haga esto directamente en la librer\u00eda PCL, he tenido que implementarla.<\/p>\n Ya tengo dos nubes de puntos que contienen solamente los puntos comunes pero en sus propias coordenadas.<\/p>\n Con la intenci\u00f3n de mejorar las prestaciones del algoritmo iterativo, en lugar de partir de la foto 0 como source, parto de la foto 0 rotada 15\u00ba. As\u00ed, al estar m\u00e1s cercanas las nubes de puntos, he comprobado que el algoritmo necesita menos iteraciones para obtener resultados similares.<\/p>\n En un\u00a0tercer paso, una vez que he calculado todas las matrices de transformaci\u00f3n con las instant\u00e1neas originales, puedo ir aplicando estas transformaciones a cada foto para ir cambiando las coordenadas de sus puntos a las de la siguiente.<\/p>\n En este \u00faltimo paso\u00a0se va haciendo:<\/p>\n 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\u00e1nea 6.\u00a0Se hace lo mismo con el otro cuadrante y finalmente obtenemos dos ficheros .pcl, uno por cuadrante, con las mismas coordenadas de la instant\u00e1nea 6.<\/p>\n Como estos dos ficheros tienen el mismo sistema de coordenadas para obtener un \u00fanico fichero de toda la escena solamente hay que sumarlos y pasarle un filtro de voxel de 1cm3.<\/p>\n Listo!! We did it!! \u00c9ste es el objetivo que me plantee hace unos cuantos posts. Con \u00e9sto me doy por satisfecho.<\/p>\n Pod\u00e9is ver el resultado\u00a0desde el visualizador online de PCL. Si quer\u00e9is descargar el fichero para visualizarlo localmente, pinchad aqui<\/a>.<\/p>\n\n
\n
![\"12_a_6\"](\"http:\/\/www.rtbasics.com\/WP_2\/wp-content\/uploads\/2016\/08\/12_a_6-300x170.bmp\")
<\/a> ![\"0_a_6\"](\"http:\/\/www.rtbasics.com\/WP_2\/wp-content\/uploads\/2016\/08\/0_a_6-300x170.bmp\")
<\/a><\/p>\n![\"0_a_1\"](\"http:\/\/www.rtbasics.com\/WP_2\/wp-content\/uploads\/2016\/08\/0_a_1-300x104.bmp\")
<\/a><\/p>\n\n
\n\n
\n