hfwr )lq gh &duuhud ,qjhqlhutd gh...

Proyecto Fin de CarreraIngeniería de Telecomunicación

Formato de Publicación de la Escuela TécnicaSuperior de Ingeniería

Autor: F. Javier Payán Somet

Tutor: Juan José Murillo Fuentes

Dep. Teoría de la Señal y ComunicacionesEscuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla, 2013

Trabajo Fin de GradoGrado en Ingeniería de las Tecnologías deTelecomunicación

Navegación de robots en interiores usandocámaras RGBD

Autor: Alejandro Braza Barba

Tutor: Jesús Capitán Fernández

Dep. Ingeniería de Sistemas y AutomáticaEscuela Técnica Superior de Ingeniería

Universidad de Sevilla

Sevilla,

Trabajo Fin de GradoGrado en Ingeniería de las Tecnologías de Telecomunicación

Navegación de robots en interiores usando cámarasRGBD

Autor:

Alejandro Braza Barba

Tutor:

Jesús Capitán FernándezProfesor Ayudante Doctor

Dep. Ingeniería de Sistemas y AutomáticaEscuela Técnica Superior de Ingeniería

Universidad de SevillaSevilla,

Trabajo Fin de Grado: Navegación de robots en interiores usando cámaras RGBD

Autor: Alejandro Braza BarbaTutor: Jesús Capitán Fernández

El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores:

Presidente:

Vocal/es:

Secretario:

acuerdan otorgarle la cali�cación de:

El Secretario del Tribunal

Fecha:

Agradecimientos

Haber tenido la oportunidad de estudiar y llegar hasta este punto, es algo que debo principalmente a mispadres, que han realizado un gran esfuerzo para que yo disfrute de este privilegio. Además han tenido

que soportarme, junto con mi hermana, en muchas ocasiones. Pero a pesar de ello me han apoyado en todomomento, sobre todo en aquellos en los que me he derrumbado emocionalmente. Por todo ello, les estaréeternamente agradecido.También quisiera dar las gracias a todos aquellos profesores que realmente se preocupan por lograr que

los estudiantes aprendan, y consiguen despertar el interés de los alumnos en cada clase y en cada sesión deprácticas. Es difícil encontrar a profesores que lo consigan, y más en la actualidad, donde cada vez se observauna peor disciplina en los estudiantes, que no se molestan en ocultar el total desinterés y falta de atenciónhacia el profesor y su asignatura. Por eso quiero agradecer el esfuerzo que todos hacen para motivar a losestudiantes, a pesar de las circunstancias desfavorables.Y por último, quisiera agradecerle a Jesús, mi tutor, su paciencia y comprensión.

Alejandro Braza BarbaSevilla, 2017

I

Resumen

En este trabajo se desarrollará una aplicación so ware basada en Robot Operating System (ROS), cuya�nalidad es estimar mediante el algoritmo Iterative Closest Point (ICP) el movimiento que va sufriendo

un robot. Para ello, el algoritmo recibirá como datos de entrada dos nubes de puntos a alinear y devolveráuna transformación que mejor las alinea (según los criterios con�gurados). Para el desarrollo de la aplicacióny la implementación del algoritmo se hará uso de datos reales de un robot, almacenados en archivos que sepueden reproducir para crear una simulación. Se hará un análisis matemático de los sistemas de referenciay las transformaciones, y su implicación en el algoritmo ICP, para posteriormente pasar al desarrollo de laaplicación.

III

Abstract

In this work, a so ware application based on ROS will be developed. Its purpose is to estimate a robot’smovement using ICP algorithm. ¿e algorithm will receive as input data two point clouds to align andwill deliver a transformation that best aligns them (following con�gured criteria). For the development ofthe application and implementation of the algorithm real robot data will be used, saved in �les that can beplayed for doing a simulation. Previously to that development, a mathematical analysis of reference systemsand transformations, and its implication in ICP algorithm will be made.

V

Índice

Resumen IIIAbstract V

1. Introducción 11.1. Objetivo 21.2. Metodología 31.3. Estructura de la memoria 3

2. Herramientas y algoritmos usados 52.1. Robot Operating System 5

2.1.1. Nombres de recursos 6Recursos dentro de los paquetes 6Recursos del grafo 6

2.1.2. Paquetes 72.1.3. Infraestructura de comunicación (Tópicos y Servicios) 82.1.4. Mensajes 10

2.2. Nubes de puntos 112.3. Algoritmo ICP 14

2.3.1. Minimización del error 142.4. Librería libpointmatcher 15

2.4.1. Configuración del algoritmo 17Filtrado de la nube de puntos de referencia 17Filtrado de la nube de puntos de lectura 18Búsqueda de correspondencias 18Filtrado de correspondencias 18Minimización del error 18Comprobaciones 19

2.4.2. Inspección y registro 19

3. Diseño e implementación de la aplicación 213.1. Análisis de los distintos sistemas de referencia implicados en el algoritmo ICP 21

3.1.1. Sistemas de referencia implicados 213.1.2. Análisis de las transformaciones entre los sistemas de referencia y su implicación en el

algoritmo ICP 223.1.3. Comprobación de la no equivalencia entre las dos formas de calcular la estimación del

algoritmo 233.2. Cálculo de la odometría visual 24

3.2.1. Estimación absoluta 243.2.2. Estimación relativa 253.2.3. Obtención de la odometría visual 25

3.3. Diseño de la aplicación 253.3.1. Implementación de las dos maneras de calcular la estimación del algoritmo ICP 26

VII

VIII Índice

Estimación absoluta 27Estimación relativa 28

3.3.2. Descripción adicional del código 283.4. Archivos launch para lanzar la aplicación 31

4. Experimentos y resultados 394.1. Experimento inicial 394.2. Análisis offline del proceso de alineación de las nubes de puntos 41

5. Conclusiones y trabajos futuros 47Apéndice A.Preparación del entorno de desarrollo 49

A.1. Instalación de ROS 49A.2. Descarga y compilación del paquete ethzasl_icp_mapping 49A.3. Creación y configuración del espacio de trabajo donde desarrollar los paquetes propios 50

Apéndice B.Sistemas de coordenadas y transformaciones 51B.1. Sistemas de coordenadas básicos 51B.2. Sistemas de coordenadas extendidos 52B.3. Transformaciones afines 53

B.3.1. Traslación 53B.3.2. Rotación 53

B.4. Cambios de base y transformaciones múltiples 54B.5. Notación 54

Apéndice C.Código 57C.1. Código del nodo visual_odom 57C.2. Código del nodo tf_from_odom 66C.3. Código del nodo pointcloud_transform_saver 68C.4. Funciones para transformar mensajes de ROS a tipos de datos de libpointmatcher 74

Índice de Figuras 79Índice de Tablas 81Índice de Códigos 83Bibliografía 85Índice alfabético 87Glosario 89

1 Introducción

Somos como enanos aupados a hombros de gigantes, demanera quepodemos ver más cosas y más lejanas que ellos, no por la agudezade nuestra vista o por nuestra elevada estatura, sino porque estamosalazados sobre ellos y nos elevamos sobre su altura gigantesca

Bernardo de Chartres, s.XII

Desde los orígenes, la humanidad ha tratado de crear artilugios que le hicieran la vida más fácil, ayudaranen la resolución de problemas, y le permitieran liberarse de la necesidad de realizar ciertos esfuerzos.

El concepto de robot surgió a principios del siglo XX en la obra teatral R.U.R.[] del escritor checo KarelCapek, el propósito al que estaban destinados era el de servir al hombre. En esta obra, los robots eran seresbiológicos con aspecto humano fabricados de manera arti�cial, pero el concepto ha ido evolucionando de talforma que actualmente, una posible de�nición de la palabra es la siguiente: "Una máquina -especialmenteuna programable por un ordenador- capaz de llevar a cabo una serie compleja de acciones automáticamente".El primer ámbito en donde la implantación de los robots ha sido masiva, ha sido en las cadenas de fabricacióny montaje de las industrias, desde que en se colocara el primer brazo robótico en una fábrica de GeneralMotors[]. Sin embargo, las aplicaciones de los robots no se reducen a tareas repetitivas en una cadena demontaje. Sus aplicaciones actuales y futuras son múltiples, diversas y tan dispares como: cirugía, búsqueda yrescate de personas, desactivación de bombas, minería, exploración espacial, limpieza del hogar, asistencia ysupervisión de personas mayores, transporte, reparación de infraestructuras, aplicaciones militares (lamenta-blemente), ... . Son especialmente apropiados para trabajos en ambientes hostiles, sucios y/o peligrosos parael ser humano, como pueden ser: un edi�cio en peligro de derrumbe, inspección de centrales nucleares confugas radioactivas, desactivación de explosivos, etc. También son muy convenientes para sustituir al hombreen tareas repetitivas y que necesitan una gran precisión, ya que pueden llegar a ser mucho más e�cientes queun ser humano en esas mismas circunstancias.Ha sido precisamente la industria y las cadenas de montaje, el ámbito al que se ha limitado el progreso en

el mundo de la robótica hasta hace relativamente poco tiempo, cuando se ha comenzado a dedicar mayoresfuerzo en la investigación de nuevos posibles usos de los robots. Este nuevo rumbo que ha tomado larobótica, se debe sobre todo a la mayor potencia de cálculo que poseen los ordenadores actuales, el desarrollode nuevos sensores y la mejora en la relación capacidad/peso y �abilidad de las baterías. Esta nueva direcciónde investigación obliga a la creación de robots con una mayor complejidad cognitiva, ya que el robot debemoverse de manera autónoma o semi-autónoma (modo de operación asistido por un operador, el cuál indicatareas de alto nivel) en un entorno dinámico, e incluso desconocido completamente, y en el que además sepueden presentar obstáculos y personas tanto �jos como enmovimiento. Mientras que los robots industrialesoperan en un entorno estático y conocido, y carecen de la necesidad de percibir el entorno, más allá de ciertosmecanismos de seguridad y los sensores necesarios para realizar la tarea concreta a la que están destinados.Para operar en estos nuevos entornos, los robots necesitan hardware y so ware de navegación que les

permitan percibir el entorno y tomar decisiones en base a los datos captados por los sensores. Entre lossensores usados, se incluyenGlobal Positioning System (GPS), Radio Detection and Ranging (RADAR), LightDetection and Ranging (LIDAR), cámaras de vídeo, Inertial Measurement Unit (IMU), encoders, pero no son La palabra robot se deriva de robota, que signi�ca "trabajo forzado" en checo

1

2 Capítulo 1. Introducción

los únicos. De todos ellos, el LIDAR se sitúa como uno de los fundamentales a la hora de percibir el entorno,debido a la gran cantidad de información que se puede extraer de las nubes de puntos que el sensor capta.Es por ello muy útil en el problema denominado Simultaneous Location and Mapping (SLAM). El principalinconveniente de este tipo de sensores es su alto precio, lo que los hace inaccesibles para la gran mayoría, loque a su vez perjudica a la investigación y al progreso de la robótica. Por esta razón, la aparición en el mercadode la cámara Kinect de Microso , ha supuesto una gran revolución en el mundo de la robótica, debido a suprecio contenido y a las prestaciones similares a las de un LIDAR que ofrece.La cámara Kinect, es una cámara RGBD, es decir, capta imágenes en color y además mide la profundidad

asociada a los píxeles de la imagen. Por lo que realizando un procesamiento a esas imágenes de profundidad,conociendo ciertos parámetros ópticos de la cámara, se puede obtener una nube de puntos similar a la cap-turada por un LIDAR. Con la diferencia de que el alcance, precisión y campo de visión son menores, y quesu uso está limitado a entornos donde la luz solar no incida de manera directa, ya que la radiación infrarrojadel sol es ruido que inter�ere en la medición de las distancias, pudíendose producir errores de medida. Estoúltimo se debe al modo en el que la cámara estima las distancias, que consiste en emitir un patrón infrarrojohacia el entorno, y analizando el re�ejo de este patrón (mediante su captura por un sensor infrarrojo), estimarla distancia asociada a cada uno de los píxeles de la imagen captada.

Figura 1.1 Cámara Kinect.

Uno de los problemas que presentan los robots es que la odometría calculada a partir de las señales delos encoders no es del todo precisa, y siempre sufre una deriva. Este error puede aliviarse combinando lainformación de los encoders con la de otros sensores como el GPS y la IMU. Sin embargo, en algunos entornoscerrados, no se recibe señal GPS. Por lo que resulta necesario buscar una alternativa que permita al robotestimar su movimiento dentro de estos entornos.Un caso donde se presenta este problema es el del robot Sewer Inspection Autonomous Robot (SIAR)[], el

cuál está destinado a inspeccionar la red de alcantarillas, en busca de grietas, atascos, etc. De esta manera losoperarios podrían inspeccionar de manera remota este entorno tan sucio y posiblemente tóxico para el serhumano. La capacidad del robot de generar un mapa tridimensional detallado del entorno es fundamental eneste caso, ya que una simple imagen plana puede no permitir apreciar una grieta o cualquier otro desperfecto.Además es importante conocer la posición de éste con lamayor exactitud para así dirigirlo demanera correctay mantenerlo localizado en todo momento dentro de la compleja red de alcantarillado.

Figura 1.2 Robot SIAR.

1.1 Objetivo

El objetivo de este trabajo es crear una aplicación so ware que ofrezca una alternativa para estimar la posicióndel robot, evaluando las posibilidades que ofrecen las nubes de puntos capturadas por una cámara RGBD. Para mayor detalle sobre qué es una nube de puntos, ver la sección .

1.2 Metodología 3

Se hará uso del algoritmo ICP para estimar el movimiento relativo que va experimentando el robot entresucesivos instantes de tiempo, y de esta manera se obtendrá un valor de odometría que puede ser combinadocon los obtenidos mediante otros métodos (encoders, IMU, GPS,...), de manera que el robot dispondría delos datos odométricos proporcionados por los encoders de las ruedas, dispondría también de información dela IMU, y también de la odometría estimada mediante el procesamiento de las nubes de puntos.

1.2 Metodología

Para el desarrollo del so ware se hará uso de ROS, en su versión Kinetic Kame, sobre el sistema operativoUbuntu . LTS, tal como se indica en el anexo A. El lenguaje de programación usado será principalmenteC++ , y se usará como herramienta de compilación la proporcionada por ROS (catkin), con un mecanismode gestión de dependencias sencillo y e�caz.

1.3 Estructura de la memoria

Esta memoria se divide en los siguientes capítulos:

• Capítulo , Introducción: Se presenta el tema del trabajo, haciendo una introducción general, paraposteriormente centrarse en el problema especí�co a resolver. Demostrando la vigencia actual del tema.Y su importancia pŕactica.

• Capítulo , Herramientas y algoritmos usados: Aquí se hace un breve repaso sobre las característicasde ROS, se habla también de las nubes de puntos y su representación en ROS, y se explica brevementeen qué consiste el algoritmo ICP y sus fases.

• Capítulo , Diseño e implementación de la aplicación: En primer lugar, se presenta el análisis preliminarde los sistemas de referencia del robot y de las transformaciones entre ellos. De este análisis, obtenemoscomo conclusión que podemos hacer uso de dos métodos para calcular la odometría visual. Poste-riormente se explican los detalles de diseño e implementación de la aplicación, en concreto aquellosrelacionados con el cálculo de la odometría visual. También se explica el desarrollo de �cheros launchy de con�guración para automatizar el inicio de la ejecución de la aplicación.

• Capítulo , Experimentos y resultados: Aquí se muestran las diferentes pruebas que se han ido reali-zando, haciendo uso de la aplicación desarrollada y de los datos reales del robot SIAR grabados en un�chero bag.

• Capítulo , Conclusiones y trabajos futuros: Para terminar, se hace una re�exión acerca de los resultadosobtenidos, valorando si se ha alcanzado el objetivo buscado. Se consideran posibles mejoras a realizarsobre el trabajo desarrollado, y se establece cuál podría ser la línea de investigación para futuros trabajosque continúen el actual.

Y los siguientes anexos:

• Anexo A, Preparación del entorno de desarrollo: En él se detallan los pasos a seguir para prepararel entorno de desarrollo, tanto el sistema operativo, como la compilación de las librerías de las quedependerá la aplicación.

• Anexo B, Sistemas de coordenadas y transformaciones: Aquí se presenta un resumen de la notación ylas expresiones matemáticas útiles para tratar con sistemas de referencia, ya que su uso es fundamentalen la aplicación. Siendo el análisis de éstos, una parte destacada del diseño de la aplicación y por lotanto de este trabajo.

• Anexo C, Código: Se muestran los distintos archivos de código que componen la aplicación.

2 Herramientas y algoritmos usados

En éste capítulo se hablará de las tecnologías usadas y los algoritmos. Más concretamente se hará un repasosobre los conceptos fundamentales de ROS(la infraestructura elegida para el desarrollo del so ware):

nodos, tópicos, servicios, mensajes y paquetes; se expondrá brevemente el concepto de nube de puntos ysu implementación en ROS; se explicarán las distintas fases del algoritmo ICP; y se presentará la libreríalibpointmatcher que ha sido usada para el desarrollo del so ware.

2.1 Robot Operating System

Desarrollar un sistema robótico desde cero, no es algo sencillo, debido a la multitud de tareas, situacionesy ambientes con los que el robot debe lidiar satisfactoriamente. Por lo tanto, hay gran cantidad de procesosy acciones que deben ejecutarse simultáneamente y de manera coordinada, cada una de ellas encargada deuna función especí�ca y necesaria para el control del robot. Sin embargo, es complicado encontrar a alguienexperto en todas y cada una de las distintas áreas de conocimiento involucradas en el desarrollo de estetipo de sistemas. A esto hay que añadir que es poco e�ciente diseñar completamente desde cero cada nuevosistema robótico, cuando hay partes estándar que deben estar siempre presentes y que pueden ser reutilizadasaplicando una con�guración especí�ca para el sistema en el que se vayan a emplear. Por lo tanto, ¿por qué noaunar esfuerzos entre los distintos grupos de investigación dedicados al desarrollo de la robótica?, y que cadauno aporte el componente del sistema en el que se haya especializado. Con ese objetivo, surgió ROS. Diseñadodesde un primer momento para favorecer la colaboración, la reutilización de so ware, y la resistencia a fallos,basándose para ello en un sistema de desarrollo y ejecución modular.ROS es un sistema o framework para el desarrollo de so ware robótico. Está compuesto por herramientas,

librerías de código y convenciones útiles para crear nuevo so ware. La modularidad se consigue mediante eldiseño de soluciones especí�cas en éstos dos ámbitos:

• Desarrollo: La creación y distribución del so ware se realiza mediante paquetes, siguiendo una �losofíasimilar a la de Debian. Es decir, no está todo encapsulado en un único archivo, ya que ROS es modular,y permite que cada usuario elija qué paquetes instalar, además de proveer las herramientas para lacreación de paquetes propios.

• Ejecución: La comunicación entre cada uno de los ejecutables que intervienen en el control del robot serealiza a través de procedimientos estándar diseñados especí�camente para ROS. La única restricciónque imponen éstos mecanismos de comunicación es en el formato de los mensajes, que debe coincidirpara ambos extremos de la comunicación.

Al no existir mayor restricción que el formato de los mensajes, esto permite que cualquier ejecutable sepueda comunicar con cualquier otro, siempre que conozca el formato que debe emplear para ello. Pero nosólo eso, también permite que un ejecutable pueda ser sustituido por otro de similar funcionalidad siempreque los mensajes que envíe y recibamantengan el formato. Y por último, y es lo que proporciona robustez antefallos, si un ejecutable sufre algún tipo de error que provoca su terminación, los demás seguirán su ejecución,evitando una posible pérdida del control del robot (en el caso de que el ejecutable afectado no fuera uncomponente crítico). Esto se consigue gracias a que no todas las interfaces de comunicación declaradas porun ejecutable deben estar activas en todo momento. Pueden no usarse algunas de ellas, y la desconexión entiempo de ejecución no supone un error fatal.

5

6 Capítulo 2. Herramientas y algoritmos usados

2.1.1 Nombres de recursos

En ROS existen convenciones de nombrado que aportan una gran facilidad en el acceso y uso de los recursos.Sin los mecanismos de nombrado y traducción de nombres, provistos por ROS y sus librerías, sería aún máscomplicado la construcción de sistemas complejos. Los recursos se pueden clasi�car en dos grupos:

• Los recursos correspondientes a los paquetes de ROS presentes en el sistema.

• Los recursos del grafo de computación actualmente en ejecución: Es decir nodos,tópicos, servicios yparámetros.

Recursos dentro de los paquetes

Existen más tipos de recursos que pueden estar contenidos dentro de un paquete, pero aquellos tres para losque existe una forma abreviada para referirse a ellos son los archivos de de�nición demensajes y servicios, ylos nodos. Según estas reglas, cada recurso puede ser identi�cado de la siguiente manera:

• Mensajes: En el caso de los archivos de de�nición de mensajes, éstos pueden ser localizados siempre enun subdirectorio estándar dentro de los paquetes de ROS. Un ejemplo de ruta de acceso a uno de éstosarchivos sería prefijo/nombre_paquete/msg/Mensaje.msg. Al estar estos archivos situados siem-pre en elmismo subdirectorio, tener lamisma extensión, y ROS proveer las herramientas para encontrarla ruta a cada uno de sus paquetes; el identi�cador del mensaje puede ser reducido a nombre_paque-te/Mensaje. Y podríamos mostrar en línea de comandos el contenido del mensaje mediante la orden:

rosmsg show nombre_paquete/Mensaje

• Servicios: Al igual que los archivos de de�nición de mensajes, éstos pueden ser localizados bajo unsubdirectorio estándar, en este caso srv. Y el archivo de de�nición situado en la ruta prefijo/nom-bre_paquete/srv/Servicio.srv, puede ser identi�cado simplemente como nombre_paquete/-Servicio. Su de�nición puede ser mostrada en el terminal mediante:

rossrv show nombre_paquete/Servicio

• Nodos: Los nodos presentes en un paquete se caracterizan por ser archivos con permiso de ejecución,por lo que para identi�carlos sólo es necesario conocer el nombre del paquete en el que se encuentrany el nombre del nodo en cuestión, ya que las utilidades de ROS se encargarán de buscar ese ejecutabledentro de las localizaciones estándar. Por ejemplo, para ejecutar un nodo llamado key_teleop.pyperteneciente al paquete key_teleop, ejecutariamos la orden:

rosrun key_teleop key_teleop.py

Los nombres tanto de los paquetes comode los recursos, deben comenzar por un carácter alfabético seguidoúnicamente de guiones bajos y carácteres alfanuméricos. El resto de recursos, no disponen de una formaabreviada de acceder a ellos, pero pueden ser accedidos conociendo su ruta relativa con respecto a la raízdel paquete. Por ejemplo, para acceder a un archivo dentro del subdirectorio images incluido en el paqueteturtlesim podríamos usar la ruta obtenida al ejecutar el comando siguiente:

‘rospack find turtlesim‘/images/kinetic.svg

Recursos del grafo

Los recursos del grafo de computación de ROS son los indexittópicos,servicios, parámetros y nodos. Los nom-bres que identi�can a cada recurso se organizan en una estructura jerárquica similar a la de los sistemas de�cheros de un sistema operativo. En ningún momento se indica en la documentación de ROS, pero cada unode los tipos de recurso pertenece a un árbol de nombrado diferente. Es decir, puede existir un tópico, un servi-cio, un parámetro y un nodo que simultáneamente compartan el mismo nombre sin que haya ningún tipo decon�icto. Todos los nombres parten del espacio de nombres global identi�cado como /, y cada nivel inferiores un nuevo espacio de nombres. Otra característica de este mecanismo de nombrado, es que el nombre de unrecurso puede ser a su vez espacio de nombres, es decir, haciendo una analogía con los sistemas de �cheros,es como si un archivo fuera a la vez un directorio.Al igual que en los sistemas de �cheros, donde existen rutas absolutas y relativas, aquí también existe un

mecanismo similar. Para ello existen cuatro tipos de nombres, con una sintáxis asociada:

2.1 Robot Operating System 7

• Base: nombre_base

• Relativo: nombre/relativo

• Global: /nombre/global

• Privado: ~/nombre/privado

Estos nombres son usados dentro del código de los nodos, y por lo tanto se resuelven (se traducen a unnombre global) según el espacio de nombres al que pertenezca el nodo. Los nombres de tipo base y relativose resolverían añadiendoles como pre�jo el espacio de nombres del nodo. Los nombres globales no necesitanser resueltos, mientras que para resolver los nombres privados, se antepone el nombre global del nodo comoespacio de nombres. A continuación se muestran algunos ejemplos:

• Nombre privado: Teniendo un nodo con nombre /ns/nodo, el nombre privado ~/param se resolveríacomo /ns/nodo/param.

• Nombre relativo: Siendo/ns el espacio de nombres delnodo, el nombre relativonxt/nxt_base_controllerse resolvería como /ns/nxt/nxt_base_controller.

2.1.2 Paquetes

Como ya se ha dicho, el so ware de ROS se organiza en paquetes. Estos paquetes contienen el código de losejecutables (nodos), librerías, �cheros de cabecera, �cheros de con�guración, conjuntos de datos, de�niciónde mensajes y servicios, y en general cualquier archivo que el desarrollador considere oportuno incluir y quesea coherente con el resto del contenido del paquete. El contenido de estos paquetes suele organizarse segúnuna estructura de directorios estándar similar a la que se muestra en la �gura .. Este árbol de directorios asu vez, durante el desarrollo del paquete, está anidado dentro del subdirectorio src de un espacio de trabajoque puede contener varios paquetes.

nombre_paquete/

include/

nombre_paquete/ .......................................Ficheros de cabecera

msg/...........................................................Definición de mensajes

src/

nombre_paquete/ .......................................Código fuente

srv/ .........................................................Definición de servicios

scripts/ ....................................................Scripts de Python, LISP,...

CMakeLists.txt ............................................Instrucciones de compila-ción

package.xml ................................................Información acerca delpaquete

CHANGELOG.rst .............................................Registro de cambios

Figura 2.1 Estructura de directorios de un paquete de ROS.

Los archivos package.xml y CMakeLists.txt son fundamentales para la correcta compilación y poste-rior ejecución del so ware incluido en el paquete. Una de las funciones principales de estos archivos es indicarlas dependencias que el paquete en cuestión tiene con respecto a otros paquetes de ROS o del sistema (en elcaso de usar Ubuntu, las dependencias del sistema se resuelven instalando paquetes Debian). Se provee estemecanismo de dependencias ya que amenudo unos paquetes hacen uso de librerías, herramientas, ejecutables,datos, mensajes, y otros recursos pertenecientes a otros paquetes tanto de ROS como del sistema. Se puede hacer de manera automática usando la herramienta rosdep


Además de las dependencias, el archivo package.xml de�ne otras propiedades, tales como el nombre, laversión, una descripción, un correo electrónico de contacto con el encargado del mantenimiento del paquete yel tipo de licencia aplicable al contenido del paquete. Paramás información acerca de la estructura y contenidode éste archivo se puede consultar la referencia [].El archivo CMakeLists.txt contiene información especí�ca necesaria para el sistema de compilación usa-

do por ROS, es decir catkin. Este sistema de compilación hace uso de CMake, extendiéndo su funcionalidada través de macros. Los detalles acerca de éste archivo se pueden leer en la referencia [].

2.1.3 Infraestructura de comunicación (Tópicos y Servicios)

Cada proceso involucrado en la red o grafo de comunicación de ROS, es denominado nodo. Estos nodospueden estar localizados en un mismo equipo físico, o distribuidos en varias máquinas conectadas en red.Lo cuál permite que, por ejemplo, los nodos que sean críticos para el control del robot se ejecuten en suordenador de a bordo, y aquellos nodos de supervisión y/o control remoto sean ejecutados en un terminalque maneje el operador.La comunicación entre todos ellos, se coordina gracias a un nodo central denominadomaster, cuya función

es mantener un registro de todos los nodos activos y los tópicos en los que publican y a los que se suscriben,junto a los servicios que ofrecen. Los tópicos y los servicios son conceptos fundamentales sobre los que el restodel sistema se construye.Podemos considerar a los tópicos como buses virtuales de comunicación unequivocamente identi�cables

mediante un nombre único, sobre los que sólo un tipo concreto de mensaje puede ser enviado. Varios nodospueden hacer uso simultáneamente de un mismo tópico, tanto publicando mensajes en él, como suscribién-dose a ellos. Suscriptores y publicadores no son conscientes de la existencia de los otros hasta el momentode la solicitud de conexión por parte del otro extremo, lo que desacopla la producción de la informaciónde su consumo. El concepto abstracto de tópico permite considerar que todos los nodos que hacen uso deun tópico concreto, lo hacen a través de un canal de comunicación único y compartido entre todos. Estaabstracción es útil para diseñar el sistema despreocupándose de la implementación, la cuál consiste en crearuna conexión punto a punto entre cada publicador y suscriptormediante el uso de sockets tanto TransmissionControl Protocol (TCP) como User Datagram Protocol (UDP) dependiendo de los requisitos de latencia ypérdida de paquetes que la aplicación concreta exija. Una vez establecida la comunicación entre nodos, elmaster ya no interviene entre ellos. En la �gura . se muestra un ejemplo en el que existen dos suscriptores ydos publicadores que comparten un tópico común, y en el que ya se ha realizado el establecimiento de todaslas conexiones punto a punto necesarias para el intercambio de los mensajes. Aunque las conexiones seanpunto a punto, no se hace distinción entre los mensajes enviados a distintos suscriptores, todos reciben elmismo.

/nodo_publicador_1 /nodo_suscriptor_1

/nodo_publicador_2

/nodo_suscriptor_2

/topico_comun

/topico_comun

/topico_

comun

/topico_comun

Figura 2.2 Conexión entre múltiples publicadores y múltiples suscriptores.

El proceso por el que una conexión entre dos nodos es establecida es el siguiente:

1. El suscriptor se registra en el nodomaster, indicando el tópico del que desea recibir mensajes.

2.1 Robot Operating System 9

2. El publicador se registra en el nodomaster, indicando el tópico en el que publicará mensajes, junto conel nombre de red o dirección IP y puerto a través de los cuáles cualquier otro nodo puede comunicarsecon él para negociar una conexión a ese tópico.

3. El master informa al suscriptor de la existencia del publicador (este mensaje es asíncrono, y se envíacada vez que la lista de publicadores se actualiza).

4. El suscriptor contacta con el publicadormediante el protocolo xmlrpc, usando la dirección IP y el puertoregistrado en elmaster. Así solicita una conexión al tópico deseado e indica el protocolo de transportepreferente.

5. El publicador envía al suscriptor el protocolo de comunicación �nalmente seleccionado por el primeroy la dirección IP (o nombre de red) y el puerto al que debe conectarse para recibir los mensajes.

6. El suscriptor se conecta al publicador usando los parámetros indicados por éste último.

Este proceso se ilustra en la �gura ..

publicador suscriptor

mastersuscribir(“nom

bre_tópico”)

publ

icar

(“nom

bre_

tópi

co”,

IP:p

uerto

_xm

lrpc_

pub)

respuesta: {IP:puerto_xmlrpc_pub}

conectar(“nombre_tópico”, Protocolo deseado)

respuesta: {protocolo seleccionado, IP:puerto_topico}

conectar(IP:puerto_topico)

publicación de mensajes

XMLRPC

TCP/UDP

Figura 2.3 Establecimiento de conexión entre suscriptor y publicador.

Este mecanismo de comunicación permite una gran �exibilidad en tiempo de ejecución, ya que los nodosse pueden arrancar y parar en cualquier orden y en cualquier momento, al no exister dependencias estrictasimpuestas por la interfaz de comunicación. Esta característica a su vez proporciona tolerancia a fallos, ya queen el caso de que un único nodo caiga, y como consecuencia pierda la conexión con los demás, este puederearrancarse y establecer la conexión de nuevo.En el caso de los servicios, el funcionamiento es ligeramente distinto. A diferencia del anterior mecanismo,

en el que varios nodos pueden publicar mensajes en el mismo tópico; solo un nodo puede proveer un deter-minado servicio. Para hacer uso de éste, un nodo cliente enviará un mensaje de petición y esperará recibir unmensaje de respuesta. El procedimiento para el establecimiento de la conexión y el intercambio de mensajesse puede resumir en los siguientes pasos:

1. El nodo proveedor del servicio se registra en elmaster indicando su dirección IP o nombre de red y elpuerto TCP a través del que se puede acceder a él.

2. El nodo cliente solicita almaster la dirección de contacto registrada para el servicio.3. El cliente establece una conexión TCP con el servicio e intercambian una cabecera de conexión en la que

se incluye entre otras cosas el formato de los mensajes a usar y la suma md de estos (para comprobarque ambos extremos están usando exactamente el mismo formato).

4. El cliente envía la petición.5. El proveedor del servicio envía el mensaje de respuesta.

Todos los demás mecanismos de comunicación más complejos y elaborados existentes en ROS, se basaninternamente en el uso de éstos otros. Será identi�cado de forma genérica como puerto_xmlrpc_pub. Será identi�cado de forma genérica como puerto_topico.


2.1.4 Mensajes

El formato de los tipos de mensajes intercambiados a través de los tópicos, se de�ne en unos archivos especí�-cos con extensión msg. Cada uno de estos archivos está almacenado bajo el subdirectorio msg del paquete deROS en el que el mensaje esté de�nido. Los tipos de mensaje se identi�can mediante el nombre del paquetede ROS en el que se de�nen, seguido del nombre del archivo de de�nición, sin la extensión. Por ejemplo, elarchivo geometry_msgs/msg/Twist.msg de�ne el mensaje geometry_msgs/Twist.Estos archivos son utilizados durante la compilación de los paquetes de ROS para crear �cheros de cabecera

para cada uno de los lenguajes de programación soportados. Estos �cheros de cabecera incluyen una sumaMD que permite a los nodos comprobar que están usando la misma versión del mensaje para comunicarse.En el caso de que el tipo de mensaje o la suma MD no coincidan se rechaza la conexión.El contenido de estos archivos consiste en una lista de campos de datos y de�nición de constantes. Cada

campo de datos y cada constante tienen asociados un tipo de datos y un nombre. En la de�nición de lasconstantes además, se añade el valor de ésta detrás de un símbolo "=" inmediatamente posterior al nombre.

tipo_de_dato nombre_del_campotipo_constante NOMBRE_CONSTANTE=valor_constante

Los tipos de datos que pueden ser usados son:

• Un tipo primitivo.

• Mensajes ya de�nidos en otros archivos msg, en el mismo paquete o en otro.

• Un array con tamaño �jo o variable, cuyos elementos sean de un único tipo de datos (puede ser cual-quiera de los indicados anteriormente).

Los tipos de datos primitivos son los siguientes:

• bool

• int

• uint

• int

• uint

• int

• uint

• int

• uint

• �oat

• �oat

• string

• time

• duration

En el código . se muestra un ejemplo de de�nición de un tipo de mensaje.

Código 2.1 De�nición de un mensaje de ejemplo.

# Definición de un campo de datos con un tipo primitivoint32 temperatura

# Definición de un campo de datos con un tipo compuestosensor_msgs/BatteryState estado_bateria

# Definición de un array de tamaño fijo cuyos elementos son# de un tipo de datos no primitivogeometry_msgs/Point[4] array_puntos

# Definición de un array de tamaño variable con tipo de datos primitivobool[] array_booleano

# Definición de constantesfloat64 CONSTANTE=9.413bool VERDADERO=Truestring HELLOWORLD=Hola Mundo

Si el mensaje que estamos de�niendo, y el mensaje que vamos a incrustar en él pertenecen al mismo paquete, haremos referencia aéste último únicamente con el nombre

En este caso al estar de�nido en otro paquete, debemos incluir el nombre del paquete como pre�jo al nombre del mensaje

2.2 Nubes de puntos 11

Una vez de�nido este mensaje, y si suponemos que pertenece al paquete de ROS paquete_ejemplo y elnombre del mensaje es MensajePrueba, podemos hacer uso de él en nuestro código en C++ de la maneraen que se muestra en el código ..

Código 2.2 De�nición de un mensaje de ejemplo.

#include "paquete_ejemplo/MensajePrueba.h"

int main(){geometry_msgs::Point punto;punto.x = 1;punto.y = 3;punto.z = 0;// Declaración del objeto creado a partir de la definición// del mensajepaquete_ejemplo::MensajePrueba msg;// Acceso al campo temperatura a través del atributo del mismo// nombre del objeto msgmsg.temperatura = 21;// Acceso al campo capacity del mensaje sensor_msgs/BatteryState// anidado dentro del mensaje MensajePruebamsg.estado_bateria.capacity = 68;// Acceso a los elementos del array de tamaño fijo// (El tipo de datos del array es boost::array)msg.array_puntos[0] = punto;msg.array_puntos.at(1) = punto;msg.array_puntos.back().x = msg.CONSTANTE;*msg.array_puntos.rbegin() = punto;msg.array_booleano.push_back(true);if(msg.VERDADERO) {std::cout<<msg.HELLOWORLD<<std::endl;

}return 0;

}

Para los servicios, al igual que para los tópicos, se de�nen los mensajes de petición y de respuesta. Para ellose usan archivos con extensión srv bajo el subdirectorio del mismo nombre situado en el paquete de ROS alque el servicio pertenece. Estos archivos usan los mismos tipos de datos primitivos que aquellos empleadospara de�nir los �cheros msg, también se pueden de�nir constantes en ellos, e igualmente se pueden usarmensajes de�nidos en otros �cheros msg, tanto del mismo paquete como de otros. La única diferencia conrespecto a los �cheros msg, es que para distinguir la de�nición del mensaje de petición y la del de respuesta,ambas están divididas por una línea con tres guiones tal como se muestra en el siguiente ejemplo.

# Peticiónotro_paquete/UnMensaje peticion---# RespuestaMensajeDefinidoEnElMismoPaquete respuesta

2.2 Nubes de puntos

Las nubes de puntos, del inglés "point clouds" son un conjunto de puntos que generalmente en aplicacionesrobóticas pueden estar localizados en un espacio de dos o tres dimensiones. La estructura de datos en lasque se almacenan, incluyen las coordenadas geométricas x, y, y z para cada uno de los puntos, en el casode un espacio tridimensional; y además pueden contener información adicional asociada a cada punto, tal


como el color. A las coordenadas se las denomina características y a la información adicional descriptores. Lasnubes de puntos pueden ser obtenidas mediante diferentes técnicas y usando dispositivos variados tales comocámaras estereoscópicas, escáneres D, dispositivos LIDAR y cámaras Time of Flight (TOF). En la �gura .,se muestran dos nubes de puntos de ejemplo. La sub�gura .-(a) muestra la vista superior de una nube depuntos tomada en el interior de un apartamento, mientras que la sub�gura .-(b) muestra el hueco de unaescalera. Un dispositivo que permite generar estas nubes de puntos y que está ampliamente adoptado en elmundo de la robótica, a pesar de que originalmente fue creado como una interfaz de usuario para una consolade videojuegos; es la cámara Microso Kinect®, cuya primera versión basa su funcionamiento en el envío deun patrón de luz infrarroja hacia el entorno, para posteriormente mediante el análisis del re�ejo del patrónsobre éste, conseguir obtener una nube de puntos del entorno.

(a) (b)Las imágenes mostradas han sido obtenidas haciendo uso de las nubes de puntos dis-ponibles en las referencias [] y []

Figura 2.4 Ejemplos de nubes de puntos correspondientes a un apartamento y una escalera.

En ROS existe un tipo de mensaje especí�co para el envío de éste tipo de estructuras de datos. Este mensajees concretamente del tipo sensor_msgs/PointCloud2, lo que indica que está de�nido dentro del paquetede ROS llamado sensor_msgs y a su vez dentro de un �chero del mismo nombre que el mensaje y conextensión.msg. En el código . se muestra el resultado de ejecutar el comando siguiente:

rosmsg show sensor_msgs/PointCloud2

el cuál muestra el contenido de este �chero además de incluir la de�nición de aquellos campos cuyo tipo dedatos no sea primitivo.

Código 2.3 Salida del comando rosmsg show sensor_msgs/PointCloud2, con comentarios adi-cionales.

std_msgs/Header headeruint32 seq # Número de secuenciatime stamp # Marca de tiempostring frame_id # Sistema de referencia al que los datos están asociados

# Si la nube de puntos ha sido generada a partir de los datos de# una cámara, los puntos pueden ser ordenados en un estructura 2D# En caso de que no estén ordenados, el campo height valdrá 1uint32 heightuint32 width

# Descripción de los campos asociados a cada punto# y su disposición en los datos binariossensor_msgs/PointField[] fields# Definición de constantes que identifican los tipos de datosuint8 INT8=1

2.2 Nubes de puntos 13

uint8 UINT8=2uint8 INT16=3uint8 UINT16=4uint8 INT32=5uint8 UINT32=6uint8 FLOAT32=7uint8 FLOAT64=8string name # Nombre del campo.# Desplazamiento en bytes con respecto al inicio de los datos de un puntouint32 offsetuint8 datatype # Tipo de dato del campouint32 count # Número de elementos que incluye el campo

# Indica la disposición de los bytes en los datos binariosbool is_bigendian

# Número de bytes que ocupan los datos de un puntouint32 point_step

# Número de bytes que ocupan los datos de una fila# Sólo válido si los puntos están ordenados en 2Duint32 row_step

uint8[] data # Datos binarios

bool is_dense # Indica si todos los puntos tienen valores válidos

Como se puede observar, los datos de la nube de puntos, se encapsulan en una secuencia de bytes. Estasecuencia deberá ser interpretada según las indicaciones presentes en el mensaje. Prestando especial atencióna las de�niciones de los campos asociados a cada punto (tipos de datos, número de elementos del campo,nombre) y si los bytes están ordenados según el formato big-endian o little-endian.En la �gura . se muestra un mensaje de ejemplo correspondiente a una nube con tan sólo puntos.

Cada uno de los puntos tiene asociado campos: las coordenadas x,y y z, y el color en formato A-RGB. A laizquierda de la �gura se presenta un diagrama que muestra el nombre y tipo de datos de cada campo, paraeste mensaje en concreto. A la derecha se muestra otro diagrama con el valor de cada uno de éstos campos.En el caso de los datos binarios que de�nen la nube de puntos, es decir, el contenido del campo uint8[]data; están agrupados en los conjuntos de bytes que conforman cada campo asociado a un punto. Para cadauno de estos conjuntos de bytes, se muestra el valor hexadecimal de cada byte individualmente, y también lainterpretación del conjunto como un número decimal o como un color.

Figura 2.5 Ejemplo de mensaje PointCloud.


2.3 Algoritmo ICP

El algoritmo ICP, de sus siglas en inglés Iterative Closest Point, forma parte del conjunto de algoritmos quetienen como objetivo encontrar una matriz de transformación que nos permita alinear de la manera másprecisa posible dos nubes de puntos. Este algoritmo fue ideado por Besl andMcKay y publicado originalmenteen []. Las aplicaciones de este tipo de algoritmos van desde el escaneo y creación de un modelo D de unobjeto físico (propósito para el que fue ideado), hasta la navegación y creación de mapas por parte de robots[], pasando por la generación y procesamiento de imágenes médicas [].Los datos de entrada del algoritmo son dos nubes de puntos, y opcionalmente una estimación de la trans-

formación que se debe aplicar a una de ellas para encajarla con la otra. El proceso de alineación es iterativo,y en él, una nube de puntos permanece �ja (nube de puntos de referencia), mientras que la otra va sufrien-do sucesivas transformaciones (rotaciones y traslaciones), que dependiendo de la con�guración aplicada alalgoritmo y de las características concretas de las nubes de puntos a alinear, irán mejorando en menor omayor medida la alineación de las dos nubes en cada iteración (es decir, la posición relativa de una nube depuntos con respecto a la otra se debería ir ajustando cada vez más a aquella que existe en el mundo físico).Sin embargo, no siempre ocurre así, ya que el algoritmo es sensible al ruido, a las características del entorno,a la con�guración aplicada, por lo que no se tiene garantías de que los resultados obtenidos sean buenos entodo momento. Un estudio que muestra la in�uencia de las características de las nubes de puntos y de lacon�guración aplicada se puede leer en la referencia [].De manera general, las fases del algoritmo ICP son las siguientes:

1. Filtrado: Filtrar ambas nubes de puntos (opcional). El �ltrado puede eliminar puntos, pero tambiénpuede consistir en realizar cálculos sobre la nube de puntos para añadir información adicional (descrip-tores).

2. Búsqueda de correspondencias: Emparejar cada punto de la nube de lectura con su correspondienteen la nube de referencia (Varios puntos en la nube de lectura se pueden emparejar con un único puntode la nube de referencia).

3. Filtrado de correspondencias: Eliminar aquellos emparejamientos que no cumplan determinados re-quisitos, y por lo tanto no se tendrán en cuenta para el cálculo de la transformación (opcional). Unrequisito por ejemplo puede ser la distancia máxima que separa ambos puntos.

4. Minimización del error: Calcular la transformación que minimiza la separación entre las dos nubesde puntos.

5. Transformación: Transformar la nube de puntos de lectura usando la transformación calculada.

6. Comprobaciones: Comprobar si la transformación obtenida cumple ciertos requisitos tales como noexceder un cierto ángulo de giro, o no superar el número de iteraciones máximas permitidas (indicadostodos éstos requisitos en la con�guración).

7. Volver al punto mientras que el resultado de las comprobaciones indique que el algoritmo debe conti-nuar.

En el diagrama de la �gura . se muestran éstas fases del algoritmo en un diagrama para mayor claridad.

2.3.1 Minimización del error

Existen varias maneras de calcular el error de alineación, y a su vez, varias maneras también de encontrar latransformación que los minimiza. En la referencia [] se presentan cuatro formas de cálculo del error, cadauna de ellas con su correspondiente forma de calcular la transformación que lo minimiza. En la referencia[] se presenta una forma de cálculo del error basada en la distancia de un punto a un plano, mientras quetodos los demaś métodos se basan en distancias entre puntos.La minimización del error, por lo tanto, consiste de manera general, en encontrar la transformación que

minimiza la expresión .. En la cuál, R es una matriz de rotación, t es un vector de traslación, x i e y i son

Se denomina nube de puntos de lectura a aquella que no es la de referencia

2.4 Librería libpointmatcher 15

Figura 2.6 Diagrama de �ujo general del algoritmo ICP.

cada uno de los puntos de la nube de referencia y la nube de lectura (respectivamente), entre los que se haencontrado una correspondencia.

E(t,R) =n∑

i=∥x i − (R ⋅ y i + t)∥ (.)

Mientras que si se usa la distancia entre puntos y planos, usaríamos la expresión . para el cálculo delerror. Al igual que antes, R, t, x i e y i , tienen el mismo signi�cado que en la expresión .. En este caso hay unnuevo término, n i , que se corresponde al vector ortonormal

al plano tangente a la nube de puntos en tornoal punto x i . Es decir, este plano será aquel que minimice la distancia entre él y la nube de puntos de maneralocal alrededor del punto x i .

E(t,R) =n∑

i=((R ⋅ y i + t − x i) ⋅ n i)

(.)

Como añadido, se puede dar más importancia a la correspondencia entre ciertos puntos, y limitar la in-�uencia de otros, mediante la inclusión de pesos en la ecuación del error. Estos pesos, pueden tomar valoresentre y , y están asociados a cada par de puntos emparejados en la fase de búsqueda de correspondencias.De manera que las expresiones del error mostradas en las ecuaciones . y . quedarían tal como se muestraen las ecuaciones . y ., respectivamente.

E(t,R) =n∑

i=w i ⋅ ∥x i − (R ⋅ y i + t)∥ (.)

E(t,R) =n∑

i=w i ⋅ ((R ⋅ y i + t − x i) ⋅ n i)

(.)

2.4 Librería libpointmatcher

Ésta librería[][][], desarrollada por François Pomerleau y Stéphane Magnenat en el Automatic SystemsLab de la escuela ETH en Zürich, implementa métodos para ejecutar el algoritmo ICP, y provee una in-

Este vector debe ser calculado como parte del algoritmo para cada uno de los puntos de la nube de referencia. El lugar adecuadopara hacer éstos cálculos es en la fase de �ltrado de las nubes de puntos.

http://www.asl.ethz.ch/

http://www.asl.ethz.ch/


terfaz de con�guración sencilla basada en archivos YAML Ain’t Markup Language (YAML). Para su fácilintegración con ROS, se puede hacer uso del metapaquete ethzasl_icp_mapping[], que ya incluye éstalibrería, y además proporciona los métodos necesarios para traducir entre los tipos de datos que usa ROS yaquellos que usa libpointmatcher. Esta conversión entre tipos de datos está implementada en el paquetelibpointmatcher_ros[]. La obtención y compilación de éste so ware se detalla en el apéndice A en lasección A..Tanto la propia librería como el resto de paquetes complementarios están desarrollados en el lenguaje C++ .

En el caso de la librería, todas las funciones y clases forman parte del templated struct PointMatcher. Estopermite elegir en el código el tipo de datos por el que estarán formadas las nubes de puntos con las que operaráel algoritmo ICP. En el código se puede hacer uso de dos sentencias typedef para que PM haga referencia auna especialización de éste struct.

typedef float ScalarType;typedef PointMatcher<ScalarType> PM;

Principalmente hay clases dentro de PointMatcher, que son fundamentales para ejecutar el algoritmoICP. Éstas son (siendo ScalarType el tipo de datos para el que se realizará la especialización):

• PointMatcher<ScalarType>::DataPoints: Almacena las nubes de puntos. Lo hace de maneradiferente a la usada en los mensajes de ROS, tal como se verá a continuación.

• PointMatcher<ScalarType>::TransformationParameters: Es una matriz que almacena losparámetros de una transformación afín. En el caso que nos ocupa será una matriz x.

• PointMatcher<ScalarType>::ICP: Es la clase que proporciona los métodos para con�gurar yejecutar el algoritmo ICP.

Como se ha indicado anteriormente, a diferencia de losmensajes sensor_msgs/PointCloud2, el tipo dedatos DataPoints almacena la información de los puntos de manera matricial. Usa dos matrices, una paralas características .-(a) (del inglés "features") y otra para los descriptores .-(b) (del inglés "descriptors").Correspondiendo las características a las coordenadas de los puntos, y los descriptores a la informaciónasociada a ellos (tal como color, normal, etc). En ambas matrices, cada columna contiene la informaciónrelativa a un único punto.

Características

(a) Matriz de características.

Descriptores

(b) Matriz de descriptores.

Figura 2.7 Estructura de almacenamiento de las nubes de puntos dentro de PointMatcher::DataPoints.

Los tipos de datos adecuados para almacenar los datos de las nubes de puntos son float y double, debido a que las coordenadasusualmente serán números no enteros. La elección de uno u otro dependerá de la precisión deseada y de la potencia de cálculodisponible.


2.4.1 Configuración del algoritmo

La principal ventaja que ofrece ésta librería es la facilidad que presenta para modi�car la con�guración delalgoritmo ICP en tiempo de ejecución, tanto mediante ejecución de ciertos métodos de la clase PointMat-cher<ScalarType>::ICP, como la lectura de �cheros YAML que contienen la con�guración deseada. Sien-do ésta última especialmente útil, ya que permite almacenar y compartir de una manera concisa y clara losparámetros usados en ciertos experimentos, facilitando la comparación del rendimiento y la precisión obte-nida al usar distintas con�guraciones.Las distintas fases del algoritmo ICP son aquellas que se han mostrado en la sección .. Libpointmat-

cher ofrece la posibilidad de con�gurar cada una de éstas fases del algoritmo usando un �chero YAML.Cada fase tiene asociado una lista o diccionario YAML dependiendo del caso, como se muestra en la tabla.. Mediante éstas variables del �chero YAML se indican qué módulos de so ware se cargarán, y cómo se

Tabla 2.1 Fases del algoritmo ICP, junto con la variable del �chero YAML que tiene asociada.

Fase del algoritmo ICP Variable YAML asociada Tipo de variable

Filtrado nube de puntos de lectura readingDataPointsFilters Lista

Filtrado nube de puntos de referencia referenceDataPointsFilters Lista

Búsqueda de correspondencias matcher Diccionario

Filtrado de correspondencias outlierFilters Lista

Minimización del error errorMinimizer Diccionario

Comprobaciones transformationCheckers Lista

con�gurarán, para encargarse de cada fase del algoritmo. Para que esto sea posible, los módulos correspon-dientes a cada fase comparten una misma interfaz que es usada por la clase encargada de la con�guración yejecución del algoritmo ICP. Es posible incluso desarrollar nuevos módulos para posteriormente integrarlosen la librería y hacer uso de ellos para crear una variación del algoritmo.Para conocer los módulos disponibles, clasi�cados según la fase en la que pueden ser usados, junto con las

opciones que admiten y la explicación sobre su uso, se puede hacer uso del comando siguiente:

pmicp -l

A continuación se muestran ejemplos de con�guración para cada una de las fases, haciendo uso de algunosde los módulos disponibles por defecto en la librería.

Filtrado de la nube de puntos de referencia

Mediante el uso de la variable readingDataPointsFilters del �chero YAML con�guraremos ésta fase.Debido a que se pueden concatenar varios �ltros, esta variable es una lista sin límite de elementos. Un ejemplode cuál debe ser la sintáxis usada, y algunos de los �ltros que se pueden usar, es el siguiente fragmento.

Código 2.4 Ejemplo de archivo de con�guración ICPConfig.yaml.

referenceDataPointsFilters:- MaxDistDataPointsFilter:

dim: -1maxDist: 7

- SurfaceNormalDataPointsFilter:knn: 6epsilon: 3.16keepNormals: 1keepDensities: 0keepEigenValues: 0keepEigenVectors: 0keepMatchedIds: 0


- OrientNormalsDataPointsFilter:towardCenter: 1

En este caso, se usa en primer lugar un �ltro que elimina aquellos puntos que se encuentren en un radiomayora metros desde el origen de coordenadas. En segundo lugar, se usa un �ltro que calcula las normales de lospuntos restantes y las añade como descriptores. Finalmente se orientan todas las normales hacia el origen decoordenadas.

Filtrado de la nube de puntos de lectura

La variable readingDataPointsFilters, al igual que en el caso de referenceDataPointsFilters,permite con�gurar los �ltros, que en este caso se aplicarán a la nube de lectura. Un ejemplo sería el siguiente:

readingDataPointsFilters:- MaxDistDataPointsFilter:

dim: -1maxDist: 7

- SamplingSurfaceNormalDataPointsFilter:ratio: 0.5knn: 6samplingMethod: 0maxBoxDim: 1averageExistingDescriptors: 1keepNormals: 1keepDensities: 0keepEigenValues: 0keepEigenVectors: 0

- OrientNormalsDataPointsFilter:towardCenter: 1

Esta con�guración di�ere con respecto a la mostrada para la nube de referencia en un único aspecto: El �ltroque calcula las normales de los puntos además elimina aleatoriamente la mitad de ellos.

Búsqueda de correspondencias

Un ejemplo de con�guración sería:

matcher:KDTreeMatcher:knn: 2epsilon: 1searchType: 1maxDist: 2

Filtrado de correspondencias

outlierFilters:- MedianDistOutlierFilter:

factor: 4- SurfaceNormalOutlierFilter:

maxAngle: 1.57

Minimización del error

errorMinimizer:PointToPointErrorMinimizer


Comprobaciones

transformationCheckers:- DifferentialTransformationChecker:

minDiffRotErr: 0.001minDiffTransErr: 0.01smoothLength: 4

- CounterTransformationChecker:maxIterationCount: 20

- BoundTransformationChecker:maxRotationNorm: 1maxTranslationNorm: 1

2.4.2 Inspección y registro

Existen además dos fases adicionales, que son algo especí�co de ésta librería, que llamaremos fase de inspec-ción y fase de registro, y que se con�guran haciendo uso de las variables inspector y logger del �cheroYAML de con�guración. El cometido de éstas fases es permitir al desarrollador evaluar el progreso y e�caciade la alineación de las nubes de puntos a lo largo de las distintas iteraciones del algoritmo, identi�candoposibles problemas en la con�guración actual, a través incluso de medidas que genera de forma automática.El progreso de la alineación no se puede observar en tiempo real, sino que se hace a posteriori, una vez queel algoritmo ha �nalizado sus iteraciones. Para ello se inspeccionan los archivos que se generan durante laejecución de éstas dos fases. En cada iteración se van almacenando datos relativos a ella, tales como las nubesde puntos trasladadas y giradas según las sucesivas transformaciones obtenidas. También se almacenan lascorrespondencias entre puntos establecidas en cada iteración, permitiendo visualizarlas como líneas que unenlos puntos asociados. Cada una de las correspondencias tendrá un peso asociado (éste peso es el que ha sidoasignado en la fase de �ltrado de correspondencias y posteriormente tenido en cuenta en la minimización delerror). Este peso permite que en la visualización se pueda usar una escala de colores para representar éstascorrespondencias según su peso. El resultado del uso de ésta funcionalidad se puede observar en la imagen..

La nube de puntos de referencia aparece coloreada de blanco, mientras que la nube quese pretende alinear con ella aparece de verde. Las correspondecias aparecen coloreadasde rojo o azul dependiendo de si tienen un peso de o en la minimización del error.

Figura 2.8 Visualización de las correspondencias encontradas entre dos nubes de puntos.

Un ejemplo de la con�guración que podría ser aplicada para éstas fases es la siguiente:

Las nubes de puntos obtenidas en las sucesivas iteraciones del algoritmo ICP se van almacenando en un archivo vtk, formato quepermite su visualización en el programa Paraview. De esta manera se puede visualizar fácilmente el proceso de alineación.


inspector:VTKFileInspector:baseFileName: testdumpPerfOnExit: 1dumpStats: 1dumpIterationInfo: 1dumpDataLinks: 1dumpReading: 1dumpReference: 1

logger:FileLogger:infoFileName: test_logwarningFileName: test_warningdisplayLocation: 1

3 Diseño e implementación de la aplicación

Aquí se mostrará el análisis realizado sobre los sistemas de referencia implicados en el problema, y las con-clusiones obtenidas sobre éste análisis. Éstas conclusiones indican que existen dos maneras de estimar

en un primer momento la transformación sufrida por el sistema de referencia del robot, y ámbos métodos seexpresan matemáticamente en la sección .Posteriormente se describirá el diseño preliminar de la aplicación.

3.1 Análisis de los distintos sistemas de referencia implicados en el algoritmo ICP

En esta sección identi�caré los sistemas de referencia que son de interés para la aplicación a desarrollar. Paraposteriormente analizar las relaciones entre ellos.

3.1.1 Sistemas de referencia implicados

El robot SIAR, cuyos archivos bags usaré para simular los experimentos al no disponer de un robot real,presenta los sistemas de referencia mostrados en la �gura .. Sin embargo, los sistemas de referencia de

odom

visual_odom

base_link sandwich_center

left_link

left_rgb_frame

left_depth_frame

left_rgb_optical_frame

left_depth_optical_frame

back_link

back_rgb_frame

back_depth_frame

back_rgb_optical_frame

back_depth_optical_frame

right_depth_frame right_depth_optical_frame

front_link

front_depth_frame

front_rgb_frame

front_depth_optical_frame

front_rgb_optical_frame

right_link

up_link

right_rgb_frame

up_rgb_frame

up_depth_frame

right_rgb_optical_frame

up_rgb_optical_frame

up_depth_optical_frame

Figura 3.1 Sistemas de referencia del robot SIAR.

21

22 Capítulo 3. Diseño e implementación de la aplicación

interés para esta aplicación, se pueden reducir a los siguientes:

• odom: Es el sistema de referencia raíz con respecto al cuál, el nodo encargado de la odometría delrobot (calculada a través de los encoders de las ruedas), publicará la transformación hacia el sistemade referencia situado en el chasis(base_link).

• visual_odom: Será un sistema de referencia auxiliar con respecto al cuál se publicará una transfor-mación hacia el sistema de referencia base_link, calculada a partir de los datos recibidos a través decámaras RGBD. La transformación entre éste sistema y el sistema odom es la matriz identidad, lo quesigni�ca que son idénticos.

• base_link: Es el sistema de referencia situado en la base del chasis del robot. Este sistema de referenciaestá orientado de tal manera que el eje x apunta hacia la parte frontal del robot, el eje y hacia el costadoizquierdo, y el eje z apunta verticalmente en dirección opuesta al suelo.

• camera: Es el nombre genérico para identi�car el sistema de referencia en el que estarán expresadastodas las nubes de puntos que se reciban. En la práctica, este nombre será sustituido por nombressimilares a rgb_optical_frame, o depth_optical_frame. Para el caso concreto del robot SIAR,hay varios sistemas de referencia correspondientes a las distintas cámaras de las que dispone, pero haréuso de los sistemas correspondientes a las cámaras frontales, que son front_rgb_optical_frameo front_depth_optical_frame.

3.1.2 Análisis de las transformaciones entre los sistemas de referencia y su implicación en el algoritmo ICP

Por lo tanto, las transformaciones que nos interesan son aquellas que relacionan, entre sí, los sistemas dereferencia que se han mostrado en la subsección ... Además, se debe tener en cuenta la evolución temporalque éstos sufren.Si quisiéramos expresar una nube de puntos tomada en cierto instante de tiempo en el sistema de referencia

camera, en otro instante de tiempo anterior en el mismo sistema de referencia (que no es �jo), necesitaríamosuna transformación que podríamos denominar: Tcamera−camera . Esta transformación nos permite expresarun punto tomado en el instante t en referencia al sistema correspondiente a donde se encontraba la cámaraen el instante t, tal como se muestra en la ecuación ..

P = Tcamera−camera ⋅ P (.)

Esta transformación, es equivalente a hallar la transformación del punto hacia un sistema de referencia queconsideraremos �jo para posteriormente volver a transformar el punto desde ese sistema hacia el correspon-diente a la cámara en el instante de tiempo anterior. Tomando como sistema �jo odom_visual, podemosobtener la transformación mediante la expresión mostrada en la ecuación ..

Tcamera−camera = (Tcamera−odom_v isual)t ⋅ (Todom_v isual−camera)t (.)

El algoritmo ICP nos devuelve éstas transformaciones consecutivas entre el sistema de referencia cameray él mismo en dos instantes de tiempo distintos. Estás transformaciones son las que nos permiten encajar unanube de puntos tomada en el instante t con aquella tomada en el instante anterior al que denominaremos t.Cuanto mejor sea el algoritmo, más próxima será la transformación devuelta por éste a aquella que el sistemade referencia verdaderamente ha experimentado con respecto a su posición anterior.En el instante por ejemplo, se cumpliría la expresión mostrada en ..

(PointCloudodom_v isual)t = (Todom_v isual−camera)t

⋅ Tcamera−camera

⋅ Tcamera−camera ⋅ (PointCloudcamera)t

(.)

Esta expresión puede ser desarrollada sustituyendo las transformaciones entre el sistema de referenciacamera en un instante y él mismo en un instante posterior, por su versión extendida, demanera que resultaría

Hago uso de las convenciones de nombrado de ROS disponibles en la referencia []

3.1 Análisis de los distintos sistemas de referencia implicados en el algoritmo ICP 23

en la expresión ..

(PointCloudodom_v isual)t = (Todom_v isual−camera)t

⋅ [(Tcamera−odom_v isual)t ⋅ (Todom_v isual−camera)t]

⋅ [(Tcamera−odom_v isual)t ⋅ (Todom_v isual−camera)t]

⋅ (PointCloudcamera)t

(.)

Igualando las ecuaciones . y ., y realizando las simpli�caciones oportunas, se deduce la expresión ..

(Todom_v isual−camera)t = (Todom_v isual−camera)t ⋅ Tcamera−camera ⋅ Tcamera−camera (.)

De la que a su vez extraemos la expresión de la ecuación ..

Tcamera−camera = T−camera−camera ⋅ (T

−odom_v isual−camera)t ⋅ (Todom_v isual−camera)t (.)

Por lo tanto, hemos obtenido una expresión que nos permite relacionar la transformación que nos debeproporcionar el algoritmo ICP, con las transformaciones anteriormente obtenidas por éste, con la transforma-ción inicial desde el sistema de referencia odom_visual al sistema camera y con la transformación actualentre éstos últimos.Sin embargo, nuestro objetivo es calcular (Todom_v isual−camera)t , ya que en el instante t, inmediatamente

después de haber capturado la nube de puntos, aún no la conocemos, pero debemos pasarle una estimaciónde ella al algoritmo.Lo que sí tendremos en ese instante es un valor de la odometría del robot, que nos indicará la transformación

entre odom y base_link, con el cuál podremos obtener (Todom−camera)t como muestra la expresión ..

(Todom−camera)t = (Todom−base_ l ink)t ⋅ (Tbase_ l ink−camera)t (.)

Como no disponemos de (Todom_v isual−camera)t , podemos considerar que es aproximadamente igual a(Todom−camera)t , y será ese el valor que usaremos como aproximación. Por lo tanto la expresión a usar seríala mostrada en la expresión ..


−odom_v isual−camera)t ⋅ (Todom−camera)t (.)

Otra forma de expresar la transformación entre dos posiciones consecutivas de la cámara sería la mostradaen la ecuación ., la cuál es desarrollada en la ecuación ., haciendo uso de la ecuación ..

Tcamera−camera = (Tcamera−base_ l ink)t ⋅ Tbase_ l ink−base_ l ink ⋅ (Tbase_ l ink−camera)t (.)

Tbase_ l ink−base_ l ink = (Tbase_ l ink−odom)t ⋅ (Todom−base_ l ink)t (.)

Tcamera−camera = (Tcamera−base_ l ink)t

⋅ (Tbase_ l ink−odom)t ⋅ (Todom−base_ l ink)t⋅ (Tbase_ l ink−camera)t

(.)

Como veremos en la sección .., estas formas de cálculo de la aproximación a usar para la transformaciónsufrida por el sistema de referencia camera entre dos instantes de tiempo consecutivos, no son equivalentes,y aportan resultados distintos.

3.1.3 Comprobación de la no equivalencia entre las dos formas de calcular la estimación del algoritmo

Por claridad, presento las dos formas de calcular la estimación en las ecuaciones . y ..

Tcamera−camera = (T−odom_v isual−camera)t ⋅ (Todom−camera)t (.)



Queremos comprobar si ambas expresiones son equivalentes, para ello desarrollaremos la expresión .,como se muestra en la ecuación ..

Tcamera−camera = (T−odom_v isual−camera)t ⋅ (Todom−camera)t

= (T−odom−camera)t ⋅ (Todom−camera)t= (Tcameraodom)t ⋅ (Todom−camera)t

= (Tcamera−base_ l ink)t ⋅ (Tbase_ l ink−odom)t ⋅ (Todom_base_ l ink)t ⋅ (Tbase_ l ink−camera)t

= (Tcamera−base_ l ink)t ⋅ Tbase_ l ink−base_ l ink ⋅ (Tbase_ l ink−camera)t(.)

Por lo tanto, ambas expresiones son equivalentes para la fase inicial del algoritmo. Ahora comprobaremossi la equivalencia se sigue cumpliendo para un estado más avanzado del proceso. En este caso las expresionescuya equivalencia queremos comprobar son las mostradas en la ecuación . y ..


−odom_v isual−camera)t ⋅ (Todom_v isual−camera)t (.)


Al igual que antes, desarrollaremos la primera expresión en la ecuación ..

Tcamera−camera = Tcamera−camera ⋅ (Tcamera−odom)t ⋅ (Todom−camera)t

= (Tcamera−base_ l ink)t ⋅ (Tbase_ l ink−odom_v isual)t ⋅ (Todom−camera)t

= (Tcamera−base_ l ink)t ⋅ (Tbase_ l ink−odom_v isual)t ⋅ (Todom−base_ l ink)t ⋅ (Tbase_ l ink−camera)t(.)

Lo que nos lleva a la conclusión de que para que ambas expresiones sean iguales, se debe cumplir la igualdadmostrada en la ecuación ..

Tbase_ l ink−base_ l ink = (Tbase_ l ink−odom_v isual)t ⋅ (Todom−base_ l ink)t (.)

Esta igualdad sólo se cumplirá en el caso de que usemos en ambos casos las correcciones previamentecalculadas mediante el algoritmo ICP, sin embargo, si las ignoramos y usamos únicamente la información dela odometría de las ruedas para calcular la estimación, usaremos la expresión ..

Tbase_ l ink−base_ l ink = (Tbase_ l ink−odom)t ⋅ (Todom−base_ l ink)t (.)

Esta diferencia en el cálculo de la estimación de la transformación se ilustra en la �gura ., en la quela transformación marcada con un es aquella que tiene en cuenta las correciones previas, mientras que lamarcada con un no.

3.2 Cálculo de la odometría visual

A partir de las conclusiones obtenidas en la sección ., podemos determinar que existen dos formas decalcular la odometría visual. Ambas están basadas en el uso del algoritmo ICP, di�riendo únicamente enla forma en la que se calcula la estimación inicial de la transformación Tcameran−cameran+

. Las denominaréestimación absoluta y relativa.

3.2.1 Estimación absoluta

Esta forma de cálculo de la estimación hace uso de las transformaciones previas calculadas mediante el al-goritmo y acumuladas en una única transformación Tcameran−camera . La forma de cálcular esta estimaciónsería la mostrada en la ecuación ..

Tcameran−cameran+= Tcameran−camera ⋅ (Tcamera−odom)t

⋅ (Todom−camera)tn+

(.)

3.3 Diseño de la aplicación 25

t

base_link

odomodom_visual

t (odom)

base_link

base_link

t (odom_visual)

t (odom)

*

*

base_link

Figura 3.2 Ilustración de las diferencias en la forma de cálculo de la estimación de la transformación inicialdel algoritmo ICP.

3.2.2 Estimación relativa

Mientras que para la forma de estimación relativa, el cálculo de la transformación se hará teniendo en cuentaúnicamente dos posiciones consecutivas del chasis del robot con respecto al sistemaodomTbase_ l inkn−base_ l inkn+tal como se muestra en la ecuación ..

Tcameran−cameran+= (Tcamera−base_ l ink)tn ⋅ Tbase_ l inkn−base_ l inkn+⋅ (Tbase_ l ink−camera)tn+

(.)

Tbase_ l inkn−base_ l inkn+ = (Tbase_ l ink−odom)tn ⋅ (Todom−base_ l ink)tn+ (.)

3.2.3 Obtención de la odometría visual

Una vez calculada la estimación de la transformación, y ejecutado el algoritmo ICP, obtenemos la mismatransformación Tcameran−cameran+

. Sin embargo, la transformación que queremos publicar como odometríaes (Todom_v isual−base_ l ink)tn+ . Para obtenerla aplicamos las operaciones mostradas en la ecuación ..

(Todom_v isual−base_ l ink)tn+ = (Todom−camera)t ⋅ Tcamera−cameran+

⋅Tcamera−base_ l ink(.)

Siendo (Todom−camera)t la transformación correspondiente a la posición del sistema de referencia cameraen el momento de captura de la primera nube de puntos, Tcamera−cameran+

la transformación acumuladaentre las distintas posiciones del sistema camera, y Tcamera−base_ l ink una transformación �ja, todas ellasconocidas.

3.3 Diseño de la aplicación

El ejecutable principal de ROS que desarrollaré será llamado visual_odom, y además crearé un ejecutableauxiliar llamado tf_from_odom. La funcionalidad del nodo visual_odom es la de calcular una estimacióndel movimiento del robot a partir de las sucesivas nubes de puntos que el robot va capturando. Esta estimación


se conseguirá mediante el algoritmo ICP. En cuanto al nodo tf_from_odom, su funcionalidad es convertirmensajes del tipo nav_msgs/Odometry a geometry_msgs/TransformStamped.El nodo visual_odom deberá suscribirse a los siguientes tópicos, a los que se puede aplicar un remapeo

a la hora de lanzar el nodo:

• point_cloud : Como su nombre indica será el tópico correspondiente a las nubes de puntos.

• odom_topic : Tópico del que se recibirá la odometría de las ruedas, para así tener una estimacióninicial de la alineación entre las nubes de puntos.

• /tf : Necesario para conocer la transformación entre algunos sistemas de referencia del robot.

Y publicará mensajes geometry_msgs/Odometry en el tópico /visual_odom, indicando la transforma-ción entre el sistema de referencia visual_odom y el sistema base_link.Para la con�guración del nodo, haré uso de varios parámetros de ROS, que serán almacenados en un

archivo que posteriormente será cargado por roslaunch antes de lanzar el nodo. Estos parámetros son:

• absolute_guess : Es un parámetro de tipo bool cuyo valor indica la forma en la que se calculará laestimación inicial que recibirá el algoritmo ICP. Posteriormente hablaré sobre ambas estimaciones.

• base_link_frame : Nombre del sistema de referencia de la base del robot.

• camera_frame : Nombre del sistema de referencia correspondiente a la cámara.

• processing_rate : Frecuencia a la que se intentarán alinear las nubes de puntos.

• tf_wait_time : Máximo tiempo de espera para recibir una transformación a través del tópico /tf.Necesario porque rosbag tarda un tiempo en comenzar a reproducir los mensajes almacenados en losarchivos bag.

3.3.1 Implementación de las dos maneras de calcular la estimación del algoritmo ICP

Dependiendo del método de estimación elegido, se ejecutará una función u otra en un hilo separado al quedenominaré hilo de procesamiento. Éstas dos posibles funciones son:

• void VisualOdom::processingThreadFunctionAbsolute()

• void VisualOdom::processingThreadFunctionRelative()

El código presenta una serie de variables que son comunes a las dos maneras de calcular el algoritmo. Cadauna de éstas variables se corresponde a una transformación de las mencionadas en la sección .. Todas ellasson del tipo PM::TransformationParameters.En la tabla . se muestran éstas mismas variables junto con las transformaciones que almacenan.Las dos primeras (_Tbase_link_camera y _Tcamera_base_link) se inicializan al comienzo del pro-

grama, dentro de la función main de la instancia de la clase VisualOdom. Las líneas de código que permiteninicializar estas dos variables son las siguientes:

tf2_ros::Buffer tfBuffer;tf2_ros::TransformListener tfListener(tfBuffer);geometry_msgs::TransformStamped Tbase_link_camera;Tbase_link_camera = tfBuffer.lookupTransform(_base_link_frame, _camera_frame,

ros::Time(0), ros::Duration(tf_wait_time));Eigen::Affine3d eigenTr = tf2::transformToEigen(Tbase_link_camera);_Tbase_link_camera = eigenTr.matrix().cast<float>();_Tcamera_base_link = _Tbase_link_camera.inverse();

Como se observa, obtenemos la transformación entre el sistema camera y el sistema base_link a travésde los mensajes publicados en el tópico /tf, mediante las librerías y los métodos proporcionados por ROSpara hacer uso de ese tópico. La otra variable, _Todom_visual_base_link_t0, se inicializa dentro de lasfunciones especí�cas de cada método de estimación.Posteriormente, en ambas funciones, al recibir la primera nube de puntos, se inicializan las variables _To-

dom_visual_base_link_t0, Todom_camera_t0 y Tcamera_odom_t0, a través de las líneas de códigomostradas en el fragmento ..


Tabla 3.1 Variables junto a las correspondientes transformaciones que almacenan.

Variables Transformaciones

) _Tbase_link_camera Tbase_ l ink−camera = (Tbase_ l ink−camera)tn

) _Tcamera_base_link Tcamera−base_ l ink = T−base_ l ink−camera

) _Todom_visual_base_link_t0 (Todom_v isual−base_ l ink)t = (Todom−base_ l ink)t) Todom_base_link_current (Todom−base_ l ink)tn+) Tinitguess Tcameran−cameran+

) Ticp Ti c p

) Todom_camera_t0 (Todom−camera)t

) Tcamera_odom_t0 (Tcamera−odom)t

) Tcamera_n_camera_0 Tcameran−camera

) Tcamera_0_camera_n Tcamera−cameran

) Todom_visual_base_link (Todom_v isual−base_ l ink)tn+

Código 3.1 Inicialización de transformaciones al recibir la primera nube de puntos.

_Todom_visual_base_link_t0 = PointMatcher_ros::odomMsgToEigenMatrix<float>(*_receivedOdomConstMsgPtr);

Todom_camera_t0 = _Todom_visual_base_link_t0 * _Tbase_link_camera;Tcamera_odom_t0 = Todom_camera_t0.inverse();

Para el almacenamiento de las nubes de puntos, disponemos de las variables mostradas en el código ..

Código 3.2 Variables para el almacenamiento de las nubes de puntos.

// Nube de puntos de referenciaboost::shared_ptr<PM::DataPoints> _lastCloudPtr;// Nube de puntos a alinearboost::shared_ptr<PM::DataPoints> _nextCloudPtr;

Estimación absoluta

Para cada nueva nube de puntos recibida después de la primera, se realizará lo siguiente:

1. Se asigna la nube de puntos recibida a la variable _nextCloudPtr.

2. Se asigna el valor actual de la odometría a la variable Todom_base_link_current.

3. Se obtiene la estimación de la transformación entre las nubes de puntos de la siguiente manera:

Tinitguess = Tcamera_n_camera_0 * Tcamera_odom_t0* Todom_base_link_current * _Tbase_link_camera;

4. Una vez tenemos la estimación, invocamos la función que ejecuta el algoritmo ICP:

Ticp = _icp.compute(*_nextCloudPtr, *_lastCloudPtr, Tinitguess);

5. Usando la transformación devuelta por el algoritmo para corregir la estimación, y acumular la transfor-mación obtenida de estamanera en la variable Tcamera_0_camera_n, la cuál indica la transformaciónque ha sufrido el sistema de referencia camera desde el instante inicial al actual:


Tcamera_0_camera_n = Tcamera_0_camera_n * (Ticp * Tinitguess);Tcamera_n_camera_0 = Tcamera_0_camera_n.inverse();

6. Obtenemos la transformación buscada a partir de las que disponemos, y la publicamos:

Todom_visual_base_link = Todom_camera_t0 * Tcamera0_camera_n* _Tcamera_base_link;

nav_msgs::Odometry visualOdomMsg =transformMatrixToOdomMsg<float>(Todom_visual_base_link,

"visual_odom",_base_link_frame,cloudTimeStamp);

this->_visualOdomPub.publish(visualOdomMsg);

7. Asignamos la nube de puntos de lectura a la de referencia, para la siguiente ejecución del algoritmo:

_lastCloudPtr = _nextCloudPtr;

Estimación relativa

Para éste método, necesitaremos una variable adicional que será Todom_base_link_previous, que secorresponde a la transformación (Todom−base_ l ink)tn . Esta variable es inicializada al recibir la primera nubede puntos mediante la instrucción:

Todom_base_link_previous = _Todom_visual_base_link_t0;

Los pasos son similares a los anteriores, coincidiendo exactamente la mayoría de ellos, sólo di�riendo en dosaspectos.El primero de ellos es el cálculo de la estimación, que en este caso se calcula de la siguiente manera:

Tinitguess = _Tcamera_base_link * Todom_base_link_previous.inverse()* Todom_base_link_current * _Tbase_link_camera;

Y el segundo aspecto es que mientras que en el otro método sólo asignábamos la nube de puntos de lecturaa la de referencia, para la siguiente ejecución del algoritmo; en este caso también hacemos la asignación deuna transformación a otra:

Todom_base_link_previous = Todom_base_link_current;

3.3.2 Descripción adicional del código

El código fuente del ejecutable principal está dividido en archivos: visual_odom_main.cpp, visual_-odom.cpp, visual_odom.h, message_generation.cpp y message_generation.h.El contenido del archivo visual_odom_main.cpp se muestra en el fragmento de código ..

Código 3.3 visual_odom.main.

#include "visual_odom/visual_odom.h"

int main(int argc, char** argv){

// Initialise ROSros::init(argc, argv, "visual_odom");VisualOdom visualOdom;visualOdom.main();


return 0;}

Como se puede observar, este código únicamente inicializa el nodo de ROS, para posteriormente delegartodo el procesamiento a la función main() del objeto visualOdom. Este objeto es una instancia de la clasecuyo código se muestra en C. y C..A grandes rasgos, la ejecución de la función main del objeto visualOdom se puede resumir en el diagrama

de bloques que se muestra en la �gura .

Obtener parámetros:- config_file

- processing_rate- camera_frame

- base_link_frame- absolute_guess- tf_wait_time

Obtener transformaciones:- Tbase_link-camera- Tcamera-base_link

Con�gurar ICP usando �chero con�guraciónYAML o usar con�guración por defecto

Anunciar tópico visual_odomSubscribirse a tópicos:- point_cloud- odom_topic

Iniciar hilo de procesamiento

Bucle en el hilo principalhasta recibir interrupción

Esperar la terminación delhilo de procesamiento

Figura 3.3 Diagrama de �ujo general del nodo.

En la �gura ., se muestra el diagrama de �ujo del callback que es ejecutado en el hilo principal, dentrode la función ros::spin().

Hilo principal

...

ros::spin()

VisualOdom::pointCloudandOdomCallback(... pointCloudMsg,... odomMsg)

_threadCoordinationMutex.lock()

_receivedCloudConstMsgPtr = pointCloudMsg;

_receivedOdomConstMsgPtr = odomMsg;

_�agCloudAvailable = true;

_threadCoordinationMutex.unlock()

...

Figura 3.4 Diagrama de �ujo del callback, ejecutado en el hilo principal.

En la �gura ., se muestra un diagrama de �ujo del hilo de procesamiento, en el que se mezcla código ypseudocódigo.


Figura 3.5 Diagrama de �ujo simpli�cado del hilo de procesamiento.

En el diagrama que se muestra en la �gura . se presenta de manera conceptual el procedimiento por elcuál el hilo principal y el de procesamiento acceden de manera sincronizada a las mismas variables.

sensor_msgs::PointCloudConstPtr _receivedCloudConstMsgPtr;

Variables compartidas

Hilo principal Hilo de procesamiento

Adquisición del mutex

Recepción de los mensajes

Levanto la bandera para indicarque hay nuevos mensajes disponibles

Almacenamiento de los mensajes enlas variables compartidas

boost::mutex _threadCoordinationMutex

bool _�agCloudAvailable;

nav_msgs::OdometryConstPtr _receivedOdomConstMsgPtr;

Liberación del mutex

Adquisición del mutex

Comprobar que la bandera está levantada

Leer los mensajes recibidos

Liberar el mutex

Procesar los mensajes

Figura 3.6 Diagrama conceptual que muestra la comunicación entro los hilos.

3.4 Archivos launch para lanzar la aplicación 31

El uso del mutex es necesario, ya que en el caso de que no lo usaramos, podrían darse situaciones nodeseadas como las siguientes:

• Se recibe un mensaje, y se comienza a ejecutar el callback en el hilo principal. Se levanta la banderaindicando que hay un nuevo mensaje disponible. La ejecución del callback es interrumpida para darpaso al hilo de procesamiento, el cuál comprueba el estado de la bandera, y al estar levantada cree que yapuede leer los mensajes, sin embargo, éstos aún no han sido almacenados en las variables compartidas.El contenido de las variables compartidas será nulo en el caso de que aún no se haya recibido ningúnmensaje previo, o contendrá los mensajes inmediatamente anteriores.

• Al igual que antes, se recibe unmensaje, y se ejecuta el callback en el hilo principal. Se levanta la banderaindicando la disponibilidad de nuevos mensajes, y se almacenan éstos en las variables compartidas.El hilo de procesamiento continúa su ejecución mientras que el hilo principal también lo hace. Serecibe otro mensaje y el hilo principal sube de nuevo la bandera. Inmediatamente después, el hilo deprocesamiento la baja. Se ha levantado dos veces la bandera, sin embargo, para el hilo de procesamientoaparenta ser una única vez. Es decir, se ignora uno de los mensajes. Además, puede darse el caso deque mientras que el hilo de procesamiento esté leyendo las variables compartidas, éstas estén siendomodi�cadas por el hilo principal.

3.4 Archivos launch para lanzar la aplicación

Para evitar tener que crear un �chero launch especí�camente para cada experimento a realizar, he creadolos siguientes �cheros:

• pointcloudxyzrgb_generation.launch

• pointcloudxyz_generation.launch

• visual_odom_test_odometry.launch

• visual_odom_test_tf.launch

Los �cheros pointcloudxyzrgb_generation.launch y pointcloudxyz_generation.launch in-cluyen las órdenes de lanzamiento de los nodos necesarios para convertir imágenes de profundidad y de colora nubes de puntos. La con�guración de los nodos presentes en este �chero (es decir, el remapeo de tópicospor ejemplo), es especí�ca para producir nubes de puntos a partir de bags del robot SIAR. En caso de quese desee usar en otros contextos será necesario modi�carlo. Como se puede deducir de sus nombres, el �che-ro pointcloudxyzrgb_generation.launch genera nubes de puntos a color, combinando las imágenesde profundidad y rgb, mientras que el �chero pointcloudxyz_generation.launch genera puntos cuyaúnica información asociada a ellos son sus coordenadas. El contenido de ambos �cheros se puede ver en losfragmentos de código . y ., respectivamente.

Código 3.4 Contenido del archivo pointcloudxyzrgb_generation.launch.

<launch><param name="/use_sim_time" value="true" type="boolean" />

<group ns="image_proc"><node name="depth_republisher" pkg="image_transport" type="republish" args=

"compressedDepth raw" output="screen"><remap from="in" to="/front/depth_registered/image_raw"/><remap from="out" to="front/depth/image_decompressed"/>

</node><node name="rgb_republisher" pkg="image_transport" type="republish" args="

compressed raw" output="screen"><remap from="in" to="/front/rgb/image_raw"/><remap from="out" to="front/rgb/image_decompressed"/>

</node>

<node name="nodelet_manager" pkg="nodelet" type="nodelet" args="manager"

output="screen"/>

<node name="depth_to_rgb_frame_registering" pkg="nodelet" type="nodelet"args="load depth_image_proc/register nodelet_manager" output="screen"><remap from="rgb/camera_info" to="/front/rgb/camera_info"/><remap from="depth/camera_info" to="/front/depth_registered/camera_info"/>

<remap from="depth/image_rect" to="front/depth/image_decompressed"/><remap from="depth_registered/camera_info" to="front/depth_registered/

camera_info"/><remap from="depth_registered/image_rect" to="front/depth_registered/

image_rect"/></node>

<node name="pointcloud_generator" pkg="nodelet" type="nodelet"args="load depth_image_proc/point_cloud_xyzrgb nodelet_manager" output="

screen"><remap from="rgb/camera_info" to="/front/rgb/camera_info"/><remap from="rgb/image_rect_color" to="front/rgb/image_decompressed"/><remap from="depth_registered/image_rect" to="front/depth_registered/

image_rect"/><remap from="depth_registered/points" to="pointcloud"/>

</node></group>

</launch>

Código 3.5 Contenido del archivo pointcloudxyz_generation.launch.

<launch><param name="/use_sim_time" value="true" type="boolean" />

<group ns="image_proc"><node name="depth_republisher" pkg="image_transport" type="republish"args="compressedDepth raw" output="screen"><remap from="in" to="/front/depth_registered/image_raw"/><remap from="out" to="front/depth/image_decompressed"/>

</node>

<node name="cam_info_rep" pkg="topic_tools" type="relay" output="screen"

args="/front/depth_registered/camera_info front/depth/camera_info"/>

<node name="nodelet_manager" pkg="nodelet" type="nodelet" args="manager"

output="screen"/>

<node name="pointcloud_generator" pkg="nodelet" type="nodelet"args="load depth_image_proc/point_cloud_xyz nodelet_manager" output="

screen"><remap from="camera_info" to="/front/depth_registered/camera_info"/>

<remap from="image_rect" to="front/depth/image_decompressed"/><remap from="points" to="pointcloud"/>

</node></group>

</launch>

Además de estos �cheros, disponemos de otros dos �cheros que a su vez hacen uso de éstos mediante unasentencia include. Éstos dos �cheros son:

• visual_odom_test_odometry.launch, cuyo contenido se puede ver en el fragmento de código..

• visual_odom_test_tf.launch, cuyo contenido se puede ver en el fragmento de código ..

Éstos �cheros contienen las órdenes necesarias para lanzar ademaś de los nodos de generación de las nubes depuntos, el nodo rosbag encargado de la reproducción del archivo bag, rviz para visualizar el experimento,el nodo de odometría visual, un nodo para publicar una transformación estática, y nodos para publicar losmensajes de la odometría en forma de mensajes tf.

Código 3.6 Contenido del archivo visual_odom_test_odometry.launch.

<?xml version="1.0" encoding="UTF-8"?><launch><arg name="exp_num" default="1"/>

<include file="$(find visual_odom)/launch/pointcloudxyzrgb_generation.launch"/>

<node pkg="rosbag" type="play" name="rosbag_player" output="screen"args="--clock -s 1080 $(find visual_odom)/bags/

siar_front_camera_with_odom_and_imu.bag" />

<rosparam command="load" file="$(find visual_odom)/Experimentos/Experimento_$(

arg exp_num)/visual_odom.config" />

<node name="visual_odom" pkg="visual_odom" type="visual_odom" output="

screen"><param name="config_file" value="$(find visual_odom)/Experimentos/

Experimento_$(arg exp_num)/ICPConfig.yaml"/><remap from="point_cloud" to="/image_proc/pointcloud" /><remap from="odom_topic" to="/odom" />

</node>

<node name="tf_from_odom" pkg="visual_odom" type="tf_from_odom"><param name="use_odom_msg_parent_frame_name" value="true" type="bool" /><param name="use_odom_msg_child_frame_name" value="false" type="bool" /><param name="parent_frame" value=" " type="string" /><param name="child_frame" value="base_link" type="string" /><param name="invert_transform" value="false" type="bool" /><remap from="odom" to="/odom" />

</node>



<node name="static_tf" pkg="tf2_ros" type="static_transform_publisher"args="0 0 0 0 0 0 odom visual_odom" />

<node name="rviz" pkg="rviz" type="rviz"args="-d $(find visual_odom)/launch/visual_odom_odometry.rviz"/>

</launch>

Código 3.7 Contenido del archivo visual_odom_test_tf.launch.

<?xml version="1.0" encoding="UTF-8"?><launch><arg name="exp_num" default="1"/>

<include file="$(find visual_odom)/launch/pointcloudxyzrgb_generation.launch"/>

<node pkg="rosbag" type="play" name="rosbag_player" output="screen"args="--clock -s 1080 $(find visual_odom)/bags/

siar_front_camera_with_odom_and_imu.bag" />

<rosparam command="load"file="$(find visual_odom)/Experimentos/Experimento_$(arg exp_num)/visual_odom.

config" />

<node name="visual_odom" pkg="visual_odom" type="visual_odom" output="

screen"><param name="config_file"value="$(find visual_odom)/Experimentos/Experimento_$(arg exp_num)/

ICPConfig.yaml"/><remap from="point_cloud" to="/image_proc/pointcloud" /><remap from="odom_topic" to="/odom" />

</node><node name="tf_from_odom" pkg="visual_odom" type="tf_from_odom" output="

screen"><param name="use_odom_msg_parent_frame_name" value="false" type="bool" /><param name="use_odom_msg_child_frame_name" value="false" type="bool" /><param name="parent_frame" value="visual_odom" type="string" />

<param name="child_frame" value="base_link_visual_odom" type="string" /><param name="invert_transform" value="false" type="bool" /><remap from="odom" to="/visual_odom" />

</node><node name="tf_from_odom2" pkg="visual_odom" type="tf_from_odom"><param name="use_odom_msg_parent_frame_name" value="true" type="bool" /><param name="use_odom_msg_child_frame_name" value="false" type="bool" /><param name="parent_frame" value="odom" type="string" /><param name="child_frame" value="base_link" type="string" /><param name="invert_transform" value="false" type="bool" /><remap from="odom" to="/odom" />

</node>



<node name="static_tf" pkg="tf2_ros" type="static_transform_publisher"args="0 0 0 0 0 0 odom visual_odom" />

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find visual_odom)/launch/visual_odom_tf.rviz"/>

</launch>

Como se puede observar, la única diferencia entre ambos, es que en el segundo archivo se realiza la conver-sión de los mensajes de odometría a mensajes tf, tanto del tópico /odom como de /visual_odom, mientrasque en el primero sólo se realiza la conversión de los mensajes del tópico /odom. Ésto se puede hacer porquerviz posee una opción para visualizar directamente la posición indicada por los mensajes de odometría.Ambos archivos están pensados para que se pueda modi�car la con�guración aplicada a los nodos sin necesi-dad de modi�car ninguna de sus líneas. Únicamente, deberían ser lanzados mediante una orden similar a lasiguiente:

roslaunch visual_odom visual_odom_test_odometry.launch exp_num:=[1,2,3,...]

Dondeexp_num es el argumento que permite aroslaunch determinar la ruta a los archivos de con�guraciónespecí�cos del experimento en cuestión. Esto es posible gracias a que dentro de la estructura de directoriosdel paquete visual_odom se puede acceder a los archivos correspondientes a cada experimento siguiendouna convención de nombrado, en la que lo único que varía es el número del experimento. La disposición deéstos archivos se puede observar grá�camente en el diagrama ..Los archivos de con�guración necesarios son ICPConfig.yaml, en el que se detalla la con�guración del

algoritmo ICP; y visual_odom.config en el que se incluyen otros parámetros necesarios para el funciona-miento del nodo visual_odom. Dos ejemplos del contenido de éstos �cheros son los mostrados en . y ..

Código 3.8 Ejemplo de archivo de con�guración ICPConfig.yaml.


dim: -1maxDist: 7


dim: -1maxDist: 7

- RandomSamplingDataPointsFilter:

Rviz es la herramienta de visualización de ROS


visual_odom/

...

src/

include/

launch/

Experimentos/

Experimento_1/

ICPConfig.yaml

visual_odom.config

Experimento_2/

Experimento_.../

Figura 3.7 Disposición de los archivos correspondientes a cada experimento.

prob: 0.2

matcher:KDTreeMatcher:maxDist: 0.5epsilon: 3.16

outlierFilters:




- CounterTransformationChecker:maxIterationCount: 20

- BoundTransformationChecker:maxRotationNorm: 1maxTranslationNorm: 1

inspector:# VTKFileInspectorNullInspector

Código 3.9 Ejemplo de archivo de con�guración visual_odom.config.

visual_odom: {absolute_guess: false, base_link_frame: base_link, camera_frame:front_rgb_optical_frame, processing_rate: 0.4, tf_wait_time: 20}

El resultado de invocar el �chero visual_odom_test_odometry.launch, que a su vez supone la invo-cación del �chero pointcloudxyzrgb_generation.launch, es la ejecución de todos los nodos en ellosindicados y el establecimiento de conexiones a través de ciertos tópicos tal como se muestra en la �gura ..


Figura 3.8 Grafo de nodos correspondiente a la ejecución del nodo de odometría visual, y los nodos auxiliares.Las elipses representan los nodos y los rectángulos los tópicos mediante los que se comunican.

4 Experimentos y resultados

Aquí mostraría distintos experimentos realizados, especi�cando las con�guraciones usadas, los mapasusados, los resultados obtenidos, algunas capturas de pantalla, etc.

Análisis de los resultados.

4.1 Experimento inicial

Este primer experimento tiene como objetivo comprobar el correcto funcionamiento del nodo visual_odom,y analizar la precisión de la odometría calculada. Haré uso del archivo launch creado especí�camente para larealización de los experimentos, invocándolo con el argumento exp_num:=1. En este caso, la con�guraciónaplicada al algoritmo ICP, de�nida en un archivo YAML, es la que se muestra en el código ..

Código 4.1 Contenido del archivo ICPConfig.yaml.


dim: -1maxDist: 7


dim: -1maxDist: 7

- RandomSamplingDataPointsFilter:prob: 0.2

matcher:KDTreeMatcher:maxDist: 0.5epsilon: 3.16

outlierFilters:




- CounterTransformationChecker:

39

40 Capítulo 4. Experimentos y resultados

maxIterationCount: 20- BoundTransformationChecker:

maxRotationNorm: 1maxTranslationNorm: 1

inspector:# VTKFileInspectorNullInspector

La con�guración del nodo de odometría visual será la misma que la mostrada en el código .. Aplicandoesta con�guración, y visualizando los resultados en rviz, observamos que la odometría visual diverge amplia-mente con respecto a la odometría del robot, por lo tanto deberemos re�nar la con�guración del algoritmopara que se ajuste a las características del entorno y conseguir unos mejores resultados. La amplia divergenciase puede observar en la �gura ..

Los ejes de referencia de mayor tamaño corresponden a la última posición calculadamediante la odometría de las ruedas, mientras que los ejes más pequeños se correspon-den a las sucesivas posiciones calculadas por el nodo de odometría visual.

Figura 4.1 Amplia divergencia entre la odometría visual y la propia del robot.

Esta divergencia se da principalmente en la componente traslacional de la odometría. Pero posteriormente,cuando el robot llega a un cruce de túneles, se introduce parcialmente en el canal central, inclinándose brus-camente. Es en ese momento cuando se introduce un error en la componente rotacional del algoritmo, quetambién habría que subsanar. Para ello sería necesario encontrar una con�guración adecuada del algoritmoICP, o incluso modi�car el código, para que descarte aquellas nubes de puntos que no superen cierto tamaño(esto puede ser debido a que la cámara está demasiado cerca de algún objeto y por lo tanto no es capaz demedir correctamente la distancia a él). Éstas nubes de puntos incompletas, pueden inducir a errores en la trans-formación calculada por el algoritmo, ya que en algunos casos puede que las nubes que se pretenden alinearpresenten pocos puntos en común o ninguno(pero aún así el algoritmo ICP hallará falsas correspondenciasentre ambos conjuntos), debido a que en la región del espacio en la que ambas nubes de puntos deberíansolaparse no se han capturado su�cientes puntos.

4.2 Análisis offline del proceso de alineación de las nubes de puntos 41

4.2 Análisis offline del proceso de alineación de las nubes de puntos

Para determinar cuáles deben ser las modi�caciones a la con�guración del algoritmo para conseguir mejoresresultados, se estudiará el proceso de alineación de las nubes de puntos en diferido, para posteriormente apli-car las conclusiones obtenidas para con�gurar el nodo visual_odom, cuyos cálculos se realizan en tiemporeal. Se creará para ello un programa que almacene las nubes de puntos, junto con la estimación de las trans-formaciones relativas entre cada una de ellas. Este programa será también un nodo de ROS dentro del paquetevisual_odom cuyo nombre será pointcloud_transform_saver. Este nodo se suscribirá al tópico en elque se publican las nubes de puntos, y almacenará tres archivos por cada nube de puntos capturada. Estostres archivos serán: la nube de puntos en un archivo pcd, la transformación relativa que ha experimentado elsistema de referencia de la cámara (expresada como unamatriz x en un archivo de texto plano), y lasmarcasde tiempo tanto de la nube de puntos como del mensaje de odometría del que se ha derivado la transforma-ción. Se podrá seleccionar mediante parámetros de ROS tanto el instante inicial a partir del cuál el nodo debeempezar a guardar los datos, como el instante �nal, además de la frecuencia a la que deben ser capturadas lasnubes de puntos (esta frecuencia debe ser inferior a aquella a la que los mensajes son publicados en el tópico,es decir, menor a Hz).Para lanzar el nodo pointcloud_transform_saver, junto con el resto de nodos necesarios para produ-

cir las nubes de puntos que serán almacenadas, se hará uso del archivo launchmostrado en el código ., queserá lanzado a su vez haciendo uso del shell script pointcloud_transform_saver.sh, cuyo contenidose muestra en el código ..

Código 4.2 Contenido del archivo pointcloud_transform_saver.launch.

<?xml version="1.0" encoding="UTF-8"?><launch><arg name="initialskip" doc="How many seconds from the beginning of bag

file are going to be ignored"/><arg name="fileBaseName" default="save_pcloud_tf"/><arg name="binary_mode" default="false"/><arg name="working_directory" default="$(env HOME)/.ros"/><arg name="start_time"/><arg name="stop_time"/><arg name="rate"/><arg name="save_base_link_camera_tf" default="true"/><include file="$(find visual_odom)/launch/pointcloudxyzrgb_generation.launch"

/><node pkg="rosbag" type="play" name="rosbag_player" output="screen" args="-d

5 --clock -s$(arg initialskip) $(find visual_odom)/bags/siar_front_camera_with_odom.bag"

/><node pkg="visual_odom" type="pointcloud_transform_saver" name="

pcloud_tf_saver"output="screen" args="" required="true"><remap from="cloud_in" to="/image_proc/pointcloud"/><remap from="odom" to="odom"/><param name="capture/rate" value="$(arg rate)" type="double"/><param name="capture/start_time" value="$(arg start_time)" type="double"/><param name="capture/stop_time" value="$(arg stop_time)" type="double"/><param name="capture/fileBaseName" value="$(arg fileBaseName)" type="string"

/><param name="capture/binary_mode" value="$(arg binary_mode)" type="bool"/><param name="base_link_frame" value="base_link" type="string"/><param name="camera_frame" value="front_rgb_optical_frame" type="string"/>

El código fuente del nodo pointcloud_transform_saver se puede observar en la sección C. del apéndice C. El shell script podrá ser ejecutado haciendo uso de rosrun, dándole previamente permiso de ejecución.


<param name="working_directory" value="$(arg working_directory)" type="string"/>

<param name="capture/save_base_link_camera_tf" value="$(argsave_base_link_camera_tf)" type="bool"/>

</node></launch>

Código 4.3 Contenido del archivo pointcloud_transform_saver.sh.

#!/bin/bash

# Get path to visual_odom packageVISUALODOMPATH=‘rospack find visual_odom‘

# Read bag information file and extract beginning timeROSBAGINITTIME=$( cat $VISUALODOMPATH/bags/siar_front_camera.info | sed -n ’4

s/.*($.*$).*/\1/p’ )

# Set capture start offset from bag file beginningSTARTCAPTUREOFFSET=1160# Set rosbag replay offset 5 seconds earlier than $STARTCAPTUREOFFSET# This allows pointclouds to start being published before capture startSKIPOFFSET=$(($STARTCAPTUREOFFSET - 5))# Set how many seconds the pointcloud’s capture will lastCAPTUREDURATION=100# Set how many clouds will be captured per second (of simulation time)CAPTURERATE=1.0# Calculate capture start and stop timesSTART_TIME=$( python -c "print $ROSBAGINITTIME + $STARTCAPTUREOFFSET" )STOP_TIME=$( python -c "print $START_TIME + $CAPTUREDURATION" )# Set working directory, where clouds will be savedWORKINGDIRECTORY=$VISUALODOMPATH/Experimentos/Experimento_1/saved_clouds

# Launch ros nodesroslaunch visual_odom pointcloud_transform_saver.launch initialskip:=

$SKIPOFFSET start_time:=$START_TIME stop_time:=$STOP_TIMEworking_directory:=$WORKINGDIRECTORY rate:=$CAPTURERATE

El archivo pointcloud_transform_saver.launch permite reproducir el contenido del archivo bagque nos interesa analizar (siar_front_camera_with_odom.bag), a la vez que se ejecutan los nodos quegeneran las nubes de puntos mediante la inclusión del archivo pointcloudxyzrgb_generation.launch,y simultáneamente se van almacenando las nubes de puntos mediante el nodo pointcloud_transform_-saver. La reproducción del archivo bag no comenzará desde el instante inicial a partir del cuál los mensajesempezaron a ser almacenados, sino quemediante el argumento initialskip (del archivo launch) se puedeindicar que se salte una parte del archivo. Mediante los parámetros capture/start_time y capture/s-top_time del nodo pointcloud_transform_saver se indica el intervalo de tiempo durante el que secapturarán y almacenarán las nubes de puntos y la frecuencia de captura se indicará mediante el parámetrocapture/rate. Para dar tiempo a que las nubes de puntos comiencen a ser generadas a partir de las imá-genes de profundidad disponibles en el archivo bag, y sean publicadas en el tópico, el instante de comienzode la captura tendrá un desfase con respecto al instante de comienzo de la reproducción del archivo bag. La�nalidad del shell script pointcloud_transform_saver.sh es calcular éstos parámetros y pasarlos comoargumentos al lanzar el archivo pointcloud_transform_saver.launchmediante la siguiente orden :

roslaunch visual_odom pointcloud_transform_saver.launch initialskip:=$SKIPOFFSET start_time:=$START_TIME stop_time:=$STOP_TIMEworking_directory:=$WORKINGDIRECTORY rate:=$CAPTURERATE


Una vez que ha �nalizado la ejecución del nodo de almacenamiento, podemos observar las nubes de puntoshaciendo uso del shell script mostrado en ., el cuál muestra secuencialmente (siendo necesario pulsarcualquier tecla para continuar) las nubes de puntos guardadas, haciendo uso del visor pcl_viewer incluidoen el paquete de herramientas de la librería pcl.

Código 4.4 Contenido del archivo view_captured_clouds.sh, el cuál es un shell script para automatizarla visualización de las nubes de puntos.

#!/bin/bash# This script will show every saved pointcloud in pcd format in the current

directory.# It will use pcl_viewer to show them. They will be shown sequentially.# The reference axis will be displayed

cd saved_clouds

# Get current terminal window namePREVIOUS_NAME=$(xdotool getwindowfocus getwindowname)# Change window nameNAME="VIEW_CLOUDS_SCRIPT_$$"; echo -en "\033];$NAME\a"# Set auxiliar file nameAUX_CLOUD_FILE_NAME="cloud_aux.pcd"

# Read transform information and put it in the right formatTRANSFORM_FILE_MATRIX=$( cat Tbase_link_camera.txt )TRANSFORM_FILE_MATRIX=$( echo "$TRANSFORM_FILE_MATRIX" | sed ’s/ /,/g’ | tr -d

$’\n’ )TRANSFORM_FILE_MATRIX=${TRANSFORM_FILE_MATRIX::-1}TRANSFORM_OPTIONS="-matrix $TRANSFORM_FILE_MATRIX"

# Delete auxiliar file from previous execution[ -e $AUX_CLOUD_FILE_NAME ] && rm $AUX_CLOUD_FILE_NAME

for file in $( ls -v *.pcd); do# Transform every point cloud to show them in base_link framepcl_transform_point_cloud $file $AUX_CLOUD_FILE_NAME $TRANSFORM_OPTIONS &> /

dev/null# Show cloud and axispcl_viewer -ax 1 -cam pcd_viewer.cam $AUX_CLOUD_FILE_NAME &> /dev/null &# Save PID of pointcloud viewer to kill it laterVIEWER_PID=$!# Wait for the viewer to appear in screensleep 0.75s# Focus on terminal againwmctrl -F -a $NAMEecho "Showing file $file"read -p "Press any key to continue, q to exit" -n1 -s KEY# Kill viewerkill -INT $VIEWER_PID# End loop if q has been pressedif [ "$KEY" = "q" ]; then { echo ""; break; } fi

done

# Reset previous window titleecho -en "\033];$PREVIOUS_NAME\a"cd ..


La ejecución de éste shell script (view_captured_clouds.sh da como resultado la pantalla que semues-tra en la �gura ..

Figura 4.2 Visualización secuencial de las nubes de puntos capturadas para su análisis.

De ésta manera se pueden ir ajustando los valores de los parámetros start_time y stop_time paradeterminar el intervalo de tiempo (y las nubes de puntos correspondientes a él) que nos interesa analizar.En éste caso, las nubes de puntos se corresponden con el momento en el que el robot atraviesa un cruce de

túneles.Una vez determinadas las nubes de puntos de interés, se pasaría a ejecutar el algoritmo ICP en una selección

de éstas nubes de puntos para determinar cuál debe ser la con�guración a aplicar en la ejecución en tiempo real.Para la ejecución del algoritmo se hará uso del programa pmicp, basado en la librería libpointmatcher eincluido junto con el código de ésta.Para poder hacer uso de éste programa debemos hacer una conversión al formato en el que las nubes de

puntos han sido guardadas. Pasará de ser pcd a csv. Un archivo de ejemplo en éste formato sería el mostradoa continuación.

x,y,z,rgb1,2,3,92,4,1,0

Para hacer la conversión haré uso de un script al que llamaré format_conversion.sh, cuyo código semuestra en el fragmento ..

Código 4.5 Contenido del archivo format_conversion.sh, el cuál realiza la conversión de formato de unarchivo pcd a csv.

FILE_NAME=$1

# The first 11 lines are removed, then nan is substituted by NaN, and thenspaces are changed by ,

tail -n +12 $FILE_NAME | sed ’s/nan/NaN/g’ | sed ’s/ /,/g’ | sed ’1ix,y,z,rgb’> ${FILE_NAME::-4}.csv

Una vez transformados los archivos al formato adecuado, se usará el comando siguiente


pmicp --config config.yaml --initTranslation $TRANSLATION --initRotation$ROTATION save_pcloud_tf_cloud0.csv save_pcloud_tf_cloud1.csv

En el cuál $TRANSLATION y $ROTATION deben ser extraídas del archivo save_pcloud_tf_transf1.txt.Haciendo pruebas con distintos valores de con�guración del algoritmo se pueden encontrar aquellos que

mejor se adapten a las características del entorno.

5 Conclusiones y trabajos futuros

Resulta complicado encontrar una con�guración del algoritmo ICP que logre unos resultados con pocadesviación con respecto a lo esperado. Esto se debe principalmente a las características del entorno,

que al ser tan homogéneo longitudinalmente, resulta casi imposible hacer una distinción entre una nube depuntos tomada un instante y la siguiente. Al no haber elementos característicos que distingan una nube depuntos de la siguiente, y al ser éstas alargadas en su componente x (expresada en el sistema de referenciabase_link, el algoritmo es incapaz de encontrar la componente traslacional que realmente se correspondecon el movimiento que ha experimentado el robot. A ésto hay que sumarle los problemas que se producencuando el robot se hunde momentáneamente en el canal central del túnel, produciendo nubes de puntos conexcaso número de ellos (al no ser capaz la cámara de medir las distancias a gran parte del entorno debido ala excesiva cercanía). La alineación de éstas nubes de puntos de tamaño tan reducido dan resultados poco�ables debido a que pueden ser encajadas en las nubes de puntos de referencia de muchas maneras que elalgoritmo considerará válidas y sin embargo se alejarán mucho de serlo.Un trabajo futuro sería combinar las nubes de puntos obtenidas a través de todas las cámaras de las que dis-

pone el robot ( cámaras, cada una orientada en una dirección), en una única nube de puntos. Ésto permitiríaprobablemente mejorar la precisión del algoritmo a la hora de encontrar la transformación que mejor alineanubes de puntos consecutivas, al disponer de unamayor cantidad de puntos. También subsanaría el problemaque presenta el algoritmo cuando la cámara es incapaz de medir las distancias ya que se encuentra demasiadocerca de los obstáculos, y devuelve nubes de puntos nulas o con un número de puntos muy reducido, lo queprovoca que el algoritmo sufra una gran desviación con respecto a la realidad. Al disponer de cinco cámaras,es poco probable que todas ellas estén simulatáneamente bloqueadas o muy cerca de un obstáculo para nopoder medir la distancia a los puntos de éste.Una vez encontrada la con�guración del algoritmo que mejor se adapte a las condiciones del entorno,

quizás sería conveniente convertir el código para que hiciera uso de la librería pcl a la hora de implementarel algoritmo ICP, para lograr un mayor rendimiento (habría que realizar pruebas de ejecución para ver quécódigo es maś rápido con la misma con�guración).

47

Apéndice APreparación del entorno de desarrollo

Aquí se presentan las herramientas usadas, y las con�guraciones aplicadas para preparar el entorno de desa-rrollo.

A.1 Instalación de ROS

Las instrucciones para instalar ROS están perfectamente detalladas en su página o�cial, a la que se puedeacceder a través del siguiente enlace http://wiki.ros.org/kinetic/Installation/Ubuntu. En mi caso, trabajaré conla versión ROS Kinetic Kame, instalada sobre el sistema operativo Ubuntu 16.04.2 LTS.

A.2 Descarga y compilación del paquete ethzasl_icp_mapping

Éste paquete de ROS no se encuentra disponible en forma de paquete de Debian, por lo que será necesariocompilarlo desde el código fuente. Para ello ejecutaremos primero el código A..

Código A.1 Creación del espacio de trabajo y descarga del código.

# Creamos el espacio de trabajo y el subdirectorio para el código fuentemkdir -p ~/catkin_ws_ethzasl/srccd ~/catkin_ws_ethzasl/src# Inicializamos el espacio de trabajocatkin_init_workspace

# Las siguientes instrucciones nos permiten enlazar el espacio# de trabajo actual con varios repositorios remotoswstool initwstool set ethzasl_icp_mapping --git \git://github.com/ethz-asl/ethzasl_icp_mapping.gitwstool update

cd ./ethzasl_icp_mapping# Nos cambiamos a la rama indigo_devel que es la que# compila usando Ubuntu 16.04 y ROS Kinetic Kamegit checkout indigo_devel

Antes de proseguir, se debe instalar la dependencia libargtable2–dev. Además, se deben realizar mo-di�caciones en varios �cheros para evitar que la compilación devuelva errores. Se deben cambiar las ocu-rrencias del comando find_package(Eigen) por find_package(Eigen3), en todos los archivos CMake-lists.txtde los paquetes a compilar. Por último, en el archivoCMakelists.txtdel paquete ethzasl_gridmap_2ddebemos añadir set(eigen3_INCLUDE_DIRS /usr/include/eigen3) y modi�car la línea en la que sede�nen los directorios donde buscar los archivos de cabecera para que quede así:include_directories(include${eigen3_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}).

49

http://wiki.ros.org/kinetic/Installation/Ubuntu

50 Capítulo A. Preparación del entorno de desarrollo

Una vez realizadas éstas modi�caciones, ya se puede pasar a compilar los paquetes descargados medianteel código mostrado en A..

Código A.2 Compilación del paquete ethzasl_icp_mapping.

cd ~/catkin_ws_ethzasl# Se debe usar la versión isolated de catkin_make# porque hay paquetes cuyo sistema de compilación# es simple CMakecatkin_make_isolated

A.3 Creación y configuración del espacio de trabajo donde desarrollar los paquetespropios

Para mantener una división clara entre el código desarrollado por mí, y aquel desarrollado por terceros, usaréun espacio de trabajo diferente para el código propio. Para lograr la coexistencia de ambos espacios de trabajo,haré uso de las indicaciones mostradas en las referencias [] y [].En este caso, crearé un espacio de trabajo llamado catkin_ws_tfg, también en el escritorio, usando los

mismos comandos que empleé para crear el anterior espacio de trabajo para alojar el código del paqueteethzasl_icp_mapping.Una vez creado, para conseguir hacer uso de ambos espacios de trabajo simultáneamente, lo que se conoce

como overlay (superposición); deberemos realizar los siguientes pasos:

1. Debemos realizar la con�guración correspondiente a la instalación general de ROS en el sistema. Éstoya se realiza automáticamente en cada invocación del terminal, mediante la inclusión de la sentencia:

source /opt/ros/kinetic/setup.bash

en el archivo.bashrc. Ésto con�gura adecuadamente las variables de entorno que ROS necesita parasu funcionamiento y localización de los paquetes.

2. Ejecutar el script de con�guración del espacio de trabajo catkin_ws_ethzasl, para que éste modi�-que las variables de entorno y así los paquetes presentes en él también sean localizados y accesibles porlas herramientas y comandos de ROS. Para ello deberemos ejecutar en el terminal la siguiente orden:

source ~/Escritorio/catkin_ws_ethzasl/devel_isolated/setup.bash

3. Una vez hecho ésto, nos desplazamos al espacio de trabajo donde desarrollaré el código propio. Paraello ejecutamos:

cd ~/Escritorio/catkin_ws_tfg

y seguidamente:

catkin_make

La función de éste comando, además de compilar, es crear un script de con�guración que debe serejecutado cada vez que se quiera hacer uso de los paquetes de ROS desarrollados en él. Antes de crear elscript de con�guración para éste espacio de trabajo, catkin_make lee las variables de entorno actualesespecí�cas de ROS y las tiene en cuenta para generarlo. Por lo tanto, al haber ejecutado previamente elscript de con�guración del espacio de trabajocatkin_ws_ethzasl, las variables de entornonecesariaspara hacer uso de él también estarán incluidas dentro del script de con�guración del espacio de trabajocatkin_ws_tfg.

4. Finalmente, para hacer uso tanto de los paquetes de ROS del sistema, cómo los de ambos espacios detrabajo creados, sólo será necesario ejecutar el siguiente comando en el terminal cada vez que se necesite:

source ~/Escritorio/catkin_ws_tfg/devel/setup.bash

O alternativamente, incluirlo en el archivo ~/.bashrc.

Apéndice BSistemas de coordenadas y

transformaciones

B.1 Sistemas de coordenadas básicos

Un vector en un espacio de tres dimensiones, es decir, v ∈ R, puede ser representado como una combinaciónlineal de tres vectores linealmente independientes v ,v ,v que forman una base, tal como se muestra en laecuación B..

v = α ⋅ v + α ⋅ v + αv (B.)

Los escalares α ,α ,α son las coordenadas del vector v en la base elegida. Si quisiéramos cambiar losvectores de la base, podríamos expresar los nuevos vectores como combinaciones de los anteriores, de lamanera mostrada en la ecuación B..

u = av + av + avu = av + av + avu = av + av + av

(B.)

Podríamos expresar este mismo cambio de base mediante una ecuación matricial (ecuación B.) en la queusaremos la matrizM mostrada en la ecuación B., a la que se denomina matriz de cambio de base.

M =

⎛

⎜

⎝

a a aa a aa a a

⎞

⎟

⎠

(B.)

⎛

⎜

⎝

uuu

⎞

⎟

⎠

=

⎛

⎜

⎝

a a aa a aa a a

⎞

⎟

⎠

⋅

⎛

⎜

⎝

vvv

⎞

⎟

⎠

(B.)

Usando estas dos bases, podemos expresar el mismo vector v en ambas tal como se indica en la ecuaciónB..

v = aT ⋅⎛

⎜

⎝

vvv

⎞

⎟

⎠

v = bT ⋅⎛

⎜

⎝

uuu

⎞

⎟

⎠

(B.)

Teniendo en cuenta las igualdades mostradas en la ecuación B..

a = (ααα)T

b = (βββ)T (B.)

51

52 Capítulo B. Sistemas de coordenadas y transformaciones

De las ecuaciones B. y B., se pueden deducir las igualdades mostradas en la ecuación B..

v = aT ⋅⎛

⎜

⎝

vvv

⎞

⎟

⎠

= bT⎛

⎜

⎝

uuu

⎞

⎟

⎠

= bTM⎛

⎜

⎝

vvv

⎞

⎟

⎠

(B.)

Esto a su vez permite deducir que a = MTb y que b = (MT)−.

B.2 Sistemas de coordenadas extendidos

Los sistemas de coordenadas anteriores no son su�cientemente completos, ya que sólo permiten expresarvectores. Para permitir operar con vectores y además con puntos (trasladarlos), es necesario añadir un puntode referencia al conjunto de vectores de la base. De esta forma, los vectores se expresarían de la mismamaneraque antes, mientras que para expresar un punto sería necesario sumar el punto de referencia P, tal como semuestra en la expresión B..

P = P + αv + αv + αv (B.)

Podemos expresar vectores y puntos de forma matricial tal como se muestra en la ecuación B..

v = (α α α ) ⋅ (v v v Po)

P = (α α α ) ⋅ (v v v P)(B.)

Siendo α ,α ,α , y α ,α ,α , las coordenadas homogéneas del vector y el punto respectivamente.

En este caso, para expresar un sistema de referencia en función de otro, será necesario tener en cuen-ta el punto origen del sistema de referencia. Por lo tanto, si quisiéramos expresar el sistema de referencia(u ,u ,u ,Q) en función de (v ,v ,v ,P), lo haríamos al igual que antes mediante una combinación linealtal como se muestra en la ecuación B..

u = av + av + avu = av + av + avu = av + av + avQ = av + av + av + P

(B.)

De esta manera obtendríamos la matrizM.

M =

⎛

⎜⎜⎜

⎝

a a a a a a a a a a a a

⎞

⎟⎟⎟

⎠

(B.)

Y por consiguiente podemos expresar la misma relación entre ambos sistemas de referencia de maneramatricial (Ecuación B.).

⎛

⎜⎜⎜

⎝

uuuQ

⎞

⎟⎟⎟

⎠

= M ⋅⎛

⎜⎜⎜

⎝

vvvP

⎞

⎟⎟⎟

⎠

(B.)

Si consideramos a = (α α α )Ty b = (β β β )

Tlas coordenadas homogéneas correspon-

dientes a un punto expresado en ambos sistemas de referencia, podemos igualar ambas representacionescomo se muestra en la ecuación B..

aT⎛

⎜⎜⎜

⎝

vvvP

⎞

⎟⎟⎟

⎠

= bT⎛

⎜⎜⎜

⎝

uuuQ

⎞

⎟⎟⎟

⎠

= bT ⋅M ⋅⎛

⎜⎜⎜

⎝

vvvP

⎞

⎟⎟⎟

⎠

(B.)

B.3 Transformaciones afines 53

De esta última ecuación se puede extraer la relación entre las coordenadas a y b, que es la que se muestraen la ecuación B..

a = MTbb = (MT

)− (B.)

Cabe resaltar el hecho de que para obtener el sistema de referencia u ,u ,u ,Q (al que denominaremossistema 2), es necesario multiplicar el sistema v ,v ,v ,P (al que denominaremos sistema 1) por la izquierdapor la matrizM, y sin embargo, para obtener las coordenadas de un punto o vector expresadas en un sistemaa partir de las coordenadas expresadas en el otro, es necesario invertir las posiciones de las coordenadas enla ecuación con respecto a la que ocuparían sus respectivos sistemas de referencia, es decir, para obtener lascoordenadas en el sistema 1multiplicaríamos las coordenadas del sistema 2 por la matrizMT .

B.3 Transformaciones afines

La matriz traspuesta MT mostrada en la ecuación B., representa una transformación afín, que es aquellaque conserva las relaciones de colinealidad y coplanaridad entre puntos.

MT=

⎛

⎜⎜⎜

⎝

a a a aa a a aa a a a

⎞

⎟⎟⎟

⎠

(B.)

Las transformaciones a�nes se pueden clasi�car en rotación, traslación, escalado y sesgo (del inglés skew).Aunque en este caso, sólo serán de interés la rotación y la traslación.

B.3.1 Traslación

La traslación es una operación muy sencilla que únicamente desplaza un punto una distancia �ja en unadeterminada dirección. La dirección y la distancia viene determinada por el vector de desplazamiento. Deesta manera, la transformación que desplazaría un punto p en la dirección del vector d = (αx ,αy ,αz) paraobtener el otro punto p′ sería la mostrada en la ecuación B..

T = T(αx ,αy ,αz) =⎛

⎜⎜⎜

⎝

αx αy αz

⎞

⎟⎟⎟

⎠

(B.)

B.3.2 Rotación

Para de�nir una rotación, es necesario un eje y un ángulo. Al trabajar en robótica con espacios en tres di-mensiones, nos interesan las rotaciones alrededor de los tres ejes cartesianos x ,y,z, ya que cualquier otratransformación alrededor de un eje distinto a estos, se puede obtener mediante combinación de rotacionesalrededor de ellos y traslaciones.

Si tenemos un punto con coordenadas x = ρ cos ϕ, y = ρ sin ϕ, y éste es rotado un ángulo θ, entonces lasnuevas coordenadas del punto serán

x′ = ρ cos(ϕ + θ), y′ = ρ sin(ϕ + θ) (B.)

Desarrollando estas expresiones (haciendo uso de identidades trigonométricas), tal como se muestra en laecuación B., llegamos a la expresión mostrada en la ecuación B..

x′ = ρ cos(ϕ + θ) = ρ cos(ϕ) cos(θ) − ρ sin(ϕ) sin(θ)y′ = ρ sin(ϕ + θ) = ρ sin(ϕ) cos(θ) + ρsin(θ) cos(θ)

(B.)

54 Capítulo B. Sistemas de coordenadas y transformaciones

x′ = x cos θ − y sin θy′ = x sin θ + y cos θ

(x′y′) = (

cos θ − sin θsin θ cos θ )(

xy)

(B.)

Y a partir de ésta transformación, resulta evidente obtener la matriz de rotación que permite girar un puntoalrededor del eje z, expresado este punto en coordenadas homogéneas.

Rz(θ) =⎛

⎜⎜⎜

⎝

cos θ − sin θ sin θ cos θ

⎞

⎟⎟⎟

⎠

(B.)

Siguiendo un razonamiento análogo, podemos obtener las matrices de rotación correspondientes a los ejesx e y.

Rx(θ) =⎛

⎜⎜⎜

⎝

cos θ − sin θ sin θ cos θ

⎞

⎟⎟⎟

⎠

(B.)

Ry(θ) =⎛

⎜⎜⎜

⎝

cos θ sin θ

− sin θ cos θ

⎞

⎟⎟⎟

⎠

(B.)

B.4 Cambios de base y transformaciones múltiples

Si partimos del sistema de referencia formado por (v ,v ,v ,P), y a partir de él obtenemos el sistema dereferencia formado por (u ,u ,u ,Q) , multiplicando por la matriz M tal como se muestra en la ecuaciónB..

⎛

⎜⎜⎜

⎝

uuuQ

⎞

⎟⎟⎟

⎠

= M⎛

⎜⎜⎜

⎝

vvvP

⎞

⎟⎟⎟

⎠

(B.)

Para posteriormente, obtener el sistema (w ,w ,w ,R), mediante la multiplicación por la matriz N talcomo se muestra en la ecuación B..

⎛

⎜⎜⎜

⎝

wwwR

⎞

⎟⎟⎟

⎠

= N⎛

⎜⎜⎜

⎝

uuuQ

⎞

⎟⎟⎟

⎠

= N ⋅M ⋅⎛

⎜⎜⎜

⎝

vvvP

⎞

⎟⎟⎟

⎠

(B.)

Podríamos obtener las coordenadas a de un punto en el sistema de referencia (v ,v ,v ,P) a partir de lascoordenadas c expresadas en el sistema de referencia (w ,w ,w ,R), tal como se observa en la ecuación B..

a = (N ⋅M)T⋅ c = MT

⋅ NT⋅ c (B.)

B.5 Notación

No existe un criterio uni�cado para denotar las transformaciones entre sistemas de referencia. Podemos hacerreferencia a una misma transformación de diferentes maneras, dependiendo del criterio elegido para hacerlo.Usualmente, la matriz de transformación que nos interesa es la que anteriormente hemos denotado porMT ,es decir, la que permite obtener las coordenadas de un punto en un sistema a partir de las coordenadas enel otro. Si al sistema de referencia origen (aquel en el que están expresadas las coordenadas de un punto) lo

B.5 Notación 55

denominamos odom, y al sistema de referencia objetivo (aquel en el que queremos expresar las coordenadasdel punto) lo denominamosmap, podemos expresar la transformación entre unas coordenadas y otras de lassiguientes maneras:

• mapodomT : Esta notación es la que se usa en la referencia [], que forma parte de la wiki de ROS.

• Tmap−odom : Esta notación es la usada en el tutorial del paquete tf, al que se puede acceder en la refe-rencia [].

Optaré por hacer uso de la segunda notación, al resultar muy intuitiva, ya que los nombres de los sistemasde referencia se colocan en el mismo orden que sus correspondientes coordenadas, es decir, si tenemos unpunto expresado en el sistema odom y queremos expresarlo en el sistema map usaríamos la transformaciónmostrada en la ecuación B..

Pmap = Tmap−odom ⋅ Podom (B.)

Apéndice CCódigo

C.1 Código del nodo visual_odom

Código C.1 visual_odom_main.cpp.

#include "visual_odom/visual_odom.h"


// Initialise ROSros::init(argc, argv, "visual_odom");VisualOdom visualOdom;visualOdom.main();return 0;

}

Código C.2 visual_odom.h.

#ifndef VISUAL_ODOM_H#define VISUAL_ODOM_H

#include <boost/thread.hpp>

#include "ros/ros.h"#include "sensor_msgs/PointCloud2.h"#include "pointmatcher/PointMatcher.h"#include "pointmatcher_ros/transform.h"#include "nav_msgs/Odometry.h"

typedef float ScalarType;typedef PointMatcher<ScalarType> PM;typedef PM::DataPoints DP;

class VisualOdom {public:

VisualOdom();VisualOdom(const VisualOdom& orig);~VisualOdom();void main();

private:

57

58 Capítulo C. Código

// Functionsvoid configure();void pointCloudandOdomCallback(const sensor_msgs::PointCloud2ConstPtr&

pointCloudMsg,const nav_msgs::OdometryConstPtr& odomMsg);

void processingThreadFunctionAbsolute();void processingThreadFunctionRelative();

// Variables/*** Useful to publish and subscribe to topics,* and retrieve parameters from server*/ros::NodeHandle _privateNodeHandle;ros::NodeHandle _nodeHandle;

/*** Name of the YAML configuration file for the ICP chain*/std::string _icpConfigFile;

/*** Frequency at which we want the Point Clouds* to be processed. Point Clouds arriving at* higher rate will be discarded.*/float _processingRate;

/*** Object that does the ICP processing*/PM::ICP _icp;

// TransformationsPM::TransformationParameters _Tbase_link_camera;PM::TransformationParameters _Tcamera_base_link;PM::TransformationParameters _Todom_visual_base_link_t0;

//****************************

ros::Publisher _visualOdomPub;

std::string _base_link_frame;std::string _camera_frame;

// Flags and mutex for coordination between main thread and processingthread

bool _flagProcessing; // Must be initialised to falsebool _flagCloudAvailable; // Must be initialised to falseboost::mutex _threadCoordinationMutex;//********************************

// Variables to pass data between main thread and processing threadsensor_msgs::PointCloud2ConstPtr _receivedCloudConstMsgPtr;nav_msgs::OdometryConstPtr _receivedOdomConstMsgPtr;//*************************************

C.1 Código del nodo visual_odom 59

// Flags and variables to keep the status of the processing thread

boost::shared_ptr<PM::DataPoints> _lastCloudPtr;boost::shared_ptr<PM::DataPoints> _nextCloudPtr;//***************************

};

#endif /* VISUAL_ODOM_H */

Código C.3 visual_odom.cpp.

#include <fstream>

#include "visual_odom/visual_odom.h"#include "boost/function.hpp"#include "boost/bind.hpp"#include "visual_odom/message_transformation.h"#include "message_filters/cache.h"#include "tf2_ros/transform_listener.h"#include "tf2_ros/buffer.h"#include "tf2_eigen/tf2_eigen.h"#include "message_filters/subscriber.h"#include "message_filters/synchronizer.h"#include "message_filters/sync_policies/approximate_time.h"

/*** VisualOdom constructor*/VisualOdom::VisualOdom():_privateNodeHandle("~"),_Tbase_link_camera(PM::TransformationParameters::Identity(4,4)),_Todom_visual_base_link_t0(PM::TransformationParameters::Identity(4,4)),_flagProcessing(false),_flagCloudAvailable(false),_receivedCloudConstMsgPtr(),_receivedOdomConstMsgPtr(){}

/*** VisualOdom copy constructor** @param orig Reference to another VisualOdom object*/VisualOdom::VisualOdom(const VisualOdom& orig) {}

/*** VisualOdom destructor*/VisualOdom::~VisualOdom() {}

/*** This is the callback function called when new PointCloud messages


* are received from the topic "point_cloud" that this node is* subscribed to* @param msg*/void VisualOdom::pointCloudandOdomCallback(const sensor_msgs::

PointCloud2ConstPtr& pointCloudMsg,const nav_msgs::OdometryConstPtr& odomMsg)

{// If the processing thread is not doing any work, we update the PointCloud

and the Odom msg// pointers that we use to pass the data to the thread_threadCoordinationMutex.lock();_receivedCloudConstMsgPtr = pointCloudMsg;_receivedOdomConstMsgPtr = odomMsg;_flagCloudAvailable = true;_threadCoordinationMutex.unlock();

}

/** This function calculates the ICP’s initial guess using the transformation

between the last position* calculated by the visual odometry and the current position sent by the

wheel’s odometry**/void VisualOdom::processingThreadFunctionAbsolute(){

PM::TransformationParameters Todom_base_link_current;PM::TransformationParameters Tinitguess;PM::TransformationParameters Ticp;PM::TransformationParameters Todom_camera_t0;PM::TransformationParameters Tcamera_odom_t0;PM::TransformationParameters Tcamera_n_camera_0 = PM::

TransformationParameters::Identity(4,4);PM::TransformationParameters Tcamera_0_camera_n = PM::

TransformationParameters::Identity(4,4);PM::TransformationParameters Todom_visual_base_link;

ros::WallTime rosTime;ros::WallDuration icpElapsedTime;ros::Time cloudTimeStamp;

ros::Rate rosRate(_processingRate);// Loop for waiting for first cloudwhile (ros::ok()){

_threadCoordinationMutex.lock();if(_flagCloudAvailable){

_flagCloudAvailable = false;rosRate.reset();_lastCloudPtr = PointMatcher_ros::rosMsgToPointMatcherCloudPtr<

ScalarType>(*_receivedCloudConstMsgPtr);_Todom_visual_base_link_t0 = PointMatcher_ros::odomMsgToEigenMatrix<

ScalarType>(*_receivedOdomConstMsgPtr);Todom_camera_t0 = _Todom_visual_base_link_t0 * _Tbase_link_camera;


Tcamera_odom_t0 = Todom_camera_t0.inverse();_threadCoordinationMutex.unlock();break; // End the loop waiting for first cloud

}_threadCoordinationMutex.unlock();

}

while (ros::ok()){

rosRate.sleep();if( rosRate.cycleTime().sec > 1.05 * rosRate.expectedCycleTime().sec ){

ROS_WARN_STREAM("ICP is taking too much time: " << icpElapsedTime.toSec() << "s");

}else{

ROS_INFO_STREAM("ICP is taking: " << icpElapsedTime.toSec() << "s");}


_flagCloudAvailable = false;_flagProcessing = true;_nextCloudPtr = PointMatcher_ros::rosMsgToPointMatcherCloudPtr<

ScalarType>(*_receivedCloudConstMsgPtr);cloudTimeStamp = (*_receivedCloudConstMsgPtr).header.stamp;Todom_base_link_current = PointMatcher_ros::odomMsgToEigenMatrix<

ScalarType>(*_receivedOdomConstMsgPtr);}else{

ROS_INFO("PointCloud reception rate is less than the processing ratespecified");


// If the processing doesn’t need to be started, we just jump to thenext iteration of the loop

if(!_flagProcessing)continue;

// If not, we start to process the PointClouds// We need to calculate the initial transformation that we are going to

pass to the algorithm// This is the transformation between the camera frame in the n-1 time

instant and itself in the n time instant// We’ll call this transformation Tinitguess in the programTinitguess = Tcamera_n_camera_0 * Tcamera_odom_t0 *

Todom_base_link_current * _Tbase_link_camera;

rosTime = ros::WallTime::now();try {

Ticp = _icp.compute(*_nextCloudPtr, *_lastCloudPtr, Tinitguess);} catch (PointMatcher<ScalarType>::ConvergenceError &exception) {


// If there is a convergence error in the algorithm, we just ignorethe transformation

// calculated by it, and consider the initial guess is correct.// To ignore the transformation of the algorithm, we just equal its

returned transformation// to the identity matrix

ROS_WARN("ICP Convergence Error");Ticp = PM::TransformationParameters::Identity(4,4);

}icpElapsedTime = (ros::WallTime::now() - rosTime);// The transformation returned by the ICP algorithm is not the complete

transformation we are looking for, is the correction that needs tobe applied

// to Tinitguess in order to obtain the transformation that best alignthe two point clouds


// Here we calculate the visual odometry we’ll sendTodom_visual_base_link = Todom_camera_t0 * Tcamera_0_camera_n *

_Tcamera_base_link;

nav_msgs::Odometry visualOdomMsg = transformMatrixToOdomMsg<ScalarType>(Todom_visual_base_link, "visual_odom", _base_link_frame,cloudTimeStamp);

// And finally we publish itthis->_visualOdomPub.publish(visualOdomMsg);

_lastCloudPtr = _nextCloudPtr;

_flagProcessing = false;}

}

/** This function calculate the ICP’s initial guess using the transformation

between two consecutive* positions of the wheel’s odometry, so it doesn’t take into account previous

corrections made.*/void VisualOdom::processingThreadFunctionRelative(){

PM::TransformationParameters Todom_base_link_previous;PM::TransformationParameters Todom_base_link_current;PM::TransformationParameters Tinitguess;PM::TransformationParameters Ticp;PM::TransformationParameters Todom_camera_t0;PM::TransformationParameters Tcamera_odom_t0;PM::TransformationParameters Tcamera_n_camera_0 = PM::

TransformationParameters::Identity(4,4);PM::TransformationParameters Tcamera_0_camera_n = PM::

TransformationParameters::Identity(4,4);PM::TransformationParameters Todom_visual_base_link;

ros::WallTime rosTime;ros::WallDuration icpElapsedTime;


ros::Time cloudTimeStamp;

ros::Rate rosRate(_processingRate);// Loop for waiting for first cloudwhile (ros::ok()){


_flagCloudAvailable = false;rosRate.reset();_lastCloudPtr = PointMatcher_ros::rosMsgToPointMatcherCloudPtr<

ScalarType>(*_receivedCloudConstMsgPtr);_Todom_visual_base_link_t0 = PointMatcher_ros::odomMsgToEigenMatrix<

ScalarType>(*_receivedOdomConstMsgPtr);Todom_base_link_previous = _Todom_visual_base_link_t0;Todom_camera_t0 = _Todom_visual_base_link_t0 * _Tbase_link_camera;Tcamera_odom_t0 = Todom_camera_t0.inverse();_threadCoordinationMutex.unlock();break; // End the loop waiting for first cloud


}

while (ros::ok()){

rosRate.sleep();if( rosRate.cycleTime().sec > 1.05 * rosRate.expectedCycleTime().sec ){

ROS_WARN_STREAM("ICP is taking too much time: " << icpElapsedTime.toSec() << "s");

}else{

ROS_INFO_STREAM("ICP is taking: " << icpElapsedTime.toSec() << "s");}


_flagCloudAvailable = false;_flagProcessing = true;_nextCloudPtr = PointMatcher_ros::rosMsgToPointMatcherCloudPtr<

ScalarType>(*_receivedCloudConstMsgPtr);cloudTimeStamp = (*_receivedCloudConstMsgPtr).header.stamp;Todom_base_link_current = PointMatcher_ros::odomMsgToEigenMatrix<

ScalarType>(*_receivedOdomConstMsgPtr);_threadCoordinationMutex.unlock();

}else{

_threadCoordinationMutex.unlock();ROS_INFO("PointCloud reception rate is less than the processing rate

specified");}


// If the processing doesn’t need to be started, we just jump to thenext iteration of the loop

if(!_flagProcessing)continue;

// If not, we start to process the PointClouds// We need to calculate the initial transformation that we are going to

pass to the algorithm// This is the transformation between the camera frame in the n-1 time

instant and itself in the n time instant// We’ll call this transformation Tinitguess in the programTinitguess = _Tcamera_base_link * Todom_base_link_previous.inverse() *

Todom_base_link_current * _Tbase_link_camera;

rosTime = ros::WallTime::now();try {

Ticp = _icp.compute(*_nextCloudPtr, *_lastCloudPtr, Tinitguess);} catch (PointMatcher<ScalarType>::ConvergenceError &exception) {

// If there is a convergence error in the algorithm, we just ignorethe transformation

// calculated by it, and consider the initial guess is correct.// To ignore the transformation of the algorithm, we just equal its

returned transformation// to the identity matrix

ROS_WARN("ICP Convergence Error");Ticp = PM::TransformationParameters::Identity(4,4);

}icpElapsedTime = (ros::WallTime::now() - rosTime);// The transformation returned by the ICP algorithm is not the complete

transformation we are looking for, is the correction that needs tobe applied

// to Tinitguess in order to obtain the transformation that best alignthe two point clouds


// Here we calculate the visual odometry we’ll sendTodom_visual_base_link = Todom_camera_t0 * Tcamera_0_camera_n *

_Tcamera_base_link;

nav_msgs::Odometry visualOdomMsg = transformMatrixToOdomMsg<ScalarType>(Todom_visual_base_link, "visual_odom", _base_link_frame,cloudTimeStamp);

// And finally we publish itthis->_visualOdomPub.publish(visualOdomMsg);

// Here we update the reference pointcloud and odometry transformationfor the next iteration

_lastCloudPtr = _nextCloudPtr;Todom_base_link_previous = Todom_base_link_current;

_flagProcessing = false;}

}


/*** This function encapsulates all the functionality of the program**/void VisualOdom::main(){

if(!ros::isInitialized())ROS_WARN("ros::init() hasn’t been called");

// Here we retrieve the parameters from the server_privateNodeHandle.getParam("config_file", _icpConfigFile);_privateNodeHandle.getParam("processing_rate", _processingRate);_privateNodeHandle.getParam("camera_frame", _camera_frame);_privateNodeHandle.getParam("base_link_frame", _base_link_frame);bool absolute_guess;_privateNodeHandle.getParam("absolute_guess", absolute_guess);int tf_wait_time;_privateNodeHandle.getParam("tf_wait_time", tf_wait_time);

// ***********************************************

ros::Duration delayTime(1);delayTime.sleep();

// Save the transformation from base_link to camera// This transformation is supposed to be static, so we only need to query

it once// This block is enclosed in braces to limit the scope of the transform

listener// because we only need it at the start.{tf2_ros::Buffer tfBuffer;tf2_ros::TransformListener tfListener(tfBuffer);geometry_msgs::TransformStamped Tbase_link_camera;Tbase_link_camera = tfBuffer.lookupTransform(_base_link_frame,

_camera_frame, ros::Time(0), ros::Duration(tf_wait_time));Eigen::Affine3d eigenTr = tf2::transformToEigen(Tbase_link_camera);_Tbase_link_camera = eigenTr.matrix().cast<ScalarType>();}_Tcamera_base_link = _Tbase_link_camera.inverse();

// Configure ICPif (_icpConfigFile.empty()){

ROS_INFO("No ICP config file given, using default configuration");_icp.setDefault();

}else {

// load YAML configstd::ifstream ifs(_icpConfigFile.c_str());if (!ifs.is_open()){

ROS_WARN("Unable to open ICP config file, using default configuration");_icp.setDefault();

}else{

_icp.loadFromYaml(ifs);


ifs.close();}

}

// Advertise visual odometry topic_visualOdomPub = _nodeHandle.advertise<nav_msgs::Odometry>("visual_odom",

5);

message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSubscriber(_nodeHandle, "point_cloud", 1);

message_filters::Subscriber<nav_msgs::Odometry> odomSubscriber(_nodeHandle,"odom_topic", 1);

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,

nav_msgs::Odometry> OdomCloudSyncPolicy;

message_filters::Synchronizer<OdomCloudSyncPolicy> sync(OdomCloudSyncPolicy(10), pointCloudSubscriber, odomSubscriber);

sync.registerCallback(boost::bind(&VisualOdom::pointCloudandOdomCallback,this, _1, _2));

boost::thread *processingThread;// Depending on the parameter received, we choose to calculate ICP initial

guess one way or anotherif (absolute_guess){

processingThread = new boost::thread(boost::bind(&VisualOdom::processingThreadFunctionAbsolute, this));

}else{

processingThread = new boost::thread(boost::bind(&VisualOdom::processingThreadFunctionRelative, this));

}

// Start an infinite loop until Interrupt is receivedros::spin();

(*processingThread).join();delete processingThread;

}

C.2 Código del nodo tf_from_odom

Código C.4 tf_from_odom.cpp.

#include <ros/ros.h>#include <tf2/LinearMath/Quaternion.h>#include <tf2_ros/transform_broadcaster.h>#include <geometry_msgs/TransformStamped.h>#include <nav_msgs/Odometry.h>

C.2 Código del nodo tf_from_odom 67

class TfFromOdom{public:

TfFromOdom();void odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg);void main();

bool _fParentFrameName;std::string _parentFrameName;bool _fChildFrameName;std::string _childFrameName;bool _fInvertTransform;

};

TfFromOdom::TfFromOdom() :_fParentFrameName(true),_fChildFrameName(true),_fInvertTransform(false){}

void TfFromOdom::odomCallback(const nav_msgs::Odometry::ConstPtr& odom_msg){

static tf2_ros::TransformBroadcaster tf_broadcaster;geometry_msgs::TransformStamped transformStamped;

transformStamped.header.stamp = odom_msg->header.stamp;

if (_fParentFrameName)transformStamped.header.frame_id = odom_msg->header.frame_id;

elsetransformStamped.header.frame_id = _parentFrameName;

if (_fChildFrameName)transformStamped.child_frame_id = odom_msg->child_frame_id;

elsetransformStamped.child_frame_id = _childFrameName;

//transformStamped.transform.translation = odom_msg->pose.pose.position;transformStamped.transform.translation.x = odom_msg->pose.pose.position.x;transformStamped.transform.translation.y = odom_msg->pose.pose.position.y;transformStamped.transform.translation.z = odom_msg->pose.pose.position.z;//transformStamped.transform.rotation = odom_msg->pose.pose.orientation;transformStamped.transform.rotation.x = odom_msg->pose.pose.orientation.x;transformStamped.transform.rotation.y = odom_msg->pose.pose.orientation.y;transformStamped.transform.rotation.z = odom_msg->pose.pose.orientation.z;transformStamped.transform.rotation.w = odom_msg->pose.pose.orientation.w;

tf_broadcaster.sendTransform(transformStamped);}

void TfFromOdom::main(){

ros::NodeHandle nh;ros::NodeHandle privateNh("~");

privateNh.getParam("use_odom_msg_parent_frame_name", _fParentFrameName);


privateNh.getParam("use_odom_msg_child_frame_name", _fChildFrameName);privateNh.getParam("parent_frame", _parentFrameName);privateNh.getParam("child_frame", _childFrameName);privateNh.getParam("invert_transform", _fInvertTransform);

ros::Subscriber subscriber = nh.subscribe("odom", 50, &TfFromOdom::odomCallback, this);

ros::spin();}


ros::init(argc, argv, "tf_from_odom");TfFromOdom tfFromOdom;tfFromOdom.main();return 0;

}

C.3 Código del nodo pointcloud_transform_saver

Código C.5 pointcloud_transform_saver.cpp.

#include <iostream>#include <string>#include "boost/bind.hpp"#include "boost/function.hpp"#include "boost/thread.hpp"

#include <ros/ros.h>#include "sensor_msgs/PointCloud2.h"#include "geometry_msgs/TransformStamped.h"#include "nav_msgs/Odometry.h"#include "pointmatcher/PointMatcher.h"#include "pointmatcher_ros/transform.h"#include "tf2_ros/transform_listener.h"#include "tf2_ros/buffer.h"#include "tf2_eigen/tf2_eigen.h"#include "message_filters/subscriber.h"#include "message_filters/synchronizer.h"#include "message_filters/sync_policies/approximate_time.h"

#include <pcl_ros/point_cloud.h>#include <pcl_ros/transforms.h>#include <pcl/io/pcd_io.h>

#include "visual_odom/message_transformation.h"

typedef float ScalarType;typedef PointMatcher<ScalarType> PM;typedef PM::DataPoints DP;typedef pcl::PointCloud<pcl::PointXYZRGB> PCLPointCloudXYZRGB;

class Saver {

C.3 Código del nodo pointcloud_transform_saver 69

public:Saver();void main();

private:// Functionsvoid pointcloudOdomCallback(const sensor_msgs::PointCloud2ConstPtr&

pointCloudMsg,const nav_msgs::OdometryConstPtr& odomMsg);

void stopCallback(const ros::TimerEvent& timerEvent);void rateCallback(const ros::TimerEvent& timerEvent);void savePointCloudToFile(const PCLPointCloudXYZRGB &pclCloud, const std::

string &fileName);void saveRelativeTransformationToFile(const PM::TransformationParameters &

transformation,const std::string &fileName);

void saveTransformationToFile(const PM::TransformationParameters &transformation,

const std::string &fileName);void saveTimeStamps(const ros::Time &cloud_stamp, const ros::Time &tf_stamp,

const std::string &fileName);// Variablesunsigned int _numFilesSaved;std::string _base_link_frame;std::string _camera_frame;std::string _working_directory;PM::TransformationParameters _Tbase_link_camera;PM::TransformationParameters _Tcamera_base_link;PM::TransformationParameters _Todom_base_link_current;PM::TransformationParameters _Todom_base_link_previous;double _captureRate;double _startTime;double _stopTime;std::string _fileBaseName;boost::mutex _sharedAccessVariablesMutex;bool _fSaveToFiles;bool _binaryMode;bool _save_base_link_camera_tf;

};

Saver::Saver():_numFilesSaved(0),_Tbase_link_camera(PM::TransformationParameters::Identity(4,4)),_Tcamera_base_link(PM::TransformationParameters::Identity(4,4)),_Todom_base_link_current(PM::TransformationParameters::Identity(4,4)),_Todom_base_link_previous(PM::TransformationParameters::Identity(4,4)),_fSaveToFiles(false),_binaryMode(true){}

// One way of doing the callbackvoid Saver::pointcloudOdomCallback(const sensor_msgs::PointCloud2ConstPtr&

pointCloudMsg,const nav_msgs::OdometryConstPtr& odomMsg)

{bool fSave = false;


PCLPointCloudXYZRGB pclCloud;PM::TransformationParameters relativeTransformation;//sensor_msgs::PointCloud2Ptr pointCloud2AuxPtr;

_sharedAccessVariablesMutex.lock();fSave = _fSaveToFiles;_fSaveToFiles = false;_sharedAccessVariablesMutex.unlock();

if(fSave){pcl::fromROSMsg(*pointCloudMsg, pclCloud);std::stringstream pcloudFileNameStream;std::stringstream transfFileNameStream;std::stringstream stampFileNameStream;pcloudFileNameStream << _working_directory << "/" << _fileBaseName << "

_cloud" << _numFilesSaved << ".pcd";transfFileNameStream << _working_directory << "/" << _fileBaseName << "

_transf" << _numFilesSaved << ".txt";stampFileNameStream << _working_directory << "/" << _fileBaseName << "

_stamps" << _numFilesSaved << ".txt";

// Save pointcloudthis->savePointCloudToFile(pclCloud, pcloudFileNameStream.str());// Calculate transformation_Todom_base_link_current = PointMatcher_ros::odomMsgToEigenMatrix<

ScalarType>(*odomMsg);relativeTransformation = _Tcamera_base_link * _Todom_base_link_previous.

inverse()* _Todom_base_link_current * _Tbase_link_camera;

_Todom_base_link_previous = _Todom_base_link_current;// Save transformationthis->saveTransformationToFile(relativeTransformation, transfFileNameStream.

str());// Save time stampsthis->saveTimeStamps(pointCloudMsg->header.stamp, odomMsg->header.stamp,

stampFileNameStream.str());// Increment file counter_numFilesSaved++;

}}

void Saver::savePointCloudToFile(const PCLPointCloudXYZRGB &pclCloud, conststd::string &fileName)

{if (pcl::io::savePCDFile(fileName, pclCloud, _binaryMode) != 0){ROS_ERROR_STREAM("Problem saving " << fileName);ROS_ERROR_STREAM("Ending pointcloud and transform savings");ros::shutdown();

}else{ROS_INFO_STREAM("PointCloud " << _numFilesSaved << " saved");

}}


void Saver::saveRelativeTransformationToFile(const PM::TransformationParameters &transformation, const std::string &fileName)

{// To save the transform to a file we need to iterate over the matrix,

accesing each elementstd::stringstream fileContent;for(unsigned int i=0; i<4; i++){for(unsigned int j=0; j<4; j++){fileContent << transformation(i,j) << " ";

}fileContent << std::endl;

}std::ofstream fileStream(fileName.c_str()); // Open the fileif (fileStream.is_open()){fileStream << fileContent.str();fileStream.close();ROS_INFO_STREAM("Transformation " << _numFilesSaved << " saved");

}else{ROS_ERROR_STREAM("Problem saving " << fileName);ROS_ERROR_STREAM("Ending pointcloud and transform savings");ros::shutdown();

}}

void Saver::saveTransformationToFile(const PM::TransformationParameters &transformation, const std::string &fileName)

{std::stringstream fileContent;for(unsigned int i=0; i<4; i++){for(unsigned int j=0; j<4; j++){fileContent << transformation(i,j) << " ";

}fileContent << std::endl;

}std::ofstream fileStream(fileName.c_str());if (fileStream.is_open()){fileStream << fileContent.str();fileStream.close();

}else{ROS_ERROR_STREAM("Problem saving " << fileName);ROS_ERROR_STREAM("Shuting down pointcloud saver");ros::shutdown();

}}

void Saver::saveTimeStamps(const ros::Time &cloud_stamp, const ros::Time &tf_stamp, const std::string &fileName)


{std::stringstream fileContent;fileContent << "cloud " << cloud_stamp << std::endl;fileContent << "tf " << tf_stamp << std::endl;

std::ofstream fileStream(fileName.c_str());if (fileStream.is_open()){fileStream << fileContent.str();fileStream.close();

}else{ROS_ERROR_STREAM("Problem saving " << fileName);ROS_ERROR_STREAM("Ending pointcloud and transform savings");ros::shutdown();

}}

void Saver::rateCallback(const ros::TimerEvent& timerEvent){_sharedAccessVariablesMutex.lock();_fSaveToFiles = true;_sharedAccessVariablesMutex.unlock();

}

void Saver::stopCallback(const ros::TimerEvent& timerEvent){ros::shutdown();

}

void Saver::main(){bool fFirstCloud = true;

ros::NodeHandle nh;ros::NodeHandle privateNh("~");

privateNh.getParam("capture/rate", _captureRate);privateNh.getParam("capture/start_time", _startTime);privateNh.getParam("capture/stop_time", _stopTime);privateNh.getParam("capture/fileBaseName", _fileBaseName);privateNh.getParam("capture/binary_mode", _binaryMode);privateNh.getParam("base_link_frame", _base_link_frame);privateNh.getParam("camera_frame", _camera_frame);privateNh.getParam("working_directory", _working_directory);privateNh.getParam("capture/save_base_link_camera_tf",

_save_base_link_camera_tf);

if(_working_directory.back() == ’/’){_working_directory.pop_back();

}

ros::Duration delay(1);delay.sleep();


{tf2_ros::Buffer tfBuffer;tf2_ros::TransformListener tfListener(tfBuffer);geometry_msgs::TransformStamped auxTbase_link_camera;auxTbase_link_camera = tfBuffer.lookupTransform(_base_link_frame,

_camera_frame, ros::Time(0), ros::Duration(20));Eigen::Affine3d eigenTr = tf2::transformToEigen(auxTbase_link_camera);_Tbase_link_camera = eigenTr.matrix().cast<ScalarType>();}_Tcamera_base_link = _Tbase_link_camera.inverse();

// Save _Tbase_link_camera transformation to filestd::stringstream base_link_camera_transf_filename;base_link_camera_transf_filename << _working_directory << "/Tbase_link_camera.

txt";if(_save_base_link_camera_tf)saveTransformationToFile(_Tbase_link_camera,

base_link_camera_transf_filename.str());

message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSubscriber(nh,"cloud_in", 1);

message_filters::Subscriber<nav_msgs::Odometry> odomSubscriber(nh, "odom", 1);

typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2,

nav_msgs::Odometry> OdomCloudSyncPolicy;

message_filters::Synchronizer<OdomCloudSyncPolicy> sync(OdomCloudSyncPolicy(10), pointCloudSubscriber, odomSubscriber);

ros::Time rosStartTime(_startTime);ros::Time rosStopTime(_stopTime);// Wait to start the capturewhile(ros::Time::now() == ros::Time(0)); // Wait until we receive the first

simulated clock messageros::Duration waitDuration = rosStartTime - ros::Time::now();if (waitDuration.toSec() >= 0)waitDuration.sleep();

sync.registerCallback(boost::bind(&Saver::pointcloudOdomCallback, this, _1,_2));

ros::Duration workingDuration = rosStopTime - rosStartTime;// Timer to signal end of captureros::Timer endTimer = nh.createTimer(workingDuration, boost::bind(&Saver::

stopCallback, this, _1), true);// Timer to limit capture rateros::Timer rateTimer = nh.createTimer(ros::Duration(1/_captureRate), boost::

bind(&Saver::rateCallback, this, _1), false);// Start an infinite loop until interrupt is receivedros::spin();

}

int main(int argc, char** argv){ros::init(argc, argv, "pointcloud_transform_saver");


Saver saver;saver.main();return 0;

}

C.4 Funciones para transformar mensajes de ROS a tipos de datos de libpoint-matcher

Código C.6 message_transformation.h.

#ifndef MESSAGE_TRANSFORMATION_H#define MESSAGE_TRANSFORMATION_H

#include <boost/shared_ptr.hpp>#include "nav_msgs/Odometry.h"#include "sensor_msgs/PointCloud2.h"#include "pointmatcher/PointMatcher.h"#include "ros/ros.h"

template<typename T>nav_msgs::Odometry transformMatrixToOdomMsg(const typename PointMatcher<T>::

TransformationParameters& inTr, const std::string& parent_frame, const std::string& child_frame, const ros::Time& stamp);

namespace PointMatcher_ros{

template<typename T>boost::shared_ptr<typename PointMatcher<T>::DataPoints>

rosMsgToPointMatcherCloudPtr(const sensor_msgs::PointCloud2& rosMsg);

}; //PointMatcher_ros Namespace#endif /* MESSAGE_TRANSFORMATION_H */

Código C.7 message_transformation.cpp.

#include "visual_odom/message_transformation.h"#include "pointmatcher/PointMatcher.h"#include "sensor_msgs/PointCloud2.h"#include "tf2_eigen/tf2_eigen.h"#include "Eigen/Eigen"#include "boost/shared_ptr.hpp"#include "nav_msgs/Odometry.h"

template<typename T>nav_msgs::Odometry transformMatrixToOdomMsg(const typename PointMatcher<T>::

TransformationParameters& inTr, const std::string& parent_frame, const std::string& child_frame, const ros::Time& stamp)

{nav_msgs::Odometry odom;odom.header.stamp = stamp;odom.header.frame_id = parent_frame;odom.child_frame_id = child_frame;

C.4 Funciones para transformar mensajes de ROS a tipos de datos de libpointmatcher 75

odom.pose.pose = tf2::toMsg(Eigen::Affine3d(Eigen::Matrix4d(inTr.templatecast<double>())));

// Fill velocity, TODO: find proper computation from delta poses to twist//odom.child_frame_id = cloudMsgIn.header.frame_id;odom.twist.covariance[0+0*6] = -1;odom.twist.covariance[1+1*6] = -1;odom.twist.covariance[2+2*6] = -1;odom.twist.covariance[3+3*6] = -1;odom.twist.covariance[4+4*6] = -1;odom.twist.covariance[5+5*6] = -1;

return odom;}

templatenav_msgs::Odometry transformMatrixToOdomMsg<float>(const PointMatcher<float>::

TransformationParameters& inTr, const std::string& parent_frame, const std::string& child_frame, const ros::Time& stamp);

templatenav_msgs::Odometry transformMatrixToOdomMsg<double>(const PointMatcher<double

>::TransformationParameters& inTr, const std::string& parent_frame, conststd::string& child_frame, const ros::Time& stamp);

namespace PointMatcher_ros{using namespace std;//! Transform a ROS PointCloud2 message into a libpointmatcher point cloudtemplate<typename T>boost::shared_ptr<typename PointMatcher<T>::DataPoints>

rosMsgToPointMatcherCloudPtr(const sensor_msgs::PointCloud2& rosMsg){typedef PointMatcher<T> PM;typedef typename PM::DataPoints DataPoints;typedef typename DataPoints::Label Label;typedef typename DataPoints::Labels Labels;typedef typename DataPoints::View View;

boost::shared_ptr<DataPoints> cloudPtr;

if (rosMsg.fields.empty()){

cloudPtr.reset(new DataPoints());return cloudPtr;

}

// fill labels// conversions of descriptor fields from pcl// see http://www.ros.org/wiki/pcl/OverviewLabels featLabels;Labels descLabels;vector<bool> isFeature;for(sensor_msgs::PointCloud2::_fields_type::const_iterator it(rosMsg.fields.

begin()); it != rosMsg.fields.end(); ++it){


const string name(it->name);const size_t count(std::max<size_t>(it->count, 1));if (name == "x" || name == "y" || name == "z"){featLabels.push_back(Label(name, count));isFeature.push_back(true);

}else if (name == "rgb" || name == "rgba"){descLabels.push_back(Label("color", (name == "rgba") ? 4 : 3));isFeature.push_back(false);

}else if ((it+1) != rosMsg.fields.end() && it->name == "normal_x" && (it+1)

->name == "normal_y"){if ((it+2) != rosMsg.fields.end() && (it+2)->name == "normal_z"){descLabels.push_back(Label("normals", 3));it += 2;isFeature.push_back(false);isFeature.push_back(false);

}else{descLabels.push_back(Label("normals", 2));it += 1;isFeature.push_back(false);

}isFeature.push_back(false);

}else{descLabels.push_back(Label(name, count));isFeature.push_back(false);

}}featLabels.push_back(Label("pad", 1));assert(isFeature.size() == rosMsg.fields.size());

// create cloudconst unsigned pointCount(rosMsg.width * rosMsg.height);

cloudPtr.reset(new DataPoints(featLabels, descLabels, pointCount));

DataPoints &cloud = *cloudPtr;//DataPoints cloud(featLabels, descLabels, pointCount);cloud.getFeatureViewByName("pad").setConstant(1);

// fill cloud// TODO: support big endian, pass through endian-swapping method just after

the *reinterpret_casttypedef sensor_msgs::PointField PF;size_t fieldId = 0;for(sensor_msgs::PointCloud2::_fields_type::const_iterator it(rosMsg.fields.

begin()); it != rosMsg.fields.end(); ++it, ++fieldId){if (it->name == "rgb" || it->name == "rgba"){

C.4 Funciones para transformar mensajes de ROS a tipos de datos de libpointmatcher 77

// special case for colorsif (((it->datatype != PF::UINT32) && (it->datatype != PF::INT32) && (it

->datatype != PF::FLOAT32)) || (it->count != 1))throw runtime_error((boost::format("Colors in a point cloud must be a single element of

size 32 bits, found %1% elements of type %2%") % it->count %unsigned(it->datatype)).str()

);View view(cloud.getDescriptorViewByName("color"));int ptId(0);for (size_t y(0); y < rosMsg.height; ++y){const uint8_t* dataPtr(&rosMsg.data[0] + rosMsg.row_step*y);for (size_t x(0); x < rosMsg.width; ++x){const uint32_t rgba(*reinterpret_cast<const uint32_t*>(dataPtr + it->

offset));const T colorA(T((rgba >> 24) & 0xff) / 255.);const T colorR(T((rgba >> 16) & 0xff) / 255.);const T colorG(T((rgba >> 8) & 0xff) / 255.);const T colorB(T((rgba >> 0) & 0xff) / 255.);view(0, ptId) = colorR;view(1, ptId) = colorG;view(2, ptId) = colorB;if (view.rows() > 3)view(3, ptId) = colorA;

dataPtr += rosMsg.point_step;ptId += 1;

}}

}else{// get view for editing dataView view(

(it->name == "normal_x") ? cloud.getDescriptorRowViewByName("normals",0) :

((it->name == "normal_y") ? cloud.getDescriptorRowViewByName("normals",1) :

((it->name == "normal_z") ? cloud.getDescriptorRowViewByName("normals",2) :

((isFeature[fieldId]) ? cloud.getFeatureViewByName(it->name) :cloud.getDescriptorViewByName(it->name))))

);// use view to read dataint ptId(0);const size_t count(std::max<size_t>(it->count, 1));for (size_t y(0); y < rosMsg.height; ++y){const uint8_t* dataPtr(&rosMsg.data[0] + rosMsg.row_step*y);for (size_t x(0); x < rosMsg.width; ++x){const uint8_t* fPtr(dataPtr + it->offset);for (unsigned dim(0); dim < count; ++dim){switch (it->datatype){


case PF::INT8: view(dim, ptId) = T(*reinterpret_cast<const int8_t*>(fPtr)); fPtr += 1; break;

case PF::UINT8: view(dim, ptId) = T(*reinterpret_cast<constuint8_t*>(fPtr)); fPtr += 1; break;

case PF::INT16: view(dim, ptId) = T(*reinterpret_cast<constint16_t*>(fPtr)); fPtr += 2; break;


case PF::INT32: view(dim, ptId) = T(*reinterpret_cast<constint32_t*>(fPtr)); fPtr += 4; break;


case PF::FLOAT32: view(dim, ptId) = T(*reinterpret_cast<const float*>(fPtr)); fPtr += 4; break;

case PF::FLOAT64: view(dim, ptId) = T(*reinterpret_cast<constdouble*>(fPtr)); fPtr += 8; break;

default: abort();}

}dataPtr += rosMsg.point_step;ptId += 1;

}}

}}

boost::shared_ptr<typename PM::DataPointsFilter> filter(PM::get().DataPointsFilterRegistrar.create("RemoveNaNDataPointsFilter"));

filter->inPlaceFilter(cloud);return cloudPtr;

}

templateboost::shared_ptr<PointMatcher<float>::DataPoints>

rosMsgToPointMatcherCloudPtr<float>(const sensor_msgs::PointCloud2& rosMsg);

templateboost::shared_ptr<PointMatcher<double>::DataPoints>

rosMsgToPointMatcherCloudPtr<double>(const sensor_msgs::PointCloud2&rosMsg);

}; //PointMatcher_ros Namespace

Índice de Figuras

1.1. Cámara Kinect 21.2. Robot SIAR 2

2.1. Estructura de directorios de un paquete de ROS 72.2. Conexión entre múltiples publicadores y múltiples suscriptores 82.3. Establecimiento de conexión entre suscriptor y publicador 92.4. Ejemplos de nubes de puntos correspondientes a un apartamento y una escalera 122.5. Ejemplo de mensaje PointCloud2 132.6. Diagrama de flujo general del algoritmo ICP 152.7. Estructura de almacenamiento de las nubes de puntos dentro de PointMatcher::DataPoints 162.8. Visualización de las correspondencias encontradas entre dos nubes de puntos 19

3.1. Sistemas de referencia del robot SIAR 213.2. Ilustración de las diferencias en la forma de cálculo de la estimación de la transformación inicial del

algoritmo ICP 253.3. Diagrama de flujo general del nodo 293.4. Diagrama de flujo del callback, ejecutado en el hilo principal 293.5. Diagrama de flujo simplificado del hilo de procesamiento 303.6. Diagrama conceptual que muestra la comunicación entro los hilos 303.7. Disposición de los archivos correspondientes a cada experimento 363.8. Grafo de nodos correspondiente a la ejecución del nodo de odometría visual, y los nodos auxiliares.

Las elipses representan los nodos y los rectángulos los tópicos mediante los que se comunican 37

4.1. Amplia divergencia entre la odometría visual y la propia del robot 404.2. Visualización secuencial de las nubes de puntos capturadas para su análisis 44

79

Índice de Tablas

2.1. Fases del algoritmo ICP, junto con la variable del fichero YAML que tiene asociada 17

3.1. Variables junto a las correspondientes transformaciones que almacenan 27

81

Índice de Códigos

2.1. Definición de un mensaje de ejemplo 102.2. Definición de un mensaje de ejemplo 112.3. Salida del comando rosmsg show sensor_msgs/PointCloud2, con comentarios adicionales 122.4. Ejemplo de archivo de configuración ICPConfig.yaml 17

3.1. Inicialización de transformaciones al recibir la primera nube de puntos 273.2. Variables para el almacenamiento de las nubes de puntos 273.3. visual_odom.main 283.4. Contenido del archivo pointcloudxyzrgb_generation.launch 313.5. Contenido del archivo pointcloudxyz_generation.launch 323.6. Contenido del archivo visual_odom_test_odometry.launch 333.7. Contenido del archivo visual_odom_test_tf.launch 343.8. Ejemplo de archivo de configuración ICPConfig.yaml 353.9. Ejemplo de archivo de configuración visual_odom.config 36

4.1. Contenido del archivo ICPConfig.yaml 394.2. Contenido del archivo pointcloud_transform_saver.launch 414.3. Contenido del archivo pointcloud_transform_saver.sh 424.4. Contenido del archivo view_captured_clouds.sh, el cuál es un shell script para automatizar

la visualización de las nubes de puntos 434.5. Contenido del archivo format_conversion.sh, el cuál realiza la conversión de formato de un

archivo pcd a csv 44

A.1. Creación del espacio de trabajo y descarga del código 49A.2. Compilación del paquete ethzasl_icp_mapping 50

C.1. visual_odom_main.cpp 57C.2. visual_odom.h 57C.3. visual_odom.cpp 59C.4. tf_from_odom.cpp 66C.5. pointcloud_transform_saver.cpp 68C.6. message_transformation.h 74C.7. message_transformation.cpp 74

83

Bibliografía

[] SIAR - Sewer Inspection Autonomous Robot. IDmind. . url: http://siar.idmind.pt/about/. [Consulta: septiembre ].

[] Karel Capek. RUR; La fábrica de absoluto. Barcelona: Círculo de Lectores, . isbn: ---X.[] 1961: Installation of the First Industrial Robot. World Information Institute. . url: http://world-

information . org / wio / infostructure / 100437611663 / 100438659325 / ?ic= 100446326145. [Consulta: septiembre ].

[] Harsha Kikkeri. ethzasl_icp_mapping. ROSWiki, OSRF. mayo .url: http://wiki.ros.org/ethzasl_icp_mapping. [Consulta: septiembre .

[] Tutoriales sobre libpointmatcher. ETHZ-ASL. url: https://libpointmatcher.readthedocs.io/. [Consulta: septiembre ].

[] Repositorio del código de libpointmatcher. url: https : / / github . com / ethz - asl / libpointmatcher.[Consulta: mayo ].

[] François Pomerleau. Libpointmatcher ROS Wiki. OSRF. marzo . url: http : / / wiki . ros . org /libpointmatcher. [Consulta: julio ].

[] Stéphane Magnenat. libpointmatcher_ros. OSRF. noviembre . url: http://wiki.ros.org/libpointmatcher_ros. [Consulta: julio ].

[] Sourcing from multiple workspaces. ROS Answers, OSRF. marzo . url: http://answers.ros.org/question/205976/sourcing-from-multiple-workspaces/. [Consulta: junio ].

[] Chaining catkin workspaces. ROS Wiki, OSRF. Actualizado: // ::. url: http://wiki.ros.org/catkin/Tutorials/workspace_overlaying#Chaining_catkin_workspaces. [Consulta: junio ].

[] Wim Meeussen. Coordinate Frames for Mobile Platforms. ROS REPS, OSRF. //. url: http ://www.ros.org/reps/rep-0105.html. [Consulta: junio ].

[] F. Pomerleau y col. «Comparing ICP variants on real-world data sets». En: Autonomous Robots .(abr. de ), págs. -.

[] Colin Studholme, Derek LGHill y David J Hawkes. «Automated D Registration of TruncatedMR andCT Images of the Head.» En: BMVC. Vol. . , págs. -.

[] Paul J Besl, Neil D McKay y col. «A method for registration of -D shapes». En: IEEE Transactions onpattern analysis and machine intelligence . (), págs. -.

[] Feng Lu y col. «Robot pose estimation in unknown environments by matching d range scans». En:Computer Vision and Pattern Recognition, 1994. Proceedings CVPR’94., 1994 IEEE Computer SocietyConference on. IEEE. , págs. -.

[] David W Eggert, Adele Lorusso y Robert B Fisher. «Estimating -D rigid body transformations: acomparison of four major algorithms». En:Machine vision and applications . (), págs. -.

[] Kok-Lim Low. «Linear least-squares optimization for point-to-plane icp surface registration». En: Cha-pel Hill, University of North Carolina ().

[] ROSConcepts. ROSWiki, OSRF. junio . url: http://http://wiki.ros.org/ROS/Concepts. [Consulta: julio ].

85

http://siar.idmind.pt/about/

http://world-information.org/wio/infostructure/100437611663/100438659325/?ic=100446326145

http://world-information.org/wio/infostructure/100437611663/100438659325/?ic=100446326145

http://wiki.ros.org/ethzasl_icp_mapping

http://wiki.ros.org/ethzasl_icp_mapping

https://libpointmatcher.readthedocs.io/

https://github.com/ethz-asl/libpointmatcher

http://wiki.ros.org/libpointmatcher

http://wiki.ros.org/libpointmatcher

http://wiki.ros.org/libpointmatcher_ros

http://wiki.ros.org/libpointmatcher_ros

http://answers.ros.org/question/205976/sourcing-from-multiple-workspaces/

http://answers.ros.org/question/205976/sourcing-from-multiple-workspaces/

http://wiki.ros.org/catkin/Tutorials/workspace_overlaying#Chaining_catkin_workspaces

http://wiki.ros.org/catkin/Tutorials/workspace_overlaying#Chaining_catkin_workspaces

http://www.ros.org/reps/rep-0105.html

http://www.ros.org/reps/rep-0105.html

http://http://wiki.ros.org/ROS/Concepts

86 Bibliografía

[] ROS Nodes. ROS Wiki, OSRF. febrero . url: http://http://wiki.ros.org/Nodes. [Consulta: julio].

[] ROS Introduction. ROS Wiki, OSRF. mayo . url: http://http://wiki.ros.org/ROS/Introduction.[Consulta: julio ].

[] François Pomerleau. ASL Datasets: Stairs. ETH, Zurich, Suiza. agosto . url: http://projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:stairs:home. [Consulta: julio ].

[] François Pomerleau. ASL Datasets: Apartment. ETH, Zurich, Suiza. diciembre . url: http : / /projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:apartment:home. [Consulta: julio ].

[] Catkin package.xml. ROS Wiki, OSRF. julio . url: http : / /wiki . ros .org/ catkin/ package .xml.[Consulta: julio ].

[] Catkin CMakeLists.txt. ROS Wiki, OSRF. febrero . url: http://wiki.ros.org/catkin/package.xml.[Consulta: julio ].

[] Transformations Overview. ROS Wiki, OSRF. febrero . url: http://wiki.ros.org/tf/Overview/Transformations. [Consulta: agosto ].

[] Introduction to tf. ROS Wiki, OSRF. junio . url: http://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf . [Consulta: agosto ].

http://http://wiki.ros.org/Nodes

http://http://wiki.ros.org/ROS/Introduction

http://projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:stairs:home

http://projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:stairs:home

http://projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:apartment:home

http://projects.asl.ethz.ch/datasets/doku.php?id=laserregistration:apartment:home

http://wiki.ros.org/catkin/package.xml

http://wiki.ros.org/catkin/package.xml

http://wiki.ros.org/tf/Overview/Transformations

http://wiki.ros.org/tf/Overview/Transformations

http://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf

http://wiki.ros.org/tf/Tutorials/Introduction%20to%20tf

Índice alfabético

C++ , árbol de directorios, árbol de nombrado, CMakeLists.txt, CMake, bag, , catkin, , libpointmatcher, , package.xml, pmicp, rviz, mensajes,

algoritmo, , , algoritmo ICP, , algoritmos, alineación, array,

búsqueda de correspondencias, big-endian, buses virtuales,

cámara RGBD, cámaras, cámaras de vídeo, cámaras estereoscópicas, código fuente, callback, características, , clase, cliente, compilación, conexión punto a punto, con�guración, constantes, control remoto, coordenadas, correspondencias,

Debian, de�nición de mensajes, , dependencias, , dependencias del sistema, deriva, descriptores, , , diagrama de �ujo, dimensiones,

ecuación del error, ejecutable, ,

encoders, , , entorno de desarrollo, error de alineación, escáneres D, espacio de nombres, espacio de trabajo, espacio tridimensional, estimación, , estimación inicial, evolución temporal, experimentos, , extensión,

�cheros de cabecera, �cheros de con�guración, �ltrado, �ltros, framework,

grafo de computación,

hilo, ,

inspección, instancia, IP, iteraciones,

latencia, librería, , librerías, little-endian,

macros, mapa tridimensional, master, matriz de rotación, matriz de transformación, matriz identidad, mensajes, , , , metapaquete, Minimización del error, mutex,

navegación, nodo, nodos, , , nombre global, nombre relativo, nombres privados, nube de lectura,

nube de puntos, , nube de puntos de lectura, nube de referencia, nubes de puntos, , , , , ,

odometría, , –, odometría visual, , , , ordenador de a bordo,

pérdida de paquetes, paquetes, , paquetes Debian, parámetros, patrón de luz infrarroja, petición, plano tangente, pre�jo, procesamiento de imágenes, profundidad, proveedor, publicador, publicadores, puerto,

red, red de alcantarillado, referencia, registro, relativas, remapeo, , respuesta, robots, rotaciones, ruta, ruta de acceso, rutas absolutas,

señal GPS, secuencia de bytes, sensor infrarrojo, sensores, servicios, , , , shell script, sistema de referencia, Sistemas de coordenadas, sistemas de referencia, sockets, solicitud de conexión, subdirectorio, , suma MD, suscriptor,

87

88 Índice alfabético

suscriptores,

tópicos, , , , , tipo de mensaje, tipo primitivo, tolerancia a fallos, transformación, , ,

transformación afín, transformaciones, traslaciones,

Ubuntu, Ubuntu . LTS,

variables, vector de traslación, vector ortonormal,

xmlrpc,

Glosario

GPS Global Positioning System.

ICP Iterative Closest Point. III, V, , –, –,

IMU Inertial Measurement Unit.

LIDAR Light Detection and Ranging. , ,

RADAR Radio Detection and Ranging.

ROS Robot Operating System. III, V, , , , , , , ,

SIAR Sewer Inspection Autonomous Robot. ,

SLAM Simultaneous Location and Mapping.

TCP Transmission Control Protocol.

TOF Time of Flight.

UDP User Datagram Protocol.

YAML YAML Ain’t Markup Language. , ,

89

hfwr )lq gh &duuhud ,qjhqlhutd gh...

Documents