Equation Chapter 1 Section 1

Trabajo Fin de Maacutester

Maacutester Universitario en Roboacutetica Automaacutetica y

Telemaacutetica

Sistema odomeacutetrico por visioacuten artificial para UAV

Autor Luis Felipe Alonso Perdomo

Tutores Fernando Caballero Beniacutetez Jesuacutes Capitaacuten Fernandez

Departamento de Ingenieriacutea de Sistemas y

Automaacutetica

Escuela Teacutecnica Superior de Ingenieriacutea

Universidad de Sevilla

Sevilla 2017

2

Trabajo Fin de Maacutester

Maacutester Universitario en Roboacutetica Automaacutetica y Telemaacutetica

Sistema odomeacutetrico por visioacuten artificial para UAV

Autor

Luis Felipe Alonso Perdomo

Tutor

Fernando Caballero Beniacutetez

Jesuacutes Capitaacuten Fernandez

Departamento de Ingenieriacutea de Sistemas y Automaacutetica

Escuela Teacutecnica Superior de Ingenieriacutea

Universidad de Sevilla

Sevilla 2018

4

Proyecto Fin de Maacutester Sistema odomeacutetrico por visioacuten artificial para UAV

Autor Luis Felipe Alonso Perdomo

Tutor Fernando Caballero Beniacutetez

Jesuacutes Capitaacuten Fernandez

El tribunal nombrado para juzgar el Proyecto arriba indicado compuesto por los siguientes miembros

Presidente

Vocales

Secretario

Acuerdan otorgarle la calificacioacuten de

Sevilla 2018

El Secretario del Tribunal

A mi familia

A mis amigos

A mis maestros

6

Agradecimientos

El poder finalizar este trabajo implica cerrar una etapa de mi vida y empezar muchas nuevas etapas tanto a nivel

profesional como peronal es por ello que me gustariacutea agradecer a todas aquellas personas que me han ayudado

y apoyado durante este proceso

A mi familia a mi madre padre y hermanas que nunca han desistido en su deseo de verme triunfar en todo lo

que me proponga

A mi novia que no me abandona bajo ninguna circunstancia

A mis amigos que compartieron conmigo estos antildeos de maacutester que tanto me han ensentildeado y me han ayudado

para llegar hasta aquiacute

iexclMuchas gracias a todos

Luis Felipe Alonso Perdomo

Sevilla 2017

8

Iacutendice

Agradecimientos 6

Iacutendice 8

Iacutendice de Figuras 10

1 Introduccioacuten 11 11 Objetivos 13 12 Software 13

121 ROS (Robot Operative System) 14 13 Hardware 15

131 Kinect 16

2 BRISK Deteccioacuten y seguimientos de Keypoints 19 21 Workspace en ROS Hydro 19 22 Paquetes en ROS Hydro 21

221 Paquete ldquopublisherrdquo 21 222 Nodo publicador 22 223 Publisher packagexml 23 224 Publisher CMakeListsxml 24 225 Ejecucioacuten del nodo 25

23 Problema 2D 25 231 Subscriptorcpp 25 232 Funcioacuten ldquoMainrdquo 26 233 Subrutina ldquoCallbackrdquo 26

24 BRISK 29 241 Definicioacuten 29 242 Subrutina ldquouseBriskDetectorrdquo 30 243 Implementacioacuten BRISK 30

25 Filtrado de los Matches 33 251 Implementacioacuten subrutina RobustMach 34

3 Estimacioacuten movimiento UAV 38 31 Extraccioacuten puntos de las imaacutegenes de profundidad 39 32 Correspondencia puntos 2D3D 40 33 Resolucioacuten del problema PnP 41 34 Concatenacioacuten de matrices 41

4 Conclusioacuten y trabajo futuro 44

5 Bibliografiacutea 48

6 Anexos 49 61 Anexo A Publisher_S2launch 49 62 Anexo B Subscribercpp 49

10

IacuteNDICE DE FIGURAS

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II 11

Ilustracioacuten 2 Dron de reconocimiento Fulmar 11

Ilustracioacuten 3 MQ-1 Predator 12

Ilustracioacuten 4 Drones de investigaciones y desarrollo 12

Ilustracioacuten 5 Dron comercial 12

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS 14

Ilustracioacuten 7 Nivel graacutefico computacional de ROS 15

Ilustracioacuten 8 Frontal de Kinect 16

Ilustracioacuten 9 Esquema interno de Kinect 17

Ilustracioacuten 10 Relacioacuten planos 18

Ilustracioacuten 11 Terminal Terminator 19

Ilustracioacuten 12 Maestro arrancado 20

Ilustracioacuten 13 Frame Gray 28

Ilustracioacuten 14 Depth frame 28

Ilustracioacuten 15 Piramide de capas de octavas 31

Ilustracioacuten 16 Matches sin filtrar 33

Ilustracioacuten 17 Estereovisioacuten 34

Ilustracioacuten 18 Matches filtrados 37

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo 38

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen 39

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV 45

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x 46

1 INTRODUCCIOacuteN

en el presente documento se detalla el desarrollo del coacutedigo necsario para programar un sistema

odomeacutetrico por imaacutegenes para UAV Para el desarrollo de este sistema se ha hecho uso del sistema

operativo ROS (Robot Operative System) como entorno de desarrollo el dispositivo Kinect para obtener

informacioacuten del entorno y el lenguaje de programacioacuten C++

Un UAV (Unmaned Aerial Vehicle) tambieacuten conocido como dron es una aeronave no tripulada La aviacioacuten

no tripulada tuvo sus comienzos en los modelos de pioneros de la aviacioacuten como el franceacutes Feacutelix du Temple que

en 1857 patentoacute los disentildeos de una maacutequina aeacuterea que incorporaba un tren de aterrizaje plegable o el ingleacutes

John Stringfellow que en 1948 construyoacute varios planeadores no tripulados En la actualidad debido al desarrollo

tecnoloacutegico se han conseguido desarrollar drones con mayor autonomiacutea y sistemas que son capaces de seguir

la posicioacuten de nuestro dron como el GPS Estos y otros avances nos han permitido desarrollar aeronaves con

diversas aplicaciones como

Objetivos y sentildeuelos Proporciona a la artilleriacutea de aeacuterea y de tierra un objetivo que simula una aeronave

enemiga o un misil como el modelo SCRAB II que se puede ver en la ldquoilustracioacuten 1rdquo

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II

Reconocimiento Proporciona informacioacuten sobre el entorno como el modelo Fulmar que se muestra en

la ldquoIlustracioacuten 2rdquo

Ilustracioacuten 2 Dron de reconocimiento Fulmar

E

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

2

Trabajo Fin de Maacutester

Maacutester Universitario en Roboacutetica Automaacutetica y Telemaacutetica

Sistema odomeacutetrico por visioacuten artificial para UAV

Autor

Luis Felipe Alonso Perdomo

Tutor

Fernando Caballero Beniacutetez

Jesuacutes Capitaacuten Fernandez

Departamento de Ingenieriacutea de Sistemas y Automaacutetica

Escuela Teacutecnica Superior de Ingenieriacutea

Universidad de Sevilla

Sevilla 2018

4

Proyecto Fin de Maacutester Sistema odomeacutetrico por visioacuten artificial para UAV

Autor Luis Felipe Alonso Perdomo

Tutor Fernando Caballero Beniacutetez

Jesuacutes Capitaacuten Fernandez

El tribunal nombrado para juzgar el Proyecto arriba indicado compuesto por los siguientes miembros

Presidente

Vocales

Secretario

Acuerdan otorgarle la calificacioacuten de

Sevilla 2018

El Secretario del Tribunal

A mi familia

A mis amigos

A mis maestros

6

Agradecimientos

El poder finalizar este trabajo implica cerrar una etapa de mi vida y empezar muchas nuevas etapas tanto a nivel

profesional como peronal es por ello que me gustariacutea agradecer a todas aquellas personas que me han ayudado

y apoyado durante este proceso

A mi familia a mi madre padre y hermanas que nunca han desistido en su deseo de verme triunfar en todo lo

que me proponga

A mi novia que no me abandona bajo ninguna circunstancia

A mis amigos que compartieron conmigo estos antildeos de maacutester que tanto me han ensentildeado y me han ayudado

para llegar hasta aquiacute

iexclMuchas gracias a todos

Luis Felipe Alonso Perdomo

Sevilla 2017

8

Iacutendice

Agradecimientos 6

Iacutendice 8

Iacutendice de Figuras 10

1 Introduccioacuten 11 11 Objetivos 13 12 Software 13

121 ROS (Robot Operative System) 14 13 Hardware 15

131 Kinect 16

2 BRISK Deteccioacuten y seguimientos de Keypoints 19 21 Workspace en ROS Hydro 19 22 Paquetes en ROS Hydro 21

221 Paquete ldquopublisherrdquo 21 222 Nodo publicador 22 223 Publisher packagexml 23 224 Publisher CMakeListsxml 24 225 Ejecucioacuten del nodo 25

23 Problema 2D 25 231 Subscriptorcpp 25 232 Funcioacuten ldquoMainrdquo 26 233 Subrutina ldquoCallbackrdquo 26

24 BRISK 29 241 Definicioacuten 29 242 Subrutina ldquouseBriskDetectorrdquo 30 243 Implementacioacuten BRISK 30

25 Filtrado de los Matches 33 251 Implementacioacuten subrutina RobustMach 34

3 Estimacioacuten movimiento UAV 38 31 Extraccioacuten puntos de las imaacutegenes de profundidad 39 32 Correspondencia puntos 2D3D 40 33 Resolucioacuten del problema PnP 41 34 Concatenacioacuten de matrices 41

4 Conclusioacuten y trabajo futuro 44

5 Bibliografiacutea 48

6 Anexos 49 61 Anexo A Publisher_S2launch 49 62 Anexo B Subscribercpp 49

10

IacuteNDICE DE FIGURAS

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II 11

Ilustracioacuten 2 Dron de reconocimiento Fulmar 11

Ilustracioacuten 3 MQ-1 Predator 12

Ilustracioacuten 4 Drones de investigaciones y desarrollo 12

Ilustracioacuten 5 Dron comercial 12

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS 14

Ilustracioacuten 7 Nivel graacutefico computacional de ROS 15

Ilustracioacuten 8 Frontal de Kinect 16

Ilustracioacuten 9 Esquema interno de Kinect 17

Ilustracioacuten 10 Relacioacuten planos 18

Ilustracioacuten 11 Terminal Terminator 19

Ilustracioacuten 12 Maestro arrancado 20

Ilustracioacuten 13 Frame Gray 28

Ilustracioacuten 14 Depth frame 28

Ilustracioacuten 15 Piramide de capas de octavas 31

Ilustracioacuten 16 Matches sin filtrar 33

Ilustracioacuten 17 Estereovisioacuten 34

Ilustracioacuten 18 Matches filtrados 37

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo 38

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen 39

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV 45

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x 46

1 INTRODUCCIOacuteN

en el presente documento se detalla el desarrollo del coacutedigo necsario para programar un sistema

odomeacutetrico por imaacutegenes para UAV Para el desarrollo de este sistema se ha hecho uso del sistema

operativo ROS (Robot Operative System) como entorno de desarrollo el dispositivo Kinect para obtener

informacioacuten del entorno y el lenguaje de programacioacuten C++

Un UAV (Unmaned Aerial Vehicle) tambieacuten conocido como dron es una aeronave no tripulada La aviacioacuten

no tripulada tuvo sus comienzos en los modelos de pioneros de la aviacioacuten como el franceacutes Feacutelix du Temple que

en 1857 patentoacute los disentildeos de una maacutequina aeacuterea que incorporaba un tren de aterrizaje plegable o el ingleacutes

John Stringfellow que en 1948 construyoacute varios planeadores no tripulados En la actualidad debido al desarrollo

tecnoloacutegico se han conseguido desarrollar drones con mayor autonomiacutea y sistemas que son capaces de seguir

la posicioacuten de nuestro dron como el GPS Estos y otros avances nos han permitido desarrollar aeronaves con

diversas aplicaciones como

Objetivos y sentildeuelos Proporciona a la artilleriacutea de aeacuterea y de tierra un objetivo que simula una aeronave

enemiga o un misil como el modelo SCRAB II que se puede ver en la ldquoilustracioacuten 1rdquo

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II

Reconocimiento Proporciona informacioacuten sobre el entorno como el modelo Fulmar que se muestra en

la ldquoIlustracioacuten 2rdquo

Ilustracioacuten 2 Dron de reconocimiento Fulmar

E

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

Trabajo Fin de Maacutester

Maacutester Universitario en Roboacutetica Automaacutetica y Telemaacutetica

Sistema odomeacutetrico por visioacuten artificial para UAV

Autor

Luis Felipe Alonso Perdomo

Tutor

Fernando Caballero Beniacutetez

Jesuacutes Capitaacuten Fernandez

Departamento de Ingenieriacutea de Sistemas y Automaacutetica

Escuela Teacutecnica Superior de Ingenieriacutea

Universidad de Sevilla

Sevilla 2018

4

Proyecto Fin de Maacutester Sistema odomeacutetrico por visioacuten artificial para UAV

Autor Luis Felipe Alonso Perdomo

Tutor Fernando Caballero Beniacutetez

Jesuacutes Capitaacuten Fernandez

El tribunal nombrado para juzgar el Proyecto arriba indicado compuesto por los siguientes miembros

Presidente

Vocales

Secretario

Acuerdan otorgarle la calificacioacuten de

Sevilla 2018

El Secretario del Tribunal

A mi familia

A mis amigos

A mis maestros

6

Agradecimientos

El poder finalizar este trabajo implica cerrar una etapa de mi vida y empezar muchas nuevas etapas tanto a nivel

profesional como peronal es por ello que me gustariacutea agradecer a todas aquellas personas que me han ayudado

y apoyado durante este proceso

A mi familia a mi madre padre y hermanas que nunca han desistido en su deseo de verme triunfar en todo lo

que me proponga

A mi novia que no me abandona bajo ninguna circunstancia

A mis amigos que compartieron conmigo estos antildeos de maacutester que tanto me han ensentildeado y me han ayudado

para llegar hasta aquiacute

iexclMuchas gracias a todos

Luis Felipe Alonso Perdomo

Sevilla 2017

8

Iacutendice

Agradecimientos 6

Iacutendice 8

Iacutendice de Figuras 10

1 Introduccioacuten 11 11 Objetivos 13 12 Software 13

121 ROS (Robot Operative System) 14 13 Hardware 15

131 Kinect 16

2 BRISK Deteccioacuten y seguimientos de Keypoints 19 21 Workspace en ROS Hydro 19 22 Paquetes en ROS Hydro 21

221 Paquete ldquopublisherrdquo 21 222 Nodo publicador 22 223 Publisher packagexml 23 224 Publisher CMakeListsxml 24 225 Ejecucioacuten del nodo 25

23 Problema 2D 25 231 Subscriptorcpp 25 232 Funcioacuten ldquoMainrdquo 26 233 Subrutina ldquoCallbackrdquo 26

24 BRISK 29 241 Definicioacuten 29 242 Subrutina ldquouseBriskDetectorrdquo 30 243 Implementacioacuten BRISK 30

25 Filtrado de los Matches 33 251 Implementacioacuten subrutina RobustMach 34

3 Estimacioacuten movimiento UAV 38 31 Extraccioacuten puntos de las imaacutegenes de profundidad 39 32 Correspondencia puntos 2D3D 40 33 Resolucioacuten del problema PnP 41 34 Concatenacioacuten de matrices 41

4 Conclusioacuten y trabajo futuro 44

5 Bibliografiacutea 48

6 Anexos 49 61 Anexo A Publisher_S2launch 49 62 Anexo B Subscribercpp 49

10

IacuteNDICE DE FIGURAS

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II 11

Ilustracioacuten 2 Dron de reconocimiento Fulmar 11

Ilustracioacuten 3 MQ-1 Predator 12

Ilustracioacuten 4 Drones de investigaciones y desarrollo 12

Ilustracioacuten 5 Dron comercial 12

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS 14

Ilustracioacuten 7 Nivel graacutefico computacional de ROS 15

Ilustracioacuten 8 Frontal de Kinect 16

Ilustracioacuten 9 Esquema interno de Kinect 17

Ilustracioacuten 10 Relacioacuten planos 18

Ilustracioacuten 11 Terminal Terminator 19

Ilustracioacuten 12 Maestro arrancado 20

Ilustracioacuten 13 Frame Gray 28

Ilustracioacuten 14 Depth frame 28

Ilustracioacuten 15 Piramide de capas de octavas 31

Ilustracioacuten 16 Matches sin filtrar 33

Ilustracioacuten 17 Estereovisioacuten 34

Ilustracioacuten 18 Matches filtrados 37

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo 38

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen 39

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV 45

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x 46

1 INTRODUCCIOacuteN

en el presente documento se detalla el desarrollo del coacutedigo necsario para programar un sistema

odomeacutetrico por imaacutegenes para UAV Para el desarrollo de este sistema se ha hecho uso del sistema

operativo ROS (Robot Operative System) como entorno de desarrollo el dispositivo Kinect para obtener

informacioacuten del entorno y el lenguaje de programacioacuten C++

Un UAV (Unmaned Aerial Vehicle) tambieacuten conocido como dron es una aeronave no tripulada La aviacioacuten

no tripulada tuvo sus comienzos en los modelos de pioneros de la aviacioacuten como el franceacutes Feacutelix du Temple que

en 1857 patentoacute los disentildeos de una maacutequina aeacuterea que incorporaba un tren de aterrizaje plegable o el ingleacutes

John Stringfellow que en 1948 construyoacute varios planeadores no tripulados En la actualidad debido al desarrollo

tecnoloacutegico se han conseguido desarrollar drones con mayor autonomiacutea y sistemas que son capaces de seguir

la posicioacuten de nuestro dron como el GPS Estos y otros avances nos han permitido desarrollar aeronaves con

diversas aplicaciones como

Objetivos y sentildeuelos Proporciona a la artilleriacutea de aeacuterea y de tierra un objetivo que simula una aeronave

enemiga o un misil como el modelo SCRAB II que se puede ver en la ldquoilustracioacuten 1rdquo

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II

Reconocimiento Proporciona informacioacuten sobre el entorno como el modelo Fulmar que se muestra en

la ldquoIlustracioacuten 2rdquo

Ilustracioacuten 2 Dron de reconocimiento Fulmar

E

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

4

Proyecto Fin de Maacutester Sistema odomeacutetrico por visioacuten artificial para UAV

Autor Luis Felipe Alonso Perdomo

Tutor Fernando Caballero Beniacutetez

Jesuacutes Capitaacuten Fernandez

El tribunal nombrado para juzgar el Proyecto arriba indicado compuesto por los siguientes miembros

Presidente

Vocales

Secretario

Acuerdan otorgarle la calificacioacuten de

Sevilla 2018

El Secretario del Tribunal

A mi familia

A mis amigos

A mis maestros

6

Agradecimientos

El poder finalizar este trabajo implica cerrar una etapa de mi vida y empezar muchas nuevas etapas tanto a nivel

profesional como peronal es por ello que me gustariacutea agradecer a todas aquellas personas que me han ayudado

y apoyado durante este proceso

A mi familia a mi madre padre y hermanas que nunca han desistido en su deseo de verme triunfar en todo lo

que me proponga

A mi novia que no me abandona bajo ninguna circunstancia

A mis amigos que compartieron conmigo estos antildeos de maacutester que tanto me han ensentildeado y me han ayudado

para llegar hasta aquiacute

iexclMuchas gracias a todos

Luis Felipe Alonso Perdomo

Sevilla 2017

8

Iacutendice

Agradecimientos 6

Iacutendice 8

Iacutendice de Figuras 10

1 Introduccioacuten 11 11 Objetivos 13 12 Software 13

121 ROS (Robot Operative System) 14 13 Hardware 15

131 Kinect 16

2 BRISK Deteccioacuten y seguimientos de Keypoints 19 21 Workspace en ROS Hydro 19 22 Paquetes en ROS Hydro 21

221 Paquete ldquopublisherrdquo 21 222 Nodo publicador 22 223 Publisher packagexml 23 224 Publisher CMakeListsxml 24 225 Ejecucioacuten del nodo 25

23 Problema 2D 25 231 Subscriptorcpp 25 232 Funcioacuten ldquoMainrdquo 26 233 Subrutina ldquoCallbackrdquo 26

24 BRISK 29 241 Definicioacuten 29 242 Subrutina ldquouseBriskDetectorrdquo 30 243 Implementacioacuten BRISK 30

25 Filtrado de los Matches 33 251 Implementacioacuten subrutina RobustMach 34

3 Estimacioacuten movimiento UAV 38 31 Extraccioacuten puntos de las imaacutegenes de profundidad 39 32 Correspondencia puntos 2D3D 40 33 Resolucioacuten del problema PnP 41 34 Concatenacioacuten de matrices 41

4 Conclusioacuten y trabajo futuro 44

5 Bibliografiacutea 48

6 Anexos 49 61 Anexo A Publisher_S2launch 49 62 Anexo B Subscribercpp 49

10

IacuteNDICE DE FIGURAS

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II 11

Ilustracioacuten 2 Dron de reconocimiento Fulmar 11

Ilustracioacuten 3 MQ-1 Predator 12

Ilustracioacuten 4 Drones de investigaciones y desarrollo 12

Ilustracioacuten 5 Dron comercial 12

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS 14

Ilustracioacuten 7 Nivel graacutefico computacional de ROS 15

Ilustracioacuten 8 Frontal de Kinect 16

Ilustracioacuten 9 Esquema interno de Kinect 17

Ilustracioacuten 10 Relacioacuten planos 18

Ilustracioacuten 11 Terminal Terminator 19

Ilustracioacuten 12 Maestro arrancado 20

Ilustracioacuten 13 Frame Gray 28

Ilustracioacuten 14 Depth frame 28

Ilustracioacuten 15 Piramide de capas de octavas 31

Ilustracioacuten 16 Matches sin filtrar 33

Ilustracioacuten 17 Estereovisioacuten 34

Ilustracioacuten 18 Matches filtrados 37

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo 38

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen 39

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV 45

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x 46

1 INTRODUCCIOacuteN

en el presente documento se detalla el desarrollo del coacutedigo necsario para programar un sistema

odomeacutetrico por imaacutegenes para UAV Para el desarrollo de este sistema se ha hecho uso del sistema

operativo ROS (Robot Operative System) como entorno de desarrollo el dispositivo Kinect para obtener

informacioacuten del entorno y el lenguaje de programacioacuten C++

Un UAV (Unmaned Aerial Vehicle) tambieacuten conocido como dron es una aeronave no tripulada La aviacioacuten

no tripulada tuvo sus comienzos en los modelos de pioneros de la aviacioacuten como el franceacutes Feacutelix du Temple que

en 1857 patentoacute los disentildeos de una maacutequina aeacuterea que incorporaba un tren de aterrizaje plegable o el ingleacutes

John Stringfellow que en 1948 construyoacute varios planeadores no tripulados En la actualidad debido al desarrollo

tecnoloacutegico se han conseguido desarrollar drones con mayor autonomiacutea y sistemas que son capaces de seguir

la posicioacuten de nuestro dron como el GPS Estos y otros avances nos han permitido desarrollar aeronaves con

diversas aplicaciones como

Objetivos y sentildeuelos Proporciona a la artilleriacutea de aeacuterea y de tierra un objetivo que simula una aeronave

enemiga o un misil como el modelo SCRAB II que se puede ver en la ldquoilustracioacuten 1rdquo

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II

Reconocimiento Proporciona informacioacuten sobre el entorno como el modelo Fulmar que se muestra en

la ldquoIlustracioacuten 2rdquo

Ilustracioacuten 2 Dron de reconocimiento Fulmar

E

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

A mi familia

A mis amigos

A mis maestros

6

Agradecimientos

El poder finalizar este trabajo implica cerrar una etapa de mi vida y empezar muchas nuevas etapas tanto a nivel

profesional como peronal es por ello que me gustariacutea agradecer a todas aquellas personas que me han ayudado

y apoyado durante este proceso

A mi familia a mi madre padre y hermanas que nunca han desistido en su deseo de verme triunfar en todo lo

que me proponga

A mi novia que no me abandona bajo ninguna circunstancia

A mis amigos que compartieron conmigo estos antildeos de maacutester que tanto me han ensentildeado y me han ayudado

para llegar hasta aquiacute

iexclMuchas gracias a todos

Luis Felipe Alonso Perdomo

Sevilla 2017

8

Iacutendice

Agradecimientos 6

Iacutendice 8

Iacutendice de Figuras 10

1 Introduccioacuten 11 11 Objetivos 13 12 Software 13

121 ROS (Robot Operative System) 14 13 Hardware 15

131 Kinect 16

2 BRISK Deteccioacuten y seguimientos de Keypoints 19 21 Workspace en ROS Hydro 19 22 Paquetes en ROS Hydro 21

221 Paquete ldquopublisherrdquo 21 222 Nodo publicador 22 223 Publisher packagexml 23 224 Publisher CMakeListsxml 24 225 Ejecucioacuten del nodo 25

23 Problema 2D 25 231 Subscriptorcpp 25 232 Funcioacuten ldquoMainrdquo 26 233 Subrutina ldquoCallbackrdquo 26

24 BRISK 29 241 Definicioacuten 29 242 Subrutina ldquouseBriskDetectorrdquo 30 243 Implementacioacuten BRISK 30

25 Filtrado de los Matches 33 251 Implementacioacuten subrutina RobustMach 34

3 Estimacioacuten movimiento UAV 38 31 Extraccioacuten puntos de las imaacutegenes de profundidad 39 32 Correspondencia puntos 2D3D 40 33 Resolucioacuten del problema PnP 41 34 Concatenacioacuten de matrices 41

4 Conclusioacuten y trabajo futuro 44

5 Bibliografiacutea 48

6 Anexos 49 61 Anexo A Publisher_S2launch 49 62 Anexo B Subscribercpp 49

10

IacuteNDICE DE FIGURAS

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II 11

Ilustracioacuten 2 Dron de reconocimiento Fulmar 11

Ilustracioacuten 3 MQ-1 Predator 12

Ilustracioacuten 4 Drones de investigaciones y desarrollo 12

Ilustracioacuten 5 Dron comercial 12

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS 14

Ilustracioacuten 7 Nivel graacutefico computacional de ROS 15

Ilustracioacuten 8 Frontal de Kinect 16

Ilustracioacuten 9 Esquema interno de Kinect 17

Ilustracioacuten 10 Relacioacuten planos 18

Ilustracioacuten 11 Terminal Terminator 19

Ilustracioacuten 12 Maestro arrancado 20

Ilustracioacuten 13 Frame Gray 28

Ilustracioacuten 14 Depth frame 28

Ilustracioacuten 15 Piramide de capas de octavas 31

Ilustracioacuten 16 Matches sin filtrar 33

Ilustracioacuten 17 Estereovisioacuten 34

Ilustracioacuten 18 Matches filtrados 37

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo 38

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen 39

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV 45

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x 46

1 INTRODUCCIOacuteN

en el presente documento se detalla el desarrollo del coacutedigo necsario para programar un sistema

odomeacutetrico por imaacutegenes para UAV Para el desarrollo de este sistema se ha hecho uso del sistema

operativo ROS (Robot Operative System) como entorno de desarrollo el dispositivo Kinect para obtener

informacioacuten del entorno y el lenguaje de programacioacuten C++

Un UAV (Unmaned Aerial Vehicle) tambieacuten conocido como dron es una aeronave no tripulada La aviacioacuten

no tripulada tuvo sus comienzos en los modelos de pioneros de la aviacioacuten como el franceacutes Feacutelix du Temple que

en 1857 patentoacute los disentildeos de una maacutequina aeacuterea que incorporaba un tren de aterrizaje plegable o el ingleacutes

John Stringfellow que en 1948 construyoacute varios planeadores no tripulados En la actualidad debido al desarrollo

tecnoloacutegico se han conseguido desarrollar drones con mayor autonomiacutea y sistemas que son capaces de seguir

la posicioacuten de nuestro dron como el GPS Estos y otros avances nos han permitido desarrollar aeronaves con

diversas aplicaciones como

Objetivos y sentildeuelos Proporciona a la artilleriacutea de aeacuterea y de tierra un objetivo que simula una aeronave

enemiga o un misil como el modelo SCRAB II que se puede ver en la ldquoilustracioacuten 1rdquo

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II

Reconocimiento Proporciona informacioacuten sobre el entorno como el modelo Fulmar que se muestra en

la ldquoIlustracioacuten 2rdquo

Ilustracioacuten 2 Dron de reconocimiento Fulmar

E

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

6

Agradecimientos

El poder finalizar este trabajo implica cerrar una etapa de mi vida y empezar muchas nuevas etapas tanto a nivel

profesional como peronal es por ello que me gustariacutea agradecer a todas aquellas personas que me han ayudado

y apoyado durante este proceso

A mi familia a mi madre padre y hermanas que nunca han desistido en su deseo de verme triunfar en todo lo

que me proponga

A mi novia que no me abandona bajo ninguna circunstancia

A mis amigos que compartieron conmigo estos antildeos de maacutester que tanto me han ensentildeado y me han ayudado

para llegar hasta aquiacute

iexclMuchas gracias a todos

Luis Felipe Alonso Perdomo

Sevilla 2017

8

Iacutendice

Agradecimientos 6

Iacutendice 8

Iacutendice de Figuras 10

1 Introduccioacuten 11 11 Objetivos 13 12 Software 13

121 ROS (Robot Operative System) 14 13 Hardware 15

131 Kinect 16

2 BRISK Deteccioacuten y seguimientos de Keypoints 19 21 Workspace en ROS Hydro 19 22 Paquetes en ROS Hydro 21

221 Paquete ldquopublisherrdquo 21 222 Nodo publicador 22 223 Publisher packagexml 23 224 Publisher CMakeListsxml 24 225 Ejecucioacuten del nodo 25

23 Problema 2D 25 231 Subscriptorcpp 25 232 Funcioacuten ldquoMainrdquo 26 233 Subrutina ldquoCallbackrdquo 26

24 BRISK 29 241 Definicioacuten 29 242 Subrutina ldquouseBriskDetectorrdquo 30 243 Implementacioacuten BRISK 30

25 Filtrado de los Matches 33 251 Implementacioacuten subrutina RobustMach 34

3 Estimacioacuten movimiento UAV 38 31 Extraccioacuten puntos de las imaacutegenes de profundidad 39 32 Correspondencia puntos 2D3D 40 33 Resolucioacuten del problema PnP 41 34 Concatenacioacuten de matrices 41

4 Conclusioacuten y trabajo futuro 44

5 Bibliografiacutea 48

6 Anexos 49 61 Anexo A Publisher_S2launch 49 62 Anexo B Subscribercpp 49

10

IacuteNDICE DE FIGURAS

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II 11

Ilustracioacuten 2 Dron de reconocimiento Fulmar 11

Ilustracioacuten 3 MQ-1 Predator 12

Ilustracioacuten 4 Drones de investigaciones y desarrollo 12

Ilustracioacuten 5 Dron comercial 12

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS 14

Ilustracioacuten 7 Nivel graacutefico computacional de ROS 15

Ilustracioacuten 8 Frontal de Kinect 16

Ilustracioacuten 9 Esquema interno de Kinect 17

Ilustracioacuten 10 Relacioacuten planos 18

Ilustracioacuten 11 Terminal Terminator 19

Ilustracioacuten 12 Maestro arrancado 20

Ilustracioacuten 13 Frame Gray 28

Ilustracioacuten 14 Depth frame 28

Ilustracioacuten 15 Piramide de capas de octavas 31

Ilustracioacuten 16 Matches sin filtrar 33

Ilustracioacuten 17 Estereovisioacuten 34

Ilustracioacuten 18 Matches filtrados 37

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo 38

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen 39

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV 45

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x 46

1 INTRODUCCIOacuteN

en el presente documento se detalla el desarrollo del coacutedigo necsario para programar un sistema

odomeacutetrico por imaacutegenes para UAV Para el desarrollo de este sistema se ha hecho uso del sistema

operativo ROS (Robot Operative System) como entorno de desarrollo el dispositivo Kinect para obtener

informacioacuten del entorno y el lenguaje de programacioacuten C++

Un UAV (Unmaned Aerial Vehicle) tambieacuten conocido como dron es una aeronave no tripulada La aviacioacuten

no tripulada tuvo sus comienzos en los modelos de pioneros de la aviacioacuten como el franceacutes Feacutelix du Temple que

en 1857 patentoacute los disentildeos de una maacutequina aeacuterea que incorporaba un tren de aterrizaje plegable o el ingleacutes

John Stringfellow que en 1948 construyoacute varios planeadores no tripulados En la actualidad debido al desarrollo

tecnoloacutegico se han conseguido desarrollar drones con mayor autonomiacutea y sistemas que son capaces de seguir

la posicioacuten de nuestro dron como el GPS Estos y otros avances nos han permitido desarrollar aeronaves con

diversas aplicaciones como

Objetivos y sentildeuelos Proporciona a la artilleriacutea de aeacuterea y de tierra un objetivo que simula una aeronave

enemiga o un misil como el modelo SCRAB II que se puede ver en la ldquoilustracioacuten 1rdquo

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II

Reconocimiento Proporciona informacioacuten sobre el entorno como el modelo Fulmar que se muestra en

la ldquoIlustracioacuten 2rdquo

Ilustracioacuten 2 Dron de reconocimiento Fulmar

E

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

8

Iacutendice

Agradecimientos 6

Iacutendice 8

Iacutendice de Figuras 10

1 Introduccioacuten 11 11 Objetivos 13 12 Software 13

121 ROS (Robot Operative System) 14 13 Hardware 15

131 Kinect 16

2 BRISK Deteccioacuten y seguimientos de Keypoints 19 21 Workspace en ROS Hydro 19 22 Paquetes en ROS Hydro 21

221 Paquete ldquopublisherrdquo 21 222 Nodo publicador 22 223 Publisher packagexml 23 224 Publisher CMakeListsxml 24 225 Ejecucioacuten del nodo 25

23 Problema 2D 25 231 Subscriptorcpp 25 232 Funcioacuten ldquoMainrdquo 26 233 Subrutina ldquoCallbackrdquo 26

24 BRISK 29 241 Definicioacuten 29 242 Subrutina ldquouseBriskDetectorrdquo 30 243 Implementacioacuten BRISK 30

25 Filtrado de los Matches 33 251 Implementacioacuten subrutina RobustMach 34

3 Estimacioacuten movimiento UAV 38 31 Extraccioacuten puntos de las imaacutegenes de profundidad 39 32 Correspondencia puntos 2D3D 40 33 Resolucioacuten del problema PnP 41 34 Concatenacioacuten de matrices 41

4 Conclusioacuten y trabajo futuro 44

5 Bibliografiacutea 48

6 Anexos 49 61 Anexo A Publisher_S2launch 49 62 Anexo B Subscribercpp 49

10

IacuteNDICE DE FIGURAS

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II 11

Ilustracioacuten 2 Dron de reconocimiento Fulmar 11

Ilustracioacuten 3 MQ-1 Predator 12

Ilustracioacuten 4 Drones de investigaciones y desarrollo 12

Ilustracioacuten 5 Dron comercial 12

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS 14

Ilustracioacuten 7 Nivel graacutefico computacional de ROS 15

Ilustracioacuten 8 Frontal de Kinect 16

Ilustracioacuten 9 Esquema interno de Kinect 17

Ilustracioacuten 10 Relacioacuten planos 18

Ilustracioacuten 11 Terminal Terminator 19

Ilustracioacuten 12 Maestro arrancado 20

Ilustracioacuten 13 Frame Gray 28

Ilustracioacuten 14 Depth frame 28

Ilustracioacuten 15 Piramide de capas de octavas 31

Ilustracioacuten 16 Matches sin filtrar 33

Ilustracioacuten 17 Estereovisioacuten 34

Ilustracioacuten 18 Matches filtrados 37

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo 38

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen 39

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV 45

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x 46

1 INTRODUCCIOacuteN

en el presente documento se detalla el desarrollo del coacutedigo necsario para programar un sistema

odomeacutetrico por imaacutegenes para UAV Para el desarrollo de este sistema se ha hecho uso del sistema

operativo ROS (Robot Operative System) como entorno de desarrollo el dispositivo Kinect para obtener

informacioacuten del entorno y el lenguaje de programacioacuten C++

Un UAV (Unmaned Aerial Vehicle) tambieacuten conocido como dron es una aeronave no tripulada La aviacioacuten

no tripulada tuvo sus comienzos en los modelos de pioneros de la aviacioacuten como el franceacutes Feacutelix du Temple que

en 1857 patentoacute los disentildeos de una maacutequina aeacuterea que incorporaba un tren de aterrizaje plegable o el ingleacutes

John Stringfellow que en 1948 construyoacute varios planeadores no tripulados En la actualidad debido al desarrollo

tecnoloacutegico se han conseguido desarrollar drones con mayor autonomiacutea y sistemas que son capaces de seguir

la posicioacuten de nuestro dron como el GPS Estos y otros avances nos han permitido desarrollar aeronaves con

diversas aplicaciones como

Objetivos y sentildeuelos Proporciona a la artilleriacutea de aeacuterea y de tierra un objetivo que simula una aeronave

enemiga o un misil como el modelo SCRAB II que se puede ver en la ldquoilustracioacuten 1rdquo

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II

Reconocimiento Proporciona informacioacuten sobre el entorno como el modelo Fulmar que se muestra en

la ldquoIlustracioacuten 2rdquo

Ilustracioacuten 2 Dron de reconocimiento Fulmar

E

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

10

IacuteNDICE DE FIGURAS

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II 11

Ilustracioacuten 2 Dron de reconocimiento Fulmar 11

Ilustracioacuten 3 MQ-1 Predator 12

Ilustracioacuten 4 Drones de investigaciones y desarrollo 12

Ilustracioacuten 5 Dron comercial 12

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS 14

Ilustracioacuten 7 Nivel graacutefico computacional de ROS 15

Ilustracioacuten 8 Frontal de Kinect 16

Ilustracioacuten 9 Esquema interno de Kinect 17

Ilustracioacuten 10 Relacioacuten planos 18

Ilustracioacuten 11 Terminal Terminator 19

Ilustracioacuten 12 Maestro arrancado 20

Ilustracioacuten 13 Frame Gray 28

Ilustracioacuten 14 Depth frame 28

Ilustracioacuten 15 Piramide de capas de octavas 31

Ilustracioacuten 16 Matches sin filtrar 33

Ilustracioacuten 17 Estereovisioacuten 34

Ilustracioacuten 18 Matches filtrados 37

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo 38

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen 39

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV 45

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x 46

1 INTRODUCCIOacuteN

en el presente documento se detalla el desarrollo del coacutedigo necsario para programar un sistema

odomeacutetrico por imaacutegenes para UAV Para el desarrollo de este sistema se ha hecho uso del sistema

operativo ROS (Robot Operative System) como entorno de desarrollo el dispositivo Kinect para obtener

informacioacuten del entorno y el lenguaje de programacioacuten C++

Un UAV (Unmaned Aerial Vehicle) tambieacuten conocido como dron es una aeronave no tripulada La aviacioacuten

no tripulada tuvo sus comienzos en los modelos de pioneros de la aviacioacuten como el franceacutes Feacutelix du Temple que

en 1857 patentoacute los disentildeos de una maacutequina aeacuterea que incorporaba un tren de aterrizaje plegable o el ingleacutes

John Stringfellow que en 1948 construyoacute varios planeadores no tripulados En la actualidad debido al desarrollo

tecnoloacutegico se han conseguido desarrollar drones con mayor autonomiacutea y sistemas que son capaces de seguir

la posicioacuten de nuestro dron como el GPS Estos y otros avances nos han permitido desarrollar aeronaves con

diversas aplicaciones como

Objetivos y sentildeuelos Proporciona a la artilleriacutea de aeacuterea y de tierra un objetivo que simula una aeronave

enemiga o un misil como el modelo SCRAB II que se puede ver en la ldquoilustracioacuten 1rdquo

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II

Reconocimiento Proporciona informacioacuten sobre el entorno como el modelo Fulmar que se muestra en

la ldquoIlustracioacuten 2rdquo

Ilustracioacuten 2 Dron de reconocimiento Fulmar

E

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

1 INTRODUCCIOacuteN

en el presente documento se detalla el desarrollo del coacutedigo necsario para programar un sistema

odomeacutetrico por imaacutegenes para UAV Para el desarrollo de este sistema se ha hecho uso del sistema

operativo ROS (Robot Operative System) como entorno de desarrollo el dispositivo Kinect para obtener

informacioacuten del entorno y el lenguaje de programacioacuten C++

Un UAV (Unmaned Aerial Vehicle) tambieacuten conocido como dron es una aeronave no tripulada La aviacioacuten

no tripulada tuvo sus comienzos en los modelos de pioneros de la aviacioacuten como el franceacutes Feacutelix du Temple que

en 1857 patentoacute los disentildeos de una maacutequina aeacuterea que incorporaba un tren de aterrizaje plegable o el ingleacutes

John Stringfellow que en 1948 construyoacute varios planeadores no tripulados En la actualidad debido al desarrollo

tecnoloacutegico se han conseguido desarrollar drones con mayor autonomiacutea y sistemas que son capaces de seguir

la posicioacuten de nuestro dron como el GPS Estos y otros avances nos han permitido desarrollar aeronaves con

diversas aplicaciones como

Objetivos y sentildeuelos Proporciona a la artilleriacutea de aeacuterea y de tierra un objetivo que simula una aeronave

enemiga o un misil como el modelo SCRAB II que se puede ver en la ldquoilustracioacuten 1rdquo

Ilustracioacuten 1 Dron de sentildeuelo SCRAB II

Reconocimiento Proporciona informacioacuten sobre el entorno como el modelo Fulmar que se muestra en

la ldquoIlustracioacuten 2rdquo

Ilustracioacuten 2 Dron de reconocimiento Fulmar

E

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

12

Combate Proporciona capacidad para misiones de alto riesgo como el modelo MQ-1 Predator

mostrado en la ldquoilustracioacuten 3rdquo

Ilustracioacuten 3 MQ-1 Predator

Investigacioacuten y desarrollo Proporcionan bancos de pruebas para el desarrollo de nuevos sistemas para

UAV como el mostrado en la ldquoilustracioacuten 4rdquo

Ilustracioacuten 4 Drones de investigaciones y desarrollo

UAV civiles y comerciales Especialmente disentildeados para aplicaciones civiles y comerciales como el

que se muestra en la ldquoIlustracioacuten 5rdquo

Ilustracioacuten 5 Dron comercial

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

La definicioacuten de odoacutemetro seguacuten la RAE es ldquoinstrumento para medir distanciasrdquo es una palabra que deriva de

la palabra griega ὁδός -hodoacutes que significa ldquocaminordquo y -μετρον -metron que significa ldquoque miderdquo En nuestro

caso desarrollaremos un sistema odomeacutetrico por imaacutegenes que nos permitiraacute corregir parte del error de posicioacuten

proporcionado por el GPS Para la adquisicioacuten de las imaacutegenes usaremos el dispositivo Kinect desarollado por

Microsoft para su consola Xbox ya que implementa una caacutemara RGB que nos proporciona informacioacuten en 2D

del entorno y sensor de profundidad que nos proporciona informacioacuten 3D del entorno de navegacioacuten del dron

Como entorno de desarrollo y compilador usaremos el sistema operativo ROS (Robot Operative System) que

nos permite programar nodos es decir trozos de coacutedigos con funciones simples que se pueden comunicar con

otros nodos consiguiendo funciones maacutes complejas Una de la ventajas que nos proporciona este sistema es que

las funciones desarrolladas para un robot pueden ser adapatadas para otro robot necesitando poco cambios

En los siguientes capiacutetulos desarrollaremos el funcionamiento de cada uno de los componentes de este proyecto

mencionados anteriormente y como se interconectan para conseguir nuestro objetivo

11 Objetivos

Como mencionamos anteriormente el objetivo principal de este proyecto es programar un sistema odomeacutetrico

para UAV que nos permita estimar el posicionamiento de la aeronave Para llevarlo a cabo nos hemos

propuesto los siguientes objetivos

Implementar un nodo publicador en ROS que abra un fichero bash con las imaacutegenes que se han

tomado con usando un sistema Kinect acoplado en un dron durante un vuelo de prueba

Implementar un nodo subscriptor en ROS que se subscriba al topic de las imaacutegenes del Kinect

Desarollar un sistema de matching por fuerza bruta usando BRISK entre los frames del video

tomado con Kinect e implementarlo en el nodo subscriptor

Implementar un sistema de filtrado de matches para eliminar los falsos matches y mejorar la

robustez del sistema usando el meacutetodo de RANSAC y la matriz fundamental

Implementar una subrutina que lea las imaacutegenes de profundidad

Resolver el problema PnPRansac

Obtener las matrices de rotacioacuten y traslacioacuten de cada imagen

Obtener la concatenacioacuten de las matrices de rotacioacuten y traslacioacuten

Muestreo por pantalla de la trayectoria 3D seguida por el UAV

A continuacioacuten se presenta por capiacutetulos el desarrollo del software necesario para cumplir cada uno de los

objetivos expuestos anteriormente

12 Software

Para el desarrollo de nuestro programa hemos decidido usar el language de programacioacuten C++ desarrollado en

1980 por el daneacutes Bjarne Stroustrup cientiacutefico de la computacioacuten y catedraacutetico de Ciencias de la Computacioacuten

en la universidad AampM de Texas [1]

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

14

C++ es una extencioacuten del lenguage de programacioacuten C dotaacutendolo de mecanismos que lo convierten en un

lenguage de programacioacuten orientado a objetos Debido a que ROS nos permite la compilacioacuten de programas en

C++ y la posibilidad de usar las libreriacuteas de OpenCV hemos tomado la decision de usarlo como lenguage de

programacioacuten para nuestro proyecto

121 ROS (Robot Operative System)

El Sistema operative para Robots o ROS fue desarrollado originalmente en 2007 por el Laboratorio de

Inteligencia Artificial de Stanford (SAIL) con el soporte del Stanford AI Robot project A partir de 2008 el

instituto de investigacioacuten de roboacutetica Willow Garage en colaboracioacuten con maacutes de 20 instituciones continuan

desarrollando a traveacutes de este modelo de desarrollo federado [2]

ROS trabaja de forma similar a un sistema operativo es decir un programa organizado en carpetas que

continenen ficheros que proporcionan funciones a los robots En la ldquoIlustracioacuten 6rdquo podemos observar de forma

esquemaacutetica la jerarquiacutea de los ficheros de los que se compone un proyecto en ROS Antes de iniciar un

proyecto es importante entender cada uno de los elementos necesarios para desarrollar un programa en ROS

Ilustracioacuten 6 Jerarquiacutea de ficheros de ROS

Packages ficheros que funcionan como el elemento baacutesico de ROS Tiene la estructura y el

contenido miacutenimo para crear un programa

Manifests Ficheros del tipo ldquomanifestsxmlrdquo Contiene informacioacuten sobre los paquetes

Stacks Conjunto de paquetes interconectando que proporcionan funciones maacutes complejas

Stack manifests Ficheros del tipo ldquostackxmlrdquo Contiene informacioacuten sobre los stacks

Message (msg) types Informacioacuten que comparte un nodo con otro Hay diversos tipos de

mensajes dependiendo de la aplicacioacuten

Service (srv) types Contiene la estructura de los datos que enviaraacute el nodo y la estructura de

los datos que espera recibir

El meacutetodo de comunicacioacuten entre los procesos de ROS se basa en crear una red donde todos los procesos pueden

interactuar con otros procesos enviando informacioacuten o recibieacutendola En la ldquoIlustracioacuten 7rdquo observamos de

forma esquemaacutetica el nivel graacutefico computacional de ROS

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

Ilustracioacuten 7 Nivel graacutefico computacional de ROS

En este nivel encontramos todos los componentes para interconectar los procesos que vamos a desarrollar en

nuestro proyecto A continuacioacuten expondremos los conceptos necesarios para entender los componentes que

implementaremos en nuestro software

Nodes Son los procesos donde se realiza la computacioacuten de las funciones que queremos implementar

Para crear un proceso que se conecte con otros procesos debsmo implementar un nodo con este proceso

En nuestro caso seraacute un nodo publicador y un nodo subscriptor

Master El maestro nos proporciona un registro de registro de nombre e interconexioacuten entre los nodos

Messages Nuestros nodos enviacutean y reciben informacioacuten en forma de mensajes Dependiendo de la

aplicacioacuten ROS nos proporciona varios tipos de mensajes

Topics Es el nombre del mensaje que estamos publicando y al que se subscribiraacuten nuestros nodos Es

necesario que este nombre sea uacutenico para evitar confuciones a la hora de subscribirnos al ldquotopicrdquo

Bags Es el format que usa ROS para guardar y reproducir los mensajes que publicaremos o a los que

nos subscribiremos En nuestro proyecto publicaremos imaacutegenes de Kinect que hemos guardado en un

fichero llamado ldquosesion2bagrdquo El fichero ldquosesion1bagrdquo es un fichero de prueba que hemos usado

durante el desarrollo del proyecto y donde encontramos imaacutegenes grabadas con la web-cam de nuestro

ordenador para hacer las pruebas iniciales de matching

La version de ROS que usaremos en este proyecto seraacute ROS Hydro aunque cualquier otra version de ROS nos

serviriacutea para trabajar con las libreriacuteas necesarias

131 Octave

Para el muestreo por pantalla de la trayectoria seguida por el dron se propone el uso de Octave un software open

source que dispone de la herramienta ldquoGNU plotrdquo para muestreo de graacuteficas y que aprovecharemos para el

ploteo de la trayectoria obtenida

El proyecto Octave se inicio en 1998 como una herramienta propuesta para un curso de disentildeo de reactores

quiacutemicos antildeos maacutes tarde se emplio su funcionamiento para la resolucioacuten de operaciones numeacutericas

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

16

Este software usa un lenguaje de programacioacuten de alto nivel muy similar al de MATLAB del cual se ha Ganado

mucha experiencia durante todo el master

13 Hardware

En nuestro proyecto usaremos como hardware el dispositivo de Microsoft Kinect con el que tomaremos las

imaacutegenes y obtenemos informacioacuten 2D y 3D del entorno y desarrollaremos nuestro coacutedigo con un portaacutetil HP

Compaq 6730s

131 Kinect

Kinect es un dispositivo desarrollado por Microsoft par asu consola Xbox 360 y que se fue lanzado en

Norteameacuterica en 2010 Se basa en una caja plana con una pequentildea plataforma y con una serie de sensores que

nso permite usarla en tareas de visioacuten artificiall y roboacutetica Los sensores que nosotros usaremos son los

siguientes

Camera VGA Nos proporciona informacioacuten 2D en color del entorno

Sensor de profundidad Compuesto de un proyector de infrarojos y un sensor CMOS monocromo Nos

proporciona informacioacuten en 3D del entorno

En la ldquoIlustracioacuten 8rdquo se muestra el dispositivo Kinect que se acoplaraacute al dron y nos proporcionaraacute la informacioacuten

del entorno

Ilustracioacuten 8 Frontal de Kinect

Para desarrollar nuestro sistema odomeacutetrico es esencial tener informacioacuten sobre la profundidad de los objetos

principales de las imaacutegenes en 2D que tome nos proporcione Kinect El Sistema de deteccioacuten de profundiad de

Kinect funciona de la siguiente forma

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

1 El proyector de infrarojos hace un barrido del entorno

2 Se calcula la distancia a partir de la salida del haz de infrarrojos hasta que regresa al CMOS monocromo

3 Un software integrado en Kinect basado en un mapa de profundidad percibe e identifica los objectos

en tiempo real

El mapa de profundidad los guardaremos en un fichero de ROS ldquobashrdquo En la ldquoilustracioacuten 9rdquo podemos observar

un esquema interno del Kinect donde se puede observar como van dispuestas las distintas partes que conforman

este dispositivo

Ilustracioacuten 9 Esquema interno de Kinect

Para la realizacioacuten de nuestro proyecto debemos tambieacuten tener en cuenta los paraacutemetros de la caacutemara Kinect

[3] Estos paraacutemetros son intriacutensecos y extriacutensecos Los primeros son paraacutemetros internos que definen la

geometriacutea interna y la oacuteptica de la caacutemara y los segundos relacionan lo sistemas de referencia del mundo real y

la caacutemara

Paraacutemetros intriacutensecos proporcionados por el topic ldquocamera_infordquo

o Punto principal Punto de interseccioacuten entr el plano de la imagen y el eje oacuteptico (recta

perpendicular al plano de la imagen que pasa por el centro de la caacutemara ldquoOrdquo) Ver ldquoIlustracioacuten

10rdquo

o Distancia focal Distancia existente entre la caacutemara (punto ldquoOrdquo) y el punto principal Las

coordenadas vienen dadas en piacutexeles horizontales y verticales

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

18

Paraacutemetros extriacutensecos obtenidos una vez resuelto el problema PnP

o Vector de traslacioacuten T Determina la ubicacioacuten del centro oacuteptico de la caacutemara ldquoOrdquo con respecto

a los ejes del mundo real

o Matriz de rotacioacuten R Relaciona la rotacioacuten de la posicioacuten de la caacutemara (OXYZ) con respecto

a los ejes del mundo real (vuw)

Ilustracioacuten 10 Relacioacuten planos

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

2 BRISK DETECCIOacuteN Y SEGUIMIENTOS DE

KEYPOINTS

21 Workspace en ROS Hydro

El primer paso para empezar a trabajar con ROS es crear nuestro propio ldquoworkspacerdquo o espacio de trabajo como

se indica en el tutorial de ROS ldquoCreating a workspace for catkinrdquo para empezar a desarrollar nuestros propios

paquetes y nodos [4] Usaremos Catkin como compilador ya que es la versioacuten actualizada del compilador

original de ROS Rosbuild Las mejoras implementadas en Catkin con respecto a Rosbuild permiten una mejor

distribucioacuten de los paquetes mejora la capacidad de crear ejecutables multi-plataformas y una mejor portabilidad

de los paquetes desarollados [5]

El primer paso es iniciar el terminal ldquoTerminatorrdquo que nos permite en un uacutenico terminal abrir diversas ventanas

y arrancar en cada uno de ellos un nodo En la ldquoIlustracioacuten 11rdquo observamos la vista que tendriacuteamos del terminal

nada maacutes arrancarlo

Ilustracioacuten 11 Terminal Terminator

El siguiente paso es arrancar el maestro de ROS que crearaacute una red por la cual los nodos se podraacuten comunicar

entre ellos y nos permitiraacute compilar nuestros paquetes y nodos Para arrancar el nodo usaremos el comando

ldquoroscorerdquo en la ldquoIlustracioacuten 12rdquo vemos una imagen del maestro arrancado

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

20

Ilustracioacuten 12 Maestro arrancado

En cuando el maestro nos proporciona una ID como se muestra en la ldquoIlustracioacuten 12rdquo sabemos que lo hemos

arrancado perfectamente Una vez arrancado el maestro hacemos click derecho sobre la pantalla del terminal y

dividimos la pantalla vertical u horizontalmente creando un nuevo terminal

Para crear nuestro workspace haremos lo siguiente

1 ldquoSource the enviromentrdquo En este paso le diremos a ROS donde estaacuten las libreriacuteas que necesitaremos

para crear nuestro ldquoWorkspacerdquo y para compilar Usaremos el comando

$ source optrosindigosetupbash

2 Crear nuestro ldquoWorskpacerdquo es decir una carpeta a la que llamaremos ldquoCatkin_wsrdquo Para ello usaremos

los commandos

$ mkdir -p ~catkin_wssrc

Con este comando creamos una carpeta llamada ldquocatkin_wsrdquo y dentro de esta carpeta otra llamada ldquosrcrdquo

donde crearemos nuestros paquetes

$ cd ~catkin_wssrc

Con este comando entramos en la carpeta ldquosrcrdquo

$ catkin_init_workspace

3 Compilamos el ldquoWorkspacerdquo

$ cd ~catkin_ws

Entramos a la carpeta ldquocatkin_wsrdquo

$ catkin_make

Compilamos nuestro ldquoworkspacerdquo tras esto en nuestro directorio ldquocatkin_wsrdquo encontraremos los

ficheros ldquobuildrdquo y ldquodevelrdquo

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

4 Seteamos nuestro espacio de trabajo

$ source develsetupbash

5 Comprobamos que nuestro espacio de trabajo ha sido creado y seteado correctamente

$ echo $ROS_PACKAGE_PATH

El resultado debe ser algo parecido a

homeyourusercatkin_wssrcoptroskineticshareoptroskineticstacks

Tras estos pasos ya tendremos creados nuestro espacio de trabajo y configurado para empezar a desarrollar

nuestro software El siguiente paso es crear nuestro primer paquete como veremos a continuacioacuten

22 Paquetes en ROS Hydro

Para crear un paquete en ROS seguiremos el tutorial ldquoCreating a ROS Packagerdquo Una vez creado nuestro espacio

de trabajo y lo seteamos como mostramos anteriormente

1 Entrar en la carpeta src de nuestro workspace

$ cd ~catkin_wssrc

2 Usar el script catkin_create_pkg para crear nuestro primer paquete

$ catkin_create_pkg publisher std_msgs roscpp

Se crearaacute un fichero llamado ldquopublisherrdquo que contiene el fichero packagexml y un fichero

CMakeListsxml parcialmente completos con la informacioacuten de las dependencias dadas a

catkin_create_pkg

3 Compilar el Workspace y direccionar el archivo de setup

$ ~catkin_wsdevelsetupbash

4 Antildeadir nuestro espacio de trabajo al entorno de ROS Para ello direccionamos el fichero de setup

generado

$ ~catkin_wsdevelsetupbash

De esta forma generamos un fichero al que hemos llamado ldquopublisherrdquo que contiene los ficheros packagexml y

CMakeListsxml

221 Paquete ldquopublisherrdquo

El primer problema a resolver es programar un nodo capaz de descomprimir la informacioacuten del fichero

ldquosesion2bagrdquo y publicarla Para ello programaremos un nodo launch al que llamaremos ldquoPublisher_S2launchrdquo

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

22

Para trabajar con los ficheros bag usaremos las herramientas que nos proporciona el ldquorosbag packagerdquo

Baacutesicamente se basa en una serie de herramientas a las que podemos accede por liacutenea de commando y que nos

permiten grabar los datos que se esten transmitiendo por un topic y reproducir los datos que hayamos

almacenado

En nuestro caso la herramienta que vamos a usar es ldquorosbag playrdquo que nos permitiraacute al arrancar el nodo

bag_readerlaunch empezar a publicar los datos guardados en el fichero ldquosesion2bagrdquo El coacutedigo generado se

encuentra en el ldquoAnexo Ardquo

Ejecutamos el nodo y hacemos un ldquorostopic listrdquo entre los topics publicados nos centramos en

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_rawcompressedDepth

Para trabajar maacutes faacutecilmente usaremos la herramienta ldquorepublisrdquo que nos proporciona ldquoimage_transportrdquo de esta

forma descomprimimos las imaacutegenes Una vez implementadas estas herramientas observamos que los topics

publicados son

cameradepth_registeredimage_raw

cameradepth_registeredimage_rawcompressedDepth

camerargbimage_raw

camerargbimage_rawcompressedDepth

222 Nodo publicador

Usaremos roslaunch para gestionar la funcioacuten ldquorosbag_playrdquo basaacutendonos en el ejemplo del tutorial ldquoRoslaunch

tips for large projectsrdquo para generar el siguiente coacutedigo

ltlaunchgt

ltnode pkg= ldquorosbagrdquo type= ldquoplayrdquo name= ldquorosbagrdquo output= ldquoscreenrdquo args= ldquo- -clock

homecalculoncatkin_wsbagfilessesion2bagrdquogt

ltlaunchgt

Arrancamos el paquete ldquorosbagrdquo desde el fichero launch Node pkg= ldquorosbagrdquo

Seleccionamos la funcioacuten rosbag_play del paquete rosbag_package type= ldquoplayrdquo

Definimos el nombre de nuestro nodo name= ldquorosbagrdquo

Setearemos la funcioacuten de mostrar por pantalla output= ldquoscreenrdquo

Definiremos el argunmento del video que pasaremos por pantalla y la ubicacioacuten del fichero sesion2bag

args= ldquo- -clock homecalculoncatkin_wsbagfilessesion2bagrdquogt

Para descomprimir los topics antildeadiremos el siguiente coacutedigo

ltnode name= ldquorepublish_imgrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args= ldquocompressed in=camerargbimage_raw raw out=camerargbimage_rawrdquogt

ltnode name= ldquorepublish_depthrdquo type= ldquorepublishrdquo pkg= ldquoimage_transportrdquo output= ldquoscreenrdquo

args=ldquocompressedDepth in=cameradepth_registeredimage_raw raw out=

cameradepth_registeredimage_rawrdquogt

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

En el caso de los ficheros launch no se necesita modificar los ficheros CMakeListxml o el Packagexml para

poder ejecutar nuestro nodo

223 Publisher packagexml

Como mencionamos anteriormente el fichero packagexml es un manifiesto del paquete que nos proporciona la

informacioacuten acerca de este nombre del paquete versioacuten autor maintainer y dependencias en otros paquetes

Las dependecias de nuestro paquete se declaran en este manifiesto si las dependecias que usamos en nuestro

paquete no estaacuten declarados en este fichero o se ha hecho de forma incorrecta no podremos compilar

correctamente nuestro paquete En el terminal de Terminator podremos ver el fichero donde estaacute el error y en

queacute liacutenea con lo que podremos depurar faacutecilmente nuestro coacutedigo

Para modificar el manifiesto a nuestro paquete seguiremos el tutorial ldquocatkinpackagexmlrdquo [6] El formato

general de este fichero es el siguiente

ltpackagegt

ltnamegt Nombre del paquete ltnamegt

ltversiongt Versioacuten del paquete ltversiongt

ltdescriptiongt

Pequentildea descripcioacuten de las funciones del paquete

ltdescriptiongt

ltmaintainer email=Mail del maintainergt Nombre del maintainer ltmaintainergt

ltlicensegt Licencia con la que se publica el paquete ltlicensegt

lturlgthttprosorgwikifoo_corelturlgt

ltauthorgt Nombre del autor del paquete ltauthorgt

ltbuildtool_dependgt

Definimos el sistema con el que compilaremos nuestro paquete en nuestro caso catkin

ltbuildtool_dependgt

ltbuild_dependgt

Definimos las dependencias necesarias para compilar nuestro coacutedigo

ltbuild_dependgt

ltrun_dependgt

Definimos las dependencias necesarias para ejecutar nuestro coacutedigo

ltrun_dependgt

ltpackagegt

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

24

224 Publisher CMakeListsxml

El fichero CMakeListsxml contiene la informacioacuten necesaria para que Catkin pueda compilar nuestros

paquetes En este fichero se define coacutemo se debe compilar el paquete y donde instalarlo Para ello seguiremos

el tutorial ldquocatkinCMakeListstxtrdquo [7]

La estructura y el orden en este fichero es indispensable el formato a rellenar tiene los siguientes puntos en este

orden

Required CMake Version (cmake_minimum_required)

Package Name (project())

Find other CMakeCatkin packages needed for build (find_package())

MessageServiceAction Generators (add_message_files() add_service_files() add_action_files())

Invoke messageserviceaction generation (generate_messages())

Specify package build info export (catkin_package())

LibrariesExecutables to build (add_library()add_executable()target_link_libraries())

Tests to build (catkin_add_gtest())

Install rules (install())

El formato general que se genera al usar ldquocatkin_create_pkgrdquo contiene la informacioacuten mencionada anteriormente

y la iremos completando de la siguiente forma

1 Definimos las dependencias de nuestro paquete

find_package (catkin REQUIRED COMPONENTS [Dependencia 1] [Dependecia 2]hellip)

2 Declaramos los ficheros de mensajes a ser compilados

add_message_files (FILES [Nombre del fichero de mensaje 1]msg [nombre del fichero de mensaje

2]msghellip)

3 Declaramos los ficheros de servicios a ser compilados

add_service_files (FILES [Nombre del fichero de servicio 1]srv [Nombre del fichero de servicio

2]srvhellip)

4 Definimos los mensajes especiacuteficos del lenguaje y los ficheros de servicios

generate_messages (DEPENDENCIES std_msgs sensor_msgs)

5 Declaramos las dependencias que se usaraacute el paquete de catkin durante el tiempo de ejecucioacuten

catkin_package (CATKIN_DEPENDS [Dependencia 1] [Dependecia 2]hellip)

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

6 Definimos la direccioacuten del ejecutable

add_executable ([Nombre del paquete] src [Nombre del ejecutable]cpp)

add_dependencies ([Nombre del paquete]

$PROJECT_NAME_EXPORTED_TARGETS $catkin_EXPORTED_TARGETS)

225 Ejecucioacuten del nodo

Para ejecutar nuestro nodo arrancaremos el terminal Terminator y seguiremos los siguientes pasos

1 Arrancamos el maestro

$ roscore

2 Abrimos un nuevo terminal

3 Entramos en la carpeta de nuestro paquete y seteamos la fuente

$ cd catkin_ws

$ source develsetupbash

4 Compilamos nuestro paquete

$ catkin_make

5 Arrancamos el nodo

$ roslaunch [nombre_paquete] [nombre_ejecutable]launch

En nuestro caso arrancaremos nuestro nodo publicador con el comando

$ roslaunch publisher Publisher_S2launch

Al arrancar el nodo en el terminal donde lo hemos arrancado veremos en el caso de nuestro nodo publicador el

tiempo de ejecucioacuten de las imaacutegenes de Kinect Para saber que topics se estaacuten publicnado abriremos otro

terminal y usaremos el comando por pantalla ldquorostopic listrdquo que nos permitiraacute ver la lista de topics que se estaacuten

publicando

23 Problema 2D

231 Subscriptorcpp

Para desarollar nuestro sistema odomeacutetrico deberemos resolveremos el problema primero en 2D y luego en 3D

Para resolver el problema en 2D se implementaraacuten las siguientes funciones en nuestro nodo

Subscriptor al topic que publica las imaacutegenes 2D

Subrutina que trate las imaacutegenes 2D y las muestre por pantalla

Subrutina implementando el sistema de matching BRISK

Subrutina implementando un robust matching para mejorar el resultado del BRISK

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

26

Para resolver el problema en 3D implementaremos las siguientes funciones

Subscriptor a los datos de calibracioacuten de la caacutemara

Subscriptor a las imaacutegenes de profundidad depth_images

Obtencioacuten de los puntos de profundiad proporcionados por las depth_images

Resolucioacuten del problema PnP

Concatenacioacuten de las matrices de rotacioacuten y traslacioacuten obtenidas para cada frame

Plot de la trayectoria seguida por el UAV

232 Funcioacuten ldquoMainrdquo

La funcioacuten main es la subrutina principal de nuestro programa para generar esta funcioacuten nos basaremos en el

tutorial de ROS ldquoWriting a Simple Publisher and Subscriber (C++)rdquo [8]

Los dos grandes puntos a realizar en este coacutedigo son

1 Subscripcioacuten a los topics que nos proporciona el publicador ldquoPublisher_S2launchrdquo Nos subscribiremos

a los topics

Camerargbimage_raw Nos proporcionan las imaacutegenes con la informacioacuten en 2D

Cameradepth_registeredimage_raw Nos proporcionan las imaacutegenes con la informacioacuten 3D

Cameradepth_registeredcamera_info Nos proporciona los datos de la calibracioacuten de la

caacutemara

2 Sincronizacioacuten entre las imaacutegenes 2D y 3D

Para llevar a cabo la tarea propuesta no es solo necesario subscribirnos a las imaacutegenes 2D y 3D

sino que deben ir sincronizadas Para llevar a cabo la sincronizacioacuten de las imaacutegenes encontramos

la funcioacuten ldquosync_policiesrdquo que nos permite dos tipos de sincronizacioacuten exacta o aproacuteximada En

nuestro caso no podemos usar la sincronizacioacuten exacta de las imaacutegenes porque las imaacutegenes en 2D

y 3D auacuten siendo grabadas con el mismo sistema Kinect tienen distinta estampa de tiempo Es por

ello que usaremos la poliacutetica de sincronizacioacuten ldquoApproximaterdquo

Finalmente una vez realizado la subscripcioacuten a los topics y la sincronizacioacuten de los mismos hacemos la llamada

a la funcioacuten ldquoCallbackrdquo

233 Subrutina ldquoCallbackrdquo

En esta subrutina accederemos y haremos el tratamiento de la informacioacuten propocionada por los distintos topics

a los que nos hemos subscrito Debemos tener en cuenta que las imaacutegenes 2D y 3D son proporcionadas como

mensajes de ROS y debemos convertirlas en imaacutegenes que se puedan tratar usando las funciones de OpenCv

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

Para llevar a cabo estas tareas

1 Transformacioacuten de los mensajes de ROS en imaacutegenes que puedan ser tratadas por OpenCV y posterior

procesamiento de las imaacutegenes obtenidas

Transformacioacuten de los mensajes de ROS en imageacutenes de OpenCV

cv_ptr_rgb=cv_bridgetoCvCopy(msg_rgbsensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth=cv_bridgetoCvCopy(msg_depthsensor_msgsimage_encodingsTYPE_32

FC1)

Procesamiento de las imaacutegenes

Para empezar la resolucioacuten del problema en 2D definimos la siguiente loacutegica cada frame que

tomemos del topic ldquocamerargbimage_rawrdquo la guardaremos en una matriz geneacuterica a la que

llamaremos frameRGB Tras esto realizaremos los siguientes pasos

Conversioacuten de las imaacutegenes de RGB a escala de grises

Este es un paso geneacuterico en visioacuten artificial para tratar con imaacutegenes una imagen en

RGB tiene tres canales y es praacutecticamente lo mismo que tener una matriz

tridimensional es mucho maacutes sencillo trabajar conimaacutegenes en escala de grises Esta

conversioacuten se realiza baacutesicamente sumando el valor de cada piacutexel de la imagen y

dividiendo este valor entre el nuacutemero de canales Usando el comando mostrado a

continuacioacuten obtendremos un piacutexel en blanco y negro con un valor entre [0hellip255]

cvtColor(frameRGB frameGray1 CV_BGR2GRAY)

Ordenacioacuten de los frames

Como mencionamos anteriormente todos los frames obtenidos los iremos guardando

en la matriz frameRGB El primer frame que obtengamos lo pasaremos a escala de

frises y lo guardaremos en la matriz frameGray1 En las iteraciones consecutivas en

la matriz frameGray2 guardaremos el frame anterior mientras que en la matriz

frameGray1 mantendremos el frame actual

Obtencioacuten de las imaacutegenes de profundidad

Para pode guardar las imaacutegenes de profundidad tendremos dos posibilidad codificarlas

en formato TYPE_32FC1 o formato TYPE_16UC1 Dependiendo del formato en el

que se coja el formato a la hora de tomar el valor de profundidad habraacute que aplicarle

un factor para obtener la medida en metros En nuestro caso usaremos el formato

TYPE_32FC1 usando este formato no seraacute necesario aplicar ninguacuten factor

Comprobacioacuten

Para comprobar que tomamos correctamente las imaacutegenes con la informacioacuten en 2D

y 3D usaremos los comandos

imshow(ldquoframeRGBrdquo frameRGB) ()

imshow(ldquoDEPTH_1rdquo DEPTH_1) (Ver)

waitkey(30)

De esta forma obtenemos las imaacutegenes observadas en la ldquoIlustracioacuten 13rdquo y la

ldquoIlustracioacuten 14rdquo

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

28

Ilustracioacuten 13 Frame Gray

Ilustracioacuten 14 Depth frame

2 Acceso a los paraacutemetros de la caacutemara Estos paraacutemetros son proporcionados por el topic ldquocamera_infordquo

estos datos son proporcionados a traveacutes de la matriz de paraacutemetros intriacutensecos

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

Accederemos a los valores de la siguiente forma

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

Para acceder a esta matriz lo haremos como si fuera un vector de forma que K tendraacute

computacionalmente la siguiente forma K=[fx s xo 0 fy y0 0 0 1]

3 Llamada la funcioacuten BRISK

Una vez obtenidos los datos necesarios para el resolver nuestro sistema odomeacutetrico realizamos la

llamada a la funcioacuten ldquouseBriskDetectorrdquo en la cual implementaremos el sistema BRISK con un

matching por fuerza bruta por distancia de Hamming De esta forma obtendremos los matches con los

que resolveremos el problema en 2D

En la funcioacuten mencionada anteriormente tambieacuten implementaremos todo el coacutedigo necesario para

resolver el problema en 3D

24 BRISK

241 Definicioacuten

La descomposicioacuten de una imagen en regiones de intereacutes es una teacutecnica ampliamente aplicada en visioacuten artificial

para facilitar el estudio de las propiedades de las aacutereas obtenidas El muestreo de imaacutegenes reconocimiento de

objetos reconstruccioacuten de escenas en 3D y el seguimiento de objetos a traveacutes de imaacuteges se basan en encontrar

caracteriacutesticas reconocibles y estables entre los distintos frames La amplia variedad de meacutetodos para conseguir

estos objetivos han impulsado el desarrollo de numerosos enfoques para resolver estos problemas

Un detector ideal de puntos claves o ldquoKeypointsrdquo debe encontrar regiones de los distintos frames lo

suficientemente caracteriacutesticas como para ser detectados en los sucesivos frames a pesar del cambio de punto de

vista es decir debe ser lo suficientemente robusto como para detectar la zona de intereacutes sin importar las posibles

trasformaciones que sufran la imaacutegenes Del mismo modo el descriptor de un ldquoKeypointrdquo captura la

informacioacuten maacutes importante y distintiva de las regiones caracteriacutesticas y al igual que en el caso anterior esta

estructura debe ser reconocible en el resto de frames

En el marco de nuestro proyecto para obtener los Keypoints descriptores y realizar el matching entre los

distintos frames obtenidos usaremos el algoritmo BRISK (Binary Robust Invariant Scalable Keypoints) Como

mencionaacutebamos anteriormente la deteccioacuten efectiva y eficaz de Keypoints en imaacutegenes es una parte

fundamental en la mayoriacutea de aplicaciones de visioacuten artificial Actualmente los algoritmos maacutes usados son el

SURF [9] y el SIFT [10] debido su alto rendimiento En este sentido SURF destaca siendo considerado como

uno de los meacutetodos maacutes eficientes computacionalmente [11]

Para resolver el problema del tracking de puntos entre un frame y el siguiente se basa de forma resumida en la

obtencioacuten regiones de intereacutes o ldquofeaturesrdquo que puedan ser reonocibles en el siguiente frame La dificultad

inherente a la obtencioacuten de estas regiones de intereacutes se basa en el balance entre dos objetivos que entran en

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

30

conflicto obtener una alta calidad de los ldquofeaturesrdquo y unos bajos requisitos computacionales

Se propone la implementacioacuten del BRISK en este proyecto como un meacutetodo capaz de conseguir la misma

calidad en el ldquomatchingrdquo de las imaacutegenes que el meacutetodo SURF pero con un menor requisito computacional En

algunos casos el meacutetodo BRISK puede llegar a ser un orden de magnitud maacutes raacutepido que el meacutetodo SURF La

obtencioacuten de los Keypoints descriptores y el posterior matching de las imaacutegenes se explicaraacuten en los capiacutetulos

siguientes

242 Subrutina ldquouseBriskDetectorrdquo

Como se describiacutea en el apartado ldquo33 Subrutina Callbackrdquo una vez finalizado la obtencioacuten de los datos

necesarios para resolver el problema en 2D y 3D llamamos la funcioacuten ldquouseBriskDetectorrdquo pasando como

paraacutemetros las imaacutegenes en 2D en escala de grises las imaacutegenes de profundidad y los paraacutemetros intriacutensecos de

la caacutemara

En esta subrutina resolveremos el problema en 2D y el problema en 3D Para ello implementaremos las

siguientes funciones

Problema 2D

o Implementacioacuten BRISK

Definicioacuten de los paraacutemetros de BRISK

Crear el BRISK Matcher

Detectar los Keypoints en el frame actual y el anterior

Calcular los descriptores (feature vector)

Realizar el matching entre las imaacutegenes

o Implementacioacuten de una subrutina Robust Match

Problema 3D

o Obtencioacuten de un modelo de puntos de profundidad

o Hallar la correspondencia 2D3D

o Resolver el problema PnP usando el meacutetodo RANSAC

o Concatenacioacuten de las mastrices de Rotacioacuten y Traslacioacuten

o Plot de la trayectoria obtenida

243 Implementacioacuten BRISK

Como mencionamos anteriormente implementaremos el algoritmo BRISK para realizar el matching entre los

frames usando el meacutetodo de miacutenima distancia de Hamming que explicaremos maacutes adelante Para entender mejor

BRISK es necesario hacer mencioacuten al artiacuteculo ldquoBRISK Brinary Robust Invariant Scalable Keypointsrdquo [11]

Los pasos para implementar nuestro sistema de matching BRISK son los siguientes [12]

1 Definimos los paraacutemetros del sistema de matching BRISK

int Thresh = 60 Este valor define el umbral de aceptacioacuten para el FASTAGAST

Aumentando el umbral el sistema se vuelve menos tolerante en la definicioacuten de una esquina

es decir que un candidato necesitaraacute maacutes contraste con su vecino para ser considerado una

esquina

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

int Octave = 4 El detector brisk funciona en una piraacutemide escala-espacio que consiste en

capas de octavas e intra-octavas Para usar una escala simple setearemos este paraacutemetro a ldquo0rdquo

pero el valor tiacutepico es ldquo4rdquo

float PatternScales=10f Escala aplicada al patroacuten usado en el sampleo de los vecindarios

de un Keypoint

2 Creamos el matcher BRISK

briskDetectorcreate(ldquoFeature2DBRISKrdquo)

3 Deteccioacuten de los Keypoints

En esta etapa identificaremos zonas y puntos de intereacutes en la imaacutegenes subdiviendo las imaacutegenes en

vecindarios y tomando dentro de estos vecindarios los puntos que sean o mucho maacutes oscuro o mucho

maacutes claros que el resto de puntos del vecindario

La eficiencia del BRISK se ve mejorado al detectar los Keypoints usando un sistema capas de octavas

e intra-octavas conformando la imagen como si fuese una piraacutemide Este sistema de capa de octavas e

intra octavas se basa en dividir la imagen original en muestras maacutes pequentildeas de forma que sea maacutes

sencillo estudiar los vecindarios A las distintas muestras obtenidas se les llama octavas y a las muestras

que se localizan entre las distintas capas de octavas se les llama intra-octavas En la ldquoIlustracioacuten 15rdquo

siguiente se ve un esquema del modelo de octavas e intraoctavas

Ilustracioacuten 15 Piramide de capas de octavas

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

32

Otras de las ventajas de BRISK es que nos permite combinar distintos meacutetodos de deteccioacuten de

Keypoints y Descriptores En nuestro caso usaremos el meacutetodo de deteccioacuten de esquinas

FASTAGAST Este sistema nos proporciona diferentes alternativas de maacutescaras para la deteccioacuten de

los Keypoints de forma geneacuterica se usa la maacutescara 9-16 que requiere que al menos 9 piacutexeles

consecutivos de la muestra de 16 piacutexels sean muchos maacutes oscuros o claros que le piacutexel central para que

se cumpla el criterio de este meacutetodo de deteccioacuten

La maacutescara FAST 9-16 se aplica individualmente en cada capa de octava e intra-octava usando el

umbral (Thresh) definido anteriomente para identificar potencialmente regiones de intereacutes El punto que

consideraremos como Keypoint de la regioacuten de intereacutes en cuestioacuten deben cumplir la maacutexima condicioacuten

con respecto a los otros ocho vecinos de la misma capa como resultado ldquosrdquo del sistema FAST siendo

ldquosrdquo el umbral maacuteximo dentro del cuaacutel se sigue considerando un punto como una esquina dentro de una

imagen Ademaacutes debe cumplir que el resultado ldquosrdquo en la capa superior e inferior debe ser menor Este

proceso se realizaraacute de forma iterativa en cada una de las imaacutegenes para obtener los Keypoints en cada

uno de los frames que obtengamos

El coacutedigo necesario en c++ para obtener los Keypoints es el siguiente

briskDetectordetect(frameGray1 keypoints_1) Keypoints frame actual

4 Caacutelculo de los descriptores

Para el caacutelculo de los descriptores aplicamos un patroacuten consistente en ciacuterculos conceacutentricos aplicado a

los vecindarios de cada Keypoint para obtener valores en la escala de grises Procesando la intensidad

local de los gradientes obtenemos la direccioacuten caracteriacutestica Finalmente la muestra del patroacuten

orientado de BRISK se usa para obtener parejas realizando comparacioacuten de escalas de brillos los cuales

se ensamblan dentro del descriptor binario de BRISK

Dado un conjunto de Keypoints el descriptor BRISK basa en una cadena binaria que concatena los

resultados del test de comparacioacuten de escalas de grises Identificaremos la direccioacuten caracteriacutestica de

cada Keypoint que nos permite obtener descriptores con orientacioacuten normalizada con lo cual podremos

obtener una rotacioacuten invariante que es la clava para la robustez de nuestro sistema Ademaacutes

seleccionaremos la comparacioacuten del brillo centraacutendonos en maximizar la descriptividad

Para realizar el caacutelculo de los descriptores a partir de los Keypoints antildeadiremos la siguiente liacutenea

briskDetectorcompute(frameGray1keypoints_1descriptor_1) Descriptor del frame actual

5 Matching de los descriptores

El matching de dos descriptores de BRISK se realiza a traveacutes del caacutelculo de la distancia de Hamming

Como mencionamos anteriormente los descriptores son cadenas binarias la distancia de Hamming

entre los descriptores seraacute el nuacutemero de bits distintos entre un descriptor y el siguiente

Para realizar el matching entre los descriptores antildeadiremos la siguiente liacutenea a nuestro coacutedigo

matchermatch(descriptor_1 descriptor_2 matches) Matching entre los descriptores del

frame actual y el anterior

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

Para comprobar el resultado de lo que hemos implementado hasta ahora antildeadiremos la siguiente liacutenea

de coacutedigo para visualizar los dos frames que tomamos por iteracioacuten y los matches enter ellos

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches

img_goodmatches) Dibujamos los matches sobre los frames

imshow( Good Matches BRISK img_goodmatches ) mostramos ambos frames y los

matches enter ellos

waitKey(30)

El resultado obtenido es el que se muestra en la ldquoIlustracioacuten 16rdquo

Ilustracioacuten 16 Matches sin filtrar

25 Filtrado de los Matches

Antes de implementar la subrutina con el que eliminaremos parte de los falsos matches observamos en la

ldquoilustracioacuten 11rdquo se ha de introducir cierto conceptos como son las liacuteneas epipolares y la matriz

fundamental

Primero debemos tener en cuenta que es possible encontrar la imagen ldquoxrdquo de un punto en el plano 3D ldquoXrdquo

trazando una liacutenea que une este punto 3D con el centro de la caacutemara y que por el contrario el punto en la imagen

que observamos en la posicioacuten ldquoxrdquo puede ser ubicado en cualquier lugar de esta liacutenea en el espacio 3D Esto

implica que si queremos encontrar el correspondiente posicioacuten de un punto dado en el plano de la imagen

tenemos que buscar a lo largo de la proyeccioacuten de esta liacutenea en el segundo plano de imagen como se muestra en

la ldquoIlustracioacuten 17rdquo

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

34

Ilustracioacuten 17 Estereovisioacuten

Esta liacutenea imaginaria se llama liacutenea epipolar del punto ldquoxrdquo Esto define una restriccioacuten fundamental que debe

satisfacer dos puntos correspondientes con lo cual el match del punto en una imagen debe encontrarse en la

liacutenea epipolar de de este punto en la otra imagen Por otra parte la orientaciacuteon de la liacutenea epipolar dependeraacute de

la posicioacuten de ambas caacutemaras la posicioacuten de las liacuteneas apipolares caracterizan la geometriacutea del sistema formado

por las dos caacutemaras Tambieacuten debemos tener en cuenta que todas las liacuteneas epipolares pasan por el mismo punto

que corresponde a la proyeccioacuten del centro de una caacutemara en la otra caacutemara a este punto lo llamamos punto

epipolar

La matriz fundamental 3x3 ldquoFrdquo expresa matemaacuteticamente la relacioacuten entre un punto en el plano de la imagen y

su correspondiente liacutenea epipolar La matriz fundamental nos permite mapear un punto 2D en uno de los planos

de la imagen en la liacutenea epipolar de la otra vista

251 Implementacioacuten subrutina RobustMach

La existencia de falsos matches se debe principalmente a la similitud de los objetos a detectar De esta forma

se detectan varias tranformaciones no reales y el objeto es detectado varias veces en poses diferentes siendo la

tranforamcioacuten correcta la primera detectada y la que mejor describe al conjunto de puntos detectados [13]

Para mejorar el resultado obtenido con el Matching por fuerza bruta de la distancia de Hamming es necesario

implementar una subrutina ldquoRobustMatcherrdquo para filtrar los matches [14] Para entrar en la subrutina haremos

la siguiente llamada

match(keypoints_1 descriptor_1 keypoints_2 descriptor_2matches)

Pasaremos como argumentos los Keypoints de la imagen actual (Keypoints_1) de la imagen anterior

(Keypoints_2) sus respectivos descriptores y los matches

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

1 Subrutina Match

Esta funcioacuten nos proporcionaraacute los matches filtrados los Keypoints detectados y la matriz

fundamental ldquoFrdquo que estimaremos En el ldquoAnexo Brdquo se puede encontrar el coacutedigo necesario para

implementar esta funcioacuten

Primero haremos el matching entre los descriptores de la imagen anterior (descriptor_2) y la actual

(descriptor_1) devolviendo los dos mejores candidatos para ser un matching En segundo lugar

realizaremos la misma operacioacuten pero ahora entre los descriptores de la imagen actual

(descriptor_1) y los descriptores de la imagen anterior (descriptor_2) devolviendo nuevamente los

dos mejores candidator a matches En esta subrutina hemos implementado el matching usando el

el meacutetodo SURF

Estos candidatos a matches son obtenidos teniendo en cuenta la distancia entre sus descriptores si

esta distancia es menor para el primer candidato a match y mucho mayor para el segundo candidato

a match podemos concluir que el mejor match es el primer candidato

2 Subrutina ldquoratioTestrdquo

En ciertos casos la distancia entre ambos matches es muy similar existiendo la posibilidad de elegir

el match equivocado En estos casos desechamos ambos matches debido a su caraacutecter ambiguo

implementando una restriccioacuten por la cual la distancia entre el primer mejor candidato a ser un

match y el segundo mejor candidato a match es menor que un umbral definido previamente con un

valor de 04f

De esta forma obtendremos dos sets de matches relativamente buenos un set de matches entre la

imagen actual y la anterior y otro set entre la imagen anterior y la actual El siguiente paso es

comprobar los matches de acuerdo a ambos sets para ello llamaremos a la subrutina

ldquosymmetryTestrdquo

3 Subrutina ldquosymmetryTestrdquo

En esta rutina impondremos un esquema de matches simeacutetricos por el cual para aceptar un par de

matches estos deben ser el matching descriptor entre ellos De esta forma extraeremos los matches

comunes en ambos sets

4 Subrutina ldquoRansacTestrdquo

En esta subrutina implementaremos una restriccioacuten por la cual eliminaremos lo matches que no

cumplan la restriccioacuten epipolar Este test estaacute basado en el meacutetodo computacional Ransac por el

cual calcularemos la matriz fundamental aun habiendo falsos matches

RANSAC es una teacutecnica de remuestreo que genera soluciones candidatas mediante el uso del

nuacutemero miacutenimo de observaciones requeridos para estimar los paraacutemetros subyacentes del modelo

A diferencia de las teacutecnicas convencionales de muestreo que utilizan la mayor cantidad de datos

posible para obtener una solucioacuten y luego proceder a eliminar los valores atiacutepicos RANSAC utiliza

el menor conjunto posible y procede a ampliar este conjunto con puntos de datos consistentes [15]

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

36

Dado un problema de ajuste con paraacutemetros ldquo rdquo estimar los paraacutemetros

Asumiendo

Los paraacutemetros pueden estimarse a traveacutes de ldquoNrdquo datos

En total tendremos un conjunto de ldquoMrdquo datos

La probabilidad de seleccionar de forma aleatoria un elemento de lo datos disponibles

que pueda ser parte de un modelo correcto es ldquoPgrdquo

La probabilidad que el algoritmo salga sin encontrar un ajuste correcto en caso de que

exista al menos un ajuste correcto lo denominaremso ldquoPfrdquo

Una vez definido lo expuesto arriba el algoritmo RANSAC realiza los siguientes pasos

1 TomaldquoNrdquo datos aleatorios

2 Estima el paraacutemetro ldquo rdquo

3 Encuentra cuantos datos (del conjunto de datos ldquoMrdquo) cumplen con el modelo

constituido por un paraacutemetro vector ldquo rdquo con una cierta tolerancia dada por el

usuario a la cual llamaremos ldquoKrdquo

El tamantildeo de ldquoKrdquo dependeraacute del porcentaje de datos que creemos que se ajustan

a la estructura del modelo y cuaacutentas estructuras tengamos en la imagen En el

caso de que halla muacuteltiples estructuras en la imagen tras un ajuste exitoso

retiramos el fit y reiniciamos RANSAC

4 Si ldquoKrdquo es lo suficientemente amplia aceptamos el ajuste y salimos del bucle de

forma exitosa

5 Repetimos los puntos del 1 al 4 ldquoLrdquo iteraciones

6 Se considerara un fallo si llegamos a este punto tras ldquoLrdquo iteraciones Para

calcular el nuacutemero de iteraciones ldquoLrdquo

119871 =119897119900119892(119875119891119886119894119897)

119897119900119892(1 minus (119875119892)119873)

Una vez implementados todas las restricciones mencionadas anteriormente obtendremos un set de matches

filtrados Para comprobar el resultado mostraremos por pantalla los filtrados el resultado es el que se muestra

en la ldquoIlustracioacuten 18rdquo

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

Ilustracioacuten 18 Matches filtrados

Como podemos observar en la ldquoIlustracioacuten 18rdquo aun quedan falsos matches en la imagen pero observamos que

algunos matches que estaacuten en la imagen de la derecha ya no corresponden con matches en la izquierda Una vez

hemos obtenido estos datos damos por resuelto el problema en 2D

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

38

3 ESTIMACIOacuteN MOVIMIENTO UAV

Como describimos en capiacutetulos anteriores para el desarrollo de este proyecto habiacuteamos planteado dos grandes

problemas a resolver el problema en 2D y el problema en 3D Una vez obtenido los matches filtrados

procederemos a empezar a resolver el problema en 3D

Basaacutendonos en los puntos extraiacutedos de las imaacutegenes en 2D y 3D identificaremos la posicioacuten actual de nuestro

dron para ello deberemos obtener una matriz de traslacioacuten y una matriz de rotacioacuten Por ello resolveremos el

problema PnP usando el meacutetodo Ransac ya que como comentaacutebamos anteriormente es un meacutetodo iterativo que

nos permite eliminar todos aquellos matches incorrectos tambieacuten definidos como ldquooutliersrdquo

La finalidad del ldquoPerspective-n-Point problemrdquo (PnP problem) es determinar la posicioacuten y orientacioacuten de la

caacutemara dado los paraacutemetros intriacutensecos de la caacutemara y un set de ldquonrdquo correspondencias entre los puntos 3D y

sus proyecciones 2D La formulacioacuten del problema es el siguiente

Dado un conjunto de correspondencias entre los puntos 3D ldquoPirdquo expresado en el marco de la referencia

del mundo real y sus proyecciones 2D ldquoUirdquo en una imagen buscamos obtener la posicioacuten (R y T) de

la caacutemara con respecto el mundo y la distancia focal ldquofrdquo como se muestra en al ldquoilustracioacuten 19rdquo

Ilustracioacuten 19 Relacioacuten sistema coordenadas caacutemaramundo

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

Usando la siguiente formula podemos proyectar los puntos 3D en el plano 2D

Ilustracioacuten 20 Proyeccioacuten puntos 3D en el plano de la imagen

Una vez planteado el problema llevaremos a acabo la resolucioacuten a traveacutes de los siguientes pasos

1 Extraeremos la profundiad de las Depth_images que nos proporciona Kinect

2 Encontrar la correspondencia entre los puntos 2D y los puntos 3D

3 Obtener los paraacutemetros intriacutensecos de la caacutemara

4 Resolver el problema PnP

5 Estimar la posicioacuten dado una lista de correspondencias 2D3D

6 Concatenar las matrices de rotacioacuten y traslacioacuten

7 Mostras por pantalla el frame actual y su posicioacuten extraida de la matriz de traslacioacuten concatenada

31 Extraccioacuten puntos de las imaacutegenes de profundidad

El primer paso que realizaremos es extraer los puntos de las imaacutegenes de profundidad para poder a posteriori

resolver el problema PnP

Para llevar a cabo esta tarea proyectaremos los puntos en 3D (x y z) sobre una imagen plana (fxz fyz)

basaacutendonos en el esquema que se muestra en la ldquoIlustracioacuten 19rdquo Para trasladar las coordenadas de pixels a

posiciones en el mundo real dividiremos la posicioacuten proporcionada por la imagen en 2D entre el pixel referente

al ancho de la image ldquopxrdquo y el pixel en la coordenada ldquopyrdquo referente al alto de la imagen Dividiendo la distancia

focal ldquofrdquo expresada en unidades del mundo real (mm m etchellip) entre ldquopxrdquo obtenemos la distancia focal

expresada en pixels horizontales (fx) Dividiendo la misma distancia focal entre ldquopyrdquo obtenemos la distancia

focal expresada en pixels verticales (fy) Por lo tanto la ecuaciones resultantes son las siguientes

119909 =119891119909lowast 119883

119885+ 1199060

119910 =119891119910lowast 119884

119885+ 1199070

Siendo u0 y v0 el punto principal perteneciente a la matriz intriacutenseca de la caacutemara explicados en capiacutetulos

anteriores En nuestro caso los paraacutemetros intriacutensecos de la caacutemara Kinect proporcionados por el topic

ldquocam_infordquo son los siguientes

fx=fy= 570342

cx= 3145

cy= 2355

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

40

Para implementar lo anteriormente expuesto llevaremos a cabo los siguientes pasos

1 Crear dos bucles for que recorran toda slas posiciones de la imagen de profundidad actual

frameDEPTH1 definidos de la siguiente forma

for(int x = 0 x lt frameDEPTH1rows ++x)

for(int y = 0 y lt frameDEPTH1cols ++y)

2 Dentro del bucle for accedemos a cada uno de los valores dentro de la matriz de profundidad Al guarder

el valor en formato float tomamos el valor en metros

float d= frameDEPTH1atltfloatgt(y x)

3 Filtramos los valores de profundidad ldquodrdquo debido a que muchos valores que obtendremos seraacuten NAN o

infinito y nos daraacuten problema a la hora de resolver el problema PnP Para ello creamos el siguiente bucle

if poniendo dos condiciones muy restrictivas La primera que el valor sea mayor que cero y la segunda

teniendo en cuenta la propia capacidad de Kinect que la medida de profundiad sea menor a 8 metros

debido a limitaciones fiacutesicas inherentes a este dispositivo

if (d gt 0 ampamp d lt 80)

4 Para los valores de profundidad que cumplan las dos restriciones impuestas en el bucle if explicado

anteriormente calcularemos los puntos en el mundo real como se muestra a continuacioacuten

pz = d

px = (float)(x-cx) d (1fx)

py = (float)(y-cy) d (1fy)

Los valores que no cumplan estas condiciones seraacuten directamente desechados

5 Una vez obtenidos estos puntos los guardaremos en un vector ltpoint3fgt que usaremos para encontrar

las correspondencias entre los puntos 2D y 3D

32 Correspondencia puntos 2D3D

Una vez hemos obtenido un conjunto de puntos 2D que hacen referencia a puntos caracteriacutesticos de las imaacutegenes

y un conjunto de puntos de profundidad proporcionados por las imaacutegenes de profundidad procederemos a

buscar que valores de profunidad corresponden a esos puntos caracteriacutesticos encontrados anteriormente De esta

forma podremos generar un vector que define el punto caracteriacutestico en el espacio 3D del mundo real

Para hallar las correspondencias entre los puntos 2D y 3D nos basaremo en el coacutedigo de OpenCV ldquoReal Time

pose estimation of a textured objectrdquo [16] Para llevar a cabo esta tarea necesitaremos el vector ltpoint3fgt

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

generado anteriormente con los puntos x y z (valores en metros en el mundo real) los Keypoints de la imagen

actual ldquoKeypoints_1rdquo y los matches filtrados que hemos obtenido anteriormente

El proceso es el siguiente cogeremos los puntos del vector que contiene los posiciones de profundiad que

correspondan con los puntos en 2D que sean matches para ello implementaremos el siguiente coacutedigo

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ] 3D point from

model

Haremos lo mismo para los Keypoints solo tomaremos aquellos que se hayan tomado como matches

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt 2D point from the

scene

Tras realizer estas operaciones obtendremos dos set de puntos uno con las posiciones en el 2D y otro con las

posiciones de profundidad los cuales ya hemos hemos filtrado solo para tomar aquellos puntos que sea matches

Una vez realizado este paso podemos proceder a estimar la posicioacuten actual basaacutendonos en la imagen 2D actual

y la imagen de profundidad actual

33 Resolucioacuten del problema PnP

Como mencionaacutebamos anteriormente para obtener la posicioacuten actual de nuestro dron basaacutendonos en la

informacioacuten proporcionada por las imaacutegenes RGB y las imaacutegenes de profundidad obtenidas por Kinect

Para implementar la resolucioacuten del problema PnP usaremos la siguiente liacutenea de coacutedigo

solvePnPRansac( list_points3d_model_match list_points2d_scene_match _A_matrix distCoeffs

rvec tvec useExtrinsicGuess iterationsCount reprojectionError confidenceinliers flags )

El resultado de esta funcioacuten seraacute el vector ldquorvecrdquo y ldquotvecrdquo el siguiene paso es pasar los vectores a matrices Para

ello implementaremos las siguientes liacuteneas de coacutedigo

Rodrigues(rvecRa_matrix) En el caso de la matriz de rotacioacuten vemos que necesitamos usar la

formula de rotacioacuten de rodriacuteguez [ref] llamado Olinde Rodriguez que es un eficiente algoritmo para

rotar un vector en el espacio dado un eje y un aacutengulo de rotacioacuten Por extension se puede usar para

transformar el vector ldquorvecrdquo en la matriz ldquoRa_matrixrdquo

Ta_matrix= tvec De esta forma convertimos el vector de traslacioacuten en una matriz de traslacioacuten

De esta forma obtenemos una matriz ldquoTardquo que nos proporciona la traslacioacuten que ha habido entre el frame

anterior y el actual y una matriz ldquoRardquo que nos proporciona la rotacioacuten que ha habido entre el frame anterior y

actual

34 Concatenacioacuten de matrices

Para obtener la trayectoria total que ha recorrido nuestro dron deberemos tomar las transformaciones rotaciones

y traslaciones en cada frame y concatenarlas La concatenacioacuten es la forma en la que podemos encadenar la

sucesioacuten de transformaciones mencionadas anteriormente

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

42

La finalidad de la concatenacioacuten es obtener una matriz de 4x4 formada por la rotacioacuten y traslacioacuten total durante

todo el tiempo de vuelo Esta matriz se conforma matemaacuteticamente de la siguiente forma

Para una matriz de traslacioacuten ldquoTrdquo de 3x1 como la que obtendremos de la resolucioacuten del problema PnP

tendremos primero que antildeadir una fila maacutes con un 1 para homogeneizar la matriz y obtener una matriz

ldquoTrdquo de 4x1

T_matrix=

Para una matriz de rotacioacuten ldquoRrdquo de 3x3 como la obtenida de la resolucioacuten del problema PnP

volveremos antildeadir fila maacutes en este caso todas las posiciones con valores 0 para homogeneizar la matriz

y obtener una matriz ldquoRrdquo de 4x3

R_matrix=

Finalmente la matriz concatenada tendraacute la siguiente forma

T_matrix=

Esta matriz es la mostrada anteriormente en la ldquoilustracioacuten 20rdquo como parte de la formulacioacuten de la

proyeccioacuten de los puntos 3D en el plano de la imagen 2D

Para realizar lo expueto anteriormente el primer paso seraacute homogeneizar las matrices ldquoRa_matrixrdquo y

ldquoTa_matrixrdquo para generar matrices con el orden correcto para su posterior concatenacioacuten Una vez se realiza

la homogeneizacioacuten procedemos a su concatenacioacuten la cual implementamos a traveacutes del siguiente comando

hconcat (Ra_matrix Ta_matrix T_matrix)

T00

T10

T20

1

R00 R01 R02

R10 R11 R12

R20 R21 R22

0 0 0

R00 R01 R02 T00

R10 R11 R12 T10

R20 R21 R22 T20

0 0 0 1

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

Este commando nos permite hacer una concatenacioacuten horizontal es decir crear una matriz ldquoT_matrixrdquo cuyos

terminos seraacuten los de la matriz ldquoRa_matrixrdquo y ldquoTa_matrixrdquo la condicioacuten de este comando es que las dos matrices

que van a conformar la concatenacioacuten tengan el mismo nuacutemero de filas

Durante la primera iteracioacuten de nuestro coacutedigo calcularemos las matrices de rotacioacuten y traslacioacuten totales como

se muestra a continuacioacuten

Primera Iteracioacuten

if (k == 0)

Rt_matrix= Ra_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

k++

En caso contrario donde kgt0 realizaremos lo siguiente

Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

Rt_matrix= Rt_matrixRa_matrix

En este bucle calcularemos la matriz Tt multiplicando Tt por la inversa de la matriz T que hemos calculado

anteriormente Para realizar esto debemos tener en cuenta que el nuacutemero de columas de la matriz Tt debe

coincidir con el nuacutemero de filas de la inversa de la matriz T para poder realizar la multiplicacioacuten En este caso

la matriz Tt tendraacute 4 filas 4 columnas y la inversa de la matriz T tendraacute 4 filas 4 columnas

Una vez hemos obtenido la traslacioacuten de las matrices de traslacioacuten y de rotacioacuten procederemos a guardar la

trayectoria punto a punto en un fichero txt que a posteriori cargaremos en octave para su final muestreo por

pantalla

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

44

35 Almacenamiento de la trayectoria

La tarea a realizar se basaraacute en guarder la posicioacuten concatenada del dron paso a paso guardando las posicioacutenes

ldquoxrdquo ldquoyrdquo y ldquozrdquo linea a liacutenea Para llevar a cabo esta tarea en ROS haremos lo siguiente

1 Apertura del fichero que vamos a crear

ofstream filename De esta forma obtenemos un objecto ldquofilenamerdquo que nos permite escribir

en un fichero

filenameopen(ldquotrayectoriatxtrdquo iosapp)

El flag ldquoiosapprdquo es el que permite que todas las operaciones de salidas se realicen al final del fichero

de esta forma cada dato que antildeadamos iraacute uno debajo de otro formado un vector vertical con tantas

liacuteneas como posiciones guardemos

2 Almacenamiento de los datos

FilenameltltTt_matrixatltdoublegt(03)ltltlsquonrsquoltltTt_matrixatltdoublegt(03)ltltlsquonrsquo

ltltTt_matrixatltdoublegt(13)ltlt lsquonrsquo

ltltTt_matrixatltdoublegt(23)ltlt lsquonrsquo

De esta forma guardaremos las posiciones de la Tt_matrix correspondientes a la Px Py y Pz liacutenea a

liacutenea en el fichero ldquotrayectoriatxtrdquo creado anteriormente

3 Cierre del fichero ldquoTrayectoriatxtrdquo

Una vez guardamos la posicioacuten obtenida en la iteracioacuten actual procedemos a cerrar el fichero usando el

siguiente comando

filenameclose()

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

4 RESULTADOS EXPERIMENTALES

Como mencionaacutebamos anteriormente usaremos Octave para el mostrar la trayectoria que hemos obtenido

anteriormente Para ello implementaremos el coacutedigo que se encuentra en el Anexo C doacutende realizaremos la

funcioacuten de muestreo de la trayectoria obtenida

Los resultados obtenidos para la trayectoria que realiza nuestro UAV durante todo el vuelo en 3D son los que

se observan en la ldquoilustracioacuten 21rdquo

Ilustracioacuten 21 Trayectoria 3D recorrida por el UAV

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

46

En la ldquoIlustracioacuten 22rdquo podemos observar la perspectiva de la trayectoria de los ejes (x y)

Ilustracioacuten 22 Vista de la trayectoria en los ejes y x

Como podemos observar en los resultados el error que obtenemos en el desplazamiento de cada eje es

considerable este error no es medible ya que no tenemos un modelo del dron ni un modelo de la trayectoria

real aun asiacute teniendo en cuenta las cifras obtenidas no son realistas

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

5 CONCLUSIOacuteN Y TRABAJO FUTURO

El crecimiento exponencial que ha sufrido la industria de los UAVs a nivel civil industrial y militar en la

uacuteltima deacutecada a propiciado el desarrollo de nuevo hardware y software destinado a crear nuevas funcionalidad

como a mejorar las ya existentes En este proyecto hemos generado un sistema odomeacutetrico que proporciona

precisioacuten y fiabilidad en la navegacioacuten de estos vehiacuteculos mejorando las prestaciones que ya nos proporcionaban

los sitemas GPS que vienen integrados en la mayoriacutea de dispositivos electroacutenicos que se fabrican actualmente

Otra funcioacuten del sistema que hemos generado es la navegacioacuten en los casos en los que el GPS no sea funcional

como por ejemplo la navegacioacuten a traveacutes de un tuacutenel donde la sentildeal de GPS se pierde

Para llevar a cabo este trabajo hemos hecho un estudio en varios campos de conocimientos como es la visioacuten

artificial la programacioacuten en C++ implementacioacuten de OpenCV y la programacioacuten en ROS Estos campos de

estudio no estaacuten estrictamente relacionados con lo que hemos aprendido durante la carrera o el maacutester pero nos

proporciona un punto de vista maacutes amplio sobre estos campos de los que soacutelo poseiacuteamos conceptos y por otra

parte nos da un valor antildeadido a nivel profesional

Es necesario antildeadir que los resultados obtenidos en este proyecto son orientativos y no aportan resultados

completamente reales En caso de que quisieacutesemos hacer de este proyecto un producto comercializable

deberiacuteamos implementar o tener en cuenta lo siguiente

Definir un modelo cinemaacutetico del dron del cual tomamos las imaacutegenes De esta forma tendriacuteamos una

forma de medir el error que tiene nuestro sistema odomeacutetrico

Comparar el resultado de nuestro sistema usando diversos meacutetodos de matching

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

48

6 BIBLIOGRAFIacuteA

[1] Bjarne Stroustrup The C++ Programming Language Addison-Wesley Pub Co 3rd edition 2000

[2] Aaron Martiacutenez Enrique Fernaacutendez Learning ROS for Robotics Programming PACKT publishing 1st

edition 2013

[3] Diana Beltraacuten Guerrero Luis Basantildeez Villaluenga MICROSOFT KINECT Universidad Politeacutecnica de

Catalunya Av Diagonal 647 Planta 11 08028 Barcelona Espantildea Instituto de Organizacioacuten y Control de

Sistemas Industriales

[4] httpwikirosorgcatkinTutorialscreate_a_workspace (Creating a workspace for catkin)

[5] httpwikirosorgcatkinconceptual_overview (Catkin Conceptual overview)

[6] httpwikirosorgcatkinpackagexml (Catkin Packagexml)

[7] httpwikirosorgcatkinCMakeListstxt (Catkin CMakeListstxt)

[8] httpwikirosorgROSTutorialsWritingPublisherSubscriber28c2B2B29 (Writing a Simple

Publisher and Subscriber (C++)

[9] httpwwwcoldvisionio20160627object-detection-surf-knn-flann-opencv-3-x-cuda (Object detection

with SURF KNN FLANN OpenCV 3X and CUDA)

[10] httpsdocsopencvorg330dadf5tutorial_py_sift_introhtml (Introduction to SIFT (Scale-Invariant

Feature Transform)

[11] Stefan Leutenegger Margarita Chli and Roland Y Siegwart BRISK Binary Robust Invariant Scalable

Keypoints Autonomous Systems Lab ETH Zuumlrich

[12] Adrian Kaehler amp Gary Bradski Learning OpenCV 3 OrsquoReilly Media 1st edition 2016

[13] Manuel Lacuteopez Radcenco Reconocimiento de Objetos en Imaacutegenes a traveacutes de Matching de

Descriptores ASIFT mediante un enfoque estadiacuteıstico A Contrario y MAC-RANSAC

[14] Robert Laganiegravere OpenCV 2 Computer Vision Application Programming Cookbook PACKT Publishing

1st edition 2011

[15] httphomepagesinfedacukrbfCVonlineLOCAL_COPIESFISHERRANSAC (The RANSAC (Random

Sample Consensus) Algorithm

[16] httpsdocsopencvorg310dcd2ctutorial_real_time_posehtml (Real Time pose estimation of a textured

Object)

[17] Yinqiang Zheng Yubin Kuang Shigeki Sugimoto Kalle Aringstroumlm Masatoshi Okutomi Revisiting the PnP

Problem A Fast General and Optimal Solution Centre for Mathematical Sciences Lund University SWEDEN

7 ANEXOS

71 Anexo A Publisher_S2launch

ltlaunchgt

ltparam name=use_sim_time value=truegt

ltnode pkg=rosbag type=play name=rosbag output=screen args=mdash

clockhomecalculoncatkin_wsbagfilessesion2baggt

ltnode name=republish_img type=republish pkg=image_transport output=screen

args=compressed in=camerargbimage_raw raw out=camerargbimage_rawgt

ltnode name=republish_depth type=republish pkg=image_transport output=screen

args=compressedDepth in=cameradepth_registeredimage_raw raw

out=cameradepth_registeredimage_rawgt

ltlaunchgt

72 Anexo B Subscribercpp

Encabezados de ROS

include ltrosroshgt

include ltimage_transportimage_transporthgt

include ltcv_bridgecv_bridgehgt

include ltsensor_msgsimage_encodingshgt

Encabezados de OpenCV

include ltopencv2imgprocimgprochppgt

include ltopencv2opencvhppgt

include ltopencv2features2dfeatures2dhppgt

include ltopencv2highguihighguihppgt

Encabezados de Sincronizacioacuten imaacutegenes

include ltmessage_filterssubscriberhgt

include ltmessage_filterstime_synchronizerhgt

define APPROXIMATE

ifdef APPROXIMATE

include ltmessage_filterssync_policiesapproximate_timehgt

endif

using namespace sensor_msgs

using namespace message_filters

using namespace cv

using namespace std

50

----------------------------------------------------------------------------------------------------------------------

VARIABLES GLOBALES

int i=0

k=0

Mat frameRGB

frameDEPTH1

frameDEPTH2

frameGray1

frameGray2

F

Temp_matrix

Temp2_matrix

Temp3_matrix

TempT_matrix

Mat Ra_matrix = Matzeros(3 3 CV_64FC1) Actual rotation matrix

Mat R_matrix = Matzeros(4 3 CV_64FC1)

Mat Rt_matrix = Matzeros(4 4 CV_64FC1) Total rotation matrix

Mat Ta_matrix = Matzeros(3 1 CV_64FC1) Actual translation matrix

Mat T_matrix = Matzeros(4 1 CV_64FC1)

Mat Tt_matrix = Matzeros(4 4 CV_64FC1) Total translation matrix

vectorltPoint2fgt points1

points2

vectorltDMatchgt good_matches

matches

matches1

matches2

symMatches

outMatches

float ratio= 065f max ratio between 1st and 2nd NN

bool refineF= false if true will refine the F matrix

bool ransacTest_= true

bool ratioTest_= false

bool refineF_= false

BFMatcher matcher

----------------------------------------------------------------------------------------------------------------------

DEFINICIOacuteN FUNCIONES

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

Mat match(vectorltKeyPointgt keypoints_1

Mat descriptor_1

vectorltKeyPointgt keypoints_2

Mat descriptor_2

vectorltDMatchgt matches

bool ransacTest_

bool ratioTest_

bool refineF_)

int ratioTest(vectorltvectorltDMatchgt gt ampmatches)

void symmetryTest(const vectorltvectorltDMatchgt gt ampmatches1

const vectorltvectorltDMatchgt gt ampmatches2

vectorltDMatchgtamp symMatches)

Mat ransacTest(const vectorltDMatchgtamp matches

const vectorltKeyPointgt ampkeypoints_1

const vectorltKeyPointgt ampkeypoints_2

vectorltDMatchgtamp outMatches)

----------------------------------------------------------------------------------------------------------------------

FUNCIOacuteNN MATCHING

Mat match(vectorltKeyPointgt keypoints_1 Mat descriptor_1 vectorltKeyPointgt keypoints_2

Mat descriptor_2 vectorltDMatchgt matches bool ransacTest_ = true bool ratioTest_ = true

bool refineF_ = true)

from image 1 to image 2

based on k nearest neighbours (with k=2)

vectorltvectorltDMatchgt gt matches1

matcherknnMatch(descriptor_1descriptor_2 matches1 2)

return 2 nearest neighbours

from image 2 to image 1

based on k nearest neighbours (with k=2)

vectorltvectorltDMatchgt gt matches2

matcherknnMatch(descriptor_2descriptor_1 matches2 2)

return 2 nearest neighbours

int removed=0

for all matches

1 Remove matches for which NN ratio is gt than threshold

if(ratioTest_)

clean image 1 -gt image 2 matches

ratioTest(matches1)

clean image 2 -gt image 1 matches

ratioTest(matches2)

52

2 Remove non-symmetrical matches

vectorltDMatchgt symMatches

symmetryTest(matches1

matches2

symMatches)

3 Validate matches using RANSAC

if(ransacTest_)

refineF = refineF_

F = ransacTest(symMatches keypoints_1 keypoints_2 matches)

else

matches = symMatches

return F

----------------------------------------------------------------------------------------------------------------------

int ratioTest(vectorltvectorltDMatchgt gt ampmatches)

int removed=0

for all matches

for (vectorltvectorltDMatchgt gtiterator matchIterator= matchesbegin()

matchIterator= matchesend() ++matchIterator)

if 2 NN has been identified

if (matchIterator-gtsize() gt 1)

check distance ratio

if ((matchIterator)[0]distance(matchIterator)[1]distance gt ratio)

matchIterator-gtclear() remove match

removed++

else

does not have 2 neighbours

matchIterator-gtclear() remove match

removed++

return removed

----------------------------------------------------------------------------------------------------------------------

Insert symmetrical matches in symMatches vector

void symmetryTest(const vectorltvectorltDMatchgt gtamp matches1 const vectorltvectorltDMatchgt

gtamp matches2vectorltDMatchgtamp symMatches)

for all matches image 1 -gt image 2

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator1= matches1begin()

matchIterator1= matches1end() ++matchIterator1)

ignore deleted matches

if (matchIterator1-gtsize() lt 2)

continue

for all matches image 2 -gt image 1

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator2=

matches2begin() matchIterator2= matches2end() ++matchIterator2)

ignore deleted matches

if (matchIterator2-gtsize() lt 2)

continue

Match symmetry test

if ((matchIterator1)[0]queryIdx ==(matchIterator2)[0]trainIdx ampamp

(matchIterator2)[0]queryIdx == (matchIterator1)[0]trainIdx)

add symmetrical match

symMatchespush_back(DMatch((matchIterator1)[0]queryIdx

(matchIterator1)[0]trainIdx (matchIterator1)[0]distance))

break next match in image 1 -gt image 2

----------------------------------------------------------------------------------------------------------------------

Identify good matches using RANSAC

Return fundemental matrix

Mat ransacTest(const vectorltDMatchgtamp matches const vectorltKeyPointgtamp keypoints_1

const vectorltKeyPointgtamp keypoints_2 vectorltDMatchgtamp outMatches)

Mat nullMat

Convert keypoints into Point2f

vectorltPoint2fgt points1 points2

for (vectorltDMatchgtconst_iterator it= matchesbegin() it= matchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

54

Compute F matrix using RANSAC

if(points1size() lt 10)

return nullMat

vectorltuchargt inliers(points1size()0)

Mat fundemental= findFundamentalMat(Mat(points1)Mat(points2) inliers

CV_FM_RANSAC 30 099) confidence probability

extract the surviving (inliers) matches

vectorltuchargtconst_iterator

itIn= inliersbegin()

vectorltDMatchgtconst_iterator

itM= matchesbegin()

for all matches

for ( itIn= inliersend() ++itIn ++itM)

if (itIn)

it is a valid match

outMatchespush_back(itM)

if (refineF)

The F matrix will be recomputed with

all accepted matches

Convert keypoints into Point2f

for final F computation

points1clear()

points2clear()

for (vectorltDMatchgtconst_iterator it= outMatchesbegin()

it= outMatchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

Compute 8-point F from all accepted matches

if(points1size() lt 9)

return nullMat

fundemental= findFundamentalMat(Mat(points1)Mat(points2)

CV_FM_8POINT)

return F

----------------------------------------------------------------------------------------------------------------------

Callback

void Callback(const ImageConstPtramp msg_rgb

const ImageConstPtramp msg_depth

const CameraInfoConstPtramp cam_info)

cout ltlt lt Callback ltlt endl

Set intrinsic cameras paramenter

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

cv_bridgeCvImageConstPtr cv_ptr_depth

cv_bridgeCvImageConstPtr cv_ptr_rgb

cv_ptr_rgb = cv_bridgetoCvCopy(msg_rgb

sensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth =cv_bridgetoCvCopy(msg_depth

sensor_msgsimage_encodingsTYPE_32FC1)

frameRGB = cv_ptr_rgb-gtimage

if(frameRGBempty())

if (i==0)

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

frameDEPTH1 = cv_ptr_depth-gtimage

sleep(05)

i++

else

frameGray1copyTo(frameGray2)

frameDEPTH1copyTo(frameDEPTH2)

frameDEPTH1 = cv_ptr_depth-gtimage

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

useBriskDetector (frameGray1

frameGray2

frameDEPTH1

fx

fy

cx

cy

matches)

i=0

56

----------------------------------------------------------------------------------------------------------------------

MAIN

int main(int argc char argv)

cout ltlt lt main ltlt endl

rosinit(argc argv image_listener)

rosNodeHandle nh

message_filtersSubscriberltImagegt rgb_sub(nh camerargbimage_raw 1)

message_filtersSubscriberltImagegt depth_sub(nh cameradepth_registeredimage_raw 1)

message_filtersSubscriberltCameraInfogt cam_sub(nh

cameradepth_registeredcamera_info 1)

ifdef APPROXIMATE

typedef sync_policiesApproximateTimeltImage Image CameraInfogt SyncPolicy

endif

ApproximateTime takes a queue size as its constructor argument hence MySyncPolicy(10)

SynchronizerltSyncPolicygt imagesync(SyncPolicy(10) rgb_sub depth_sub cam_sub)

imagesyncregisterCallback(boostbind(ampCallback _1 _2 _3))

rosspin()

return 0

----------------------------------------------------------------------------------------------------------------------

BRISK

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

cout ltlt lt Brisk ltlt endl

Step -1 Define BRISK variables

double t = (double)getTickCount()

vectorltKeyPointgt keypoints_1

keypoints_2

good_matchesclear()

Mat descriptor_1

descriptor_2

Step -2 Define BRISK parameters

int Thresh = 60

int Octave = 4

float PatternScales=10f

BRISK briskDetector(Thresh Octave PatternScales)

Step -3 Create BRISK matcher

briskDetectorcreate(Feature2DBRISK)

Step -4 Detect keypoints using BRISK detector

briskDetectordetect(frameGray1

keypoints_1)

briskDetectordetect(frameGray2

keypoints_2)

Step -5 Calculate descriptors (feature vector)

briskDetectorcompute(frameGray1

keypoints_1

descriptor_1)

briskDetectorcompute(frameGray2

keypoints_2

descriptor_2)

t = ((double)getTickCount() - t)getTickFrequency()

matchermatch(descriptor_1 descriptor_2 matches)

Step -6 Refine Matches

match (keypoints_1

descriptor_1

keypoints_2

descriptor_2

matches)

matches = outMatches

OPTIONAL Step -7 Plot refined matches

cout ltlt lt Drawing correct matches ltlt endl

cout ltlt Brisk Time (seconds) ltlt tltltendl

Mat img_goodmatches

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches img_goodmatches)

imshow( Good Matches BRISK img_goodmatches )

waitKey(30)

vectorltPoint3fgt list_points3d_model

Point3f p

Step -8 Create a list of 3D points

for(int x = 0 x lt frameDEPTH1cols ++x)

for(int y = 0 y lt frameDEPTH1rows ++y)

float d

d = frameDEPTH1atltfloatgt(yx)

if (d gt 0 ampamp d lt 80)

pz = d

px = (float)(x-cx) d (1fx)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

50

----------------------------------------------------------------------------------------------------------------------

VARIABLES GLOBALES

int i=0

k=0

Mat frameRGB

frameDEPTH1

frameDEPTH2

frameGray1

frameGray2

F

Temp_matrix

Temp2_matrix

Temp3_matrix

TempT_matrix

Mat Ra_matrix = Matzeros(3 3 CV_64FC1) Actual rotation matrix

Mat R_matrix = Matzeros(4 3 CV_64FC1)

Mat Rt_matrix = Matzeros(4 4 CV_64FC1) Total rotation matrix

Mat Ta_matrix = Matzeros(3 1 CV_64FC1) Actual translation matrix

Mat T_matrix = Matzeros(4 1 CV_64FC1)

Mat Tt_matrix = Matzeros(4 4 CV_64FC1) Total translation matrix

vectorltPoint2fgt points1

points2

vectorltDMatchgt good_matches

matches

matches1

matches2

symMatches

outMatches

float ratio= 065f max ratio between 1st and 2nd NN

bool refineF= false if true will refine the F matrix

bool ransacTest_= true

bool ratioTest_= false

bool refineF_= false

BFMatcher matcher

----------------------------------------------------------------------------------------------------------------------

DEFINICIOacuteN FUNCIONES

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

Mat match(vectorltKeyPointgt keypoints_1

Mat descriptor_1

vectorltKeyPointgt keypoints_2

Mat descriptor_2

vectorltDMatchgt matches

bool ransacTest_

bool ratioTest_

bool refineF_)

int ratioTest(vectorltvectorltDMatchgt gt ampmatches)

void symmetryTest(const vectorltvectorltDMatchgt gt ampmatches1

const vectorltvectorltDMatchgt gt ampmatches2

vectorltDMatchgtamp symMatches)

Mat ransacTest(const vectorltDMatchgtamp matches

const vectorltKeyPointgt ampkeypoints_1

const vectorltKeyPointgt ampkeypoints_2

vectorltDMatchgtamp outMatches)

----------------------------------------------------------------------------------------------------------------------

FUNCIOacuteNN MATCHING

Mat match(vectorltKeyPointgt keypoints_1 Mat descriptor_1 vectorltKeyPointgt keypoints_2

Mat descriptor_2 vectorltDMatchgt matches bool ransacTest_ = true bool ratioTest_ = true

bool refineF_ = true)

from image 1 to image 2

based on k nearest neighbours (with k=2)

vectorltvectorltDMatchgt gt matches1

matcherknnMatch(descriptor_1descriptor_2 matches1 2)

return 2 nearest neighbours

from image 2 to image 1

based on k nearest neighbours (with k=2)

vectorltvectorltDMatchgt gt matches2

matcherknnMatch(descriptor_2descriptor_1 matches2 2)

return 2 nearest neighbours

int removed=0

for all matches

1 Remove matches for which NN ratio is gt than threshold

if(ratioTest_)

clean image 1 -gt image 2 matches

ratioTest(matches1)

clean image 2 -gt image 1 matches

ratioTest(matches2)

52

2 Remove non-symmetrical matches

vectorltDMatchgt symMatches

symmetryTest(matches1

matches2

symMatches)

3 Validate matches using RANSAC

if(ransacTest_)

refineF = refineF_

F = ransacTest(symMatches keypoints_1 keypoints_2 matches)

else

matches = symMatches

return F

----------------------------------------------------------------------------------------------------------------------

int ratioTest(vectorltvectorltDMatchgt gt ampmatches)

int removed=0

for all matches

for (vectorltvectorltDMatchgt gtiterator matchIterator= matchesbegin()

matchIterator= matchesend() ++matchIterator)

if 2 NN has been identified

if (matchIterator-gtsize() gt 1)

check distance ratio

if ((matchIterator)[0]distance(matchIterator)[1]distance gt ratio)

matchIterator-gtclear() remove match

removed++

else

does not have 2 neighbours

matchIterator-gtclear() remove match

removed++

return removed

----------------------------------------------------------------------------------------------------------------------

Insert symmetrical matches in symMatches vector

void symmetryTest(const vectorltvectorltDMatchgt gtamp matches1 const vectorltvectorltDMatchgt

gtamp matches2vectorltDMatchgtamp symMatches)

for all matches image 1 -gt image 2

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator1= matches1begin()

matchIterator1= matches1end() ++matchIterator1)

ignore deleted matches

if (matchIterator1-gtsize() lt 2)

continue

for all matches image 2 -gt image 1

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator2=

matches2begin() matchIterator2= matches2end() ++matchIterator2)

ignore deleted matches

if (matchIterator2-gtsize() lt 2)

continue

Match symmetry test

if ((matchIterator1)[0]queryIdx ==(matchIterator2)[0]trainIdx ampamp

(matchIterator2)[0]queryIdx == (matchIterator1)[0]trainIdx)

add symmetrical match

symMatchespush_back(DMatch((matchIterator1)[0]queryIdx

(matchIterator1)[0]trainIdx (matchIterator1)[0]distance))

break next match in image 1 -gt image 2

----------------------------------------------------------------------------------------------------------------------

Identify good matches using RANSAC

Return fundemental matrix

Mat ransacTest(const vectorltDMatchgtamp matches const vectorltKeyPointgtamp keypoints_1

const vectorltKeyPointgtamp keypoints_2 vectorltDMatchgtamp outMatches)

Mat nullMat

Convert keypoints into Point2f

vectorltPoint2fgt points1 points2

for (vectorltDMatchgtconst_iterator it= matchesbegin() it= matchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

54

Compute F matrix using RANSAC

if(points1size() lt 10)

return nullMat

vectorltuchargt inliers(points1size()0)

Mat fundemental= findFundamentalMat(Mat(points1)Mat(points2) inliers

CV_FM_RANSAC 30 099) confidence probability

extract the surviving (inliers) matches

vectorltuchargtconst_iterator

itIn= inliersbegin()

vectorltDMatchgtconst_iterator

itM= matchesbegin()

for all matches

for ( itIn= inliersend() ++itIn ++itM)

if (itIn)

it is a valid match

outMatchespush_back(itM)

if (refineF)

The F matrix will be recomputed with

all accepted matches

Convert keypoints into Point2f

for final F computation

points1clear()

points2clear()

for (vectorltDMatchgtconst_iterator it= outMatchesbegin()

it= outMatchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

Compute 8-point F from all accepted matches

if(points1size() lt 9)

return nullMat

fundemental= findFundamentalMat(Mat(points1)Mat(points2)

CV_FM_8POINT)

return F

----------------------------------------------------------------------------------------------------------------------

Callback

void Callback(const ImageConstPtramp msg_rgb

const ImageConstPtramp msg_depth

const CameraInfoConstPtramp cam_info)

cout ltlt lt Callback ltlt endl

Set intrinsic cameras paramenter

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

cv_bridgeCvImageConstPtr cv_ptr_depth

cv_bridgeCvImageConstPtr cv_ptr_rgb

cv_ptr_rgb = cv_bridgetoCvCopy(msg_rgb

sensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth =cv_bridgetoCvCopy(msg_depth

sensor_msgsimage_encodingsTYPE_32FC1)

frameRGB = cv_ptr_rgb-gtimage

if(frameRGBempty())

if (i==0)

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

frameDEPTH1 = cv_ptr_depth-gtimage

sleep(05)

i++

else

frameGray1copyTo(frameGray2)

frameDEPTH1copyTo(frameDEPTH2)

frameDEPTH1 = cv_ptr_depth-gtimage

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

useBriskDetector (frameGray1

frameGray2

frameDEPTH1

fx

fy

cx

cy

matches)

i=0

56

----------------------------------------------------------------------------------------------------------------------

MAIN

int main(int argc char argv)

cout ltlt lt main ltlt endl

rosinit(argc argv image_listener)

rosNodeHandle nh

message_filtersSubscriberltImagegt rgb_sub(nh camerargbimage_raw 1)

message_filtersSubscriberltImagegt depth_sub(nh cameradepth_registeredimage_raw 1)

message_filtersSubscriberltCameraInfogt cam_sub(nh

cameradepth_registeredcamera_info 1)

ifdef APPROXIMATE

typedef sync_policiesApproximateTimeltImage Image CameraInfogt SyncPolicy

endif

ApproximateTime takes a queue size as its constructor argument hence MySyncPolicy(10)

SynchronizerltSyncPolicygt imagesync(SyncPolicy(10) rgb_sub depth_sub cam_sub)

imagesyncregisterCallback(boostbind(ampCallback _1 _2 _3))

rosspin()

return 0

----------------------------------------------------------------------------------------------------------------------

BRISK

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

cout ltlt lt Brisk ltlt endl

Step -1 Define BRISK variables

double t = (double)getTickCount()

vectorltKeyPointgt keypoints_1

keypoints_2

good_matchesclear()

Mat descriptor_1

descriptor_2

Step -2 Define BRISK parameters

int Thresh = 60

int Octave = 4

float PatternScales=10f

BRISK briskDetector(Thresh Octave PatternScales)

Step -3 Create BRISK matcher

briskDetectorcreate(Feature2DBRISK)

Step -4 Detect keypoints using BRISK detector

briskDetectordetect(frameGray1

keypoints_1)

briskDetectordetect(frameGray2

keypoints_2)

Step -5 Calculate descriptors (feature vector)

briskDetectorcompute(frameGray1

keypoints_1

descriptor_1)

briskDetectorcompute(frameGray2

keypoints_2

descriptor_2)

t = ((double)getTickCount() - t)getTickFrequency()

matchermatch(descriptor_1 descriptor_2 matches)

Step -6 Refine Matches

match (keypoints_1

descriptor_1

keypoints_2

descriptor_2

matches)

matches = outMatches

OPTIONAL Step -7 Plot refined matches

cout ltlt lt Drawing correct matches ltlt endl

cout ltlt Brisk Time (seconds) ltlt tltltendl

Mat img_goodmatches

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches img_goodmatches)

imshow( Good Matches BRISK img_goodmatches )

waitKey(30)

vectorltPoint3fgt list_points3d_model

Point3f p

Step -8 Create a list of 3D points

for(int x = 0 x lt frameDEPTH1cols ++x)

for(int y = 0 y lt frameDEPTH1rows ++y)

float d

d = frameDEPTH1atltfloatgt(yx)

if (d gt 0 ampamp d lt 80)

pz = d

px = (float)(x-cx) d (1fx)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

Mat match(vectorltKeyPointgt keypoints_1

Mat descriptor_1

vectorltKeyPointgt keypoints_2

Mat descriptor_2

vectorltDMatchgt matches

bool ransacTest_

bool ratioTest_

bool refineF_)

int ratioTest(vectorltvectorltDMatchgt gt ampmatches)

void symmetryTest(const vectorltvectorltDMatchgt gt ampmatches1

const vectorltvectorltDMatchgt gt ampmatches2

vectorltDMatchgtamp symMatches)

Mat ransacTest(const vectorltDMatchgtamp matches

const vectorltKeyPointgt ampkeypoints_1

const vectorltKeyPointgt ampkeypoints_2

vectorltDMatchgtamp outMatches)

----------------------------------------------------------------------------------------------------------------------

FUNCIOacuteNN MATCHING

Mat match(vectorltKeyPointgt keypoints_1 Mat descriptor_1 vectorltKeyPointgt keypoints_2

Mat descriptor_2 vectorltDMatchgt matches bool ransacTest_ = true bool ratioTest_ = true

bool refineF_ = true)

from image 1 to image 2

based on k nearest neighbours (with k=2)

vectorltvectorltDMatchgt gt matches1

matcherknnMatch(descriptor_1descriptor_2 matches1 2)

return 2 nearest neighbours

from image 2 to image 1

based on k nearest neighbours (with k=2)

vectorltvectorltDMatchgt gt matches2

matcherknnMatch(descriptor_2descriptor_1 matches2 2)

return 2 nearest neighbours

int removed=0

for all matches

1 Remove matches for which NN ratio is gt than threshold

if(ratioTest_)

clean image 1 -gt image 2 matches

ratioTest(matches1)

clean image 2 -gt image 1 matches

ratioTest(matches2)

52

2 Remove non-symmetrical matches

vectorltDMatchgt symMatches

symmetryTest(matches1

matches2

symMatches)

3 Validate matches using RANSAC

if(ransacTest_)

refineF = refineF_

F = ransacTest(symMatches keypoints_1 keypoints_2 matches)

else

matches = symMatches

return F

----------------------------------------------------------------------------------------------------------------------

int ratioTest(vectorltvectorltDMatchgt gt ampmatches)

int removed=0

for all matches

for (vectorltvectorltDMatchgt gtiterator matchIterator= matchesbegin()

matchIterator= matchesend() ++matchIterator)

if 2 NN has been identified

if (matchIterator-gtsize() gt 1)

check distance ratio

if ((matchIterator)[0]distance(matchIterator)[1]distance gt ratio)

matchIterator-gtclear() remove match

removed++

else

does not have 2 neighbours

matchIterator-gtclear() remove match

removed++

return removed

----------------------------------------------------------------------------------------------------------------------

Insert symmetrical matches in symMatches vector

void symmetryTest(const vectorltvectorltDMatchgt gtamp matches1 const vectorltvectorltDMatchgt

gtamp matches2vectorltDMatchgtamp symMatches)

for all matches image 1 -gt image 2

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator1= matches1begin()

matchIterator1= matches1end() ++matchIterator1)

ignore deleted matches

if (matchIterator1-gtsize() lt 2)

continue

for all matches image 2 -gt image 1

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator2=

matches2begin() matchIterator2= matches2end() ++matchIterator2)

ignore deleted matches

if (matchIterator2-gtsize() lt 2)

continue

Match symmetry test

if ((matchIterator1)[0]queryIdx ==(matchIterator2)[0]trainIdx ampamp

(matchIterator2)[0]queryIdx == (matchIterator1)[0]trainIdx)

add symmetrical match

symMatchespush_back(DMatch((matchIterator1)[0]queryIdx

(matchIterator1)[0]trainIdx (matchIterator1)[0]distance))

break next match in image 1 -gt image 2

----------------------------------------------------------------------------------------------------------------------

Identify good matches using RANSAC

Return fundemental matrix

Mat ransacTest(const vectorltDMatchgtamp matches const vectorltKeyPointgtamp keypoints_1

const vectorltKeyPointgtamp keypoints_2 vectorltDMatchgtamp outMatches)

Mat nullMat

Convert keypoints into Point2f

vectorltPoint2fgt points1 points2

for (vectorltDMatchgtconst_iterator it= matchesbegin() it= matchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

54

Compute F matrix using RANSAC

if(points1size() lt 10)

return nullMat

vectorltuchargt inliers(points1size()0)

Mat fundemental= findFundamentalMat(Mat(points1)Mat(points2) inliers

CV_FM_RANSAC 30 099) confidence probability

extract the surviving (inliers) matches

vectorltuchargtconst_iterator

itIn= inliersbegin()

vectorltDMatchgtconst_iterator

itM= matchesbegin()

for all matches

for ( itIn= inliersend() ++itIn ++itM)

if (itIn)

it is a valid match

outMatchespush_back(itM)

if (refineF)

The F matrix will be recomputed with

all accepted matches

Convert keypoints into Point2f

for final F computation

points1clear()

points2clear()

for (vectorltDMatchgtconst_iterator it= outMatchesbegin()

it= outMatchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

Compute 8-point F from all accepted matches

if(points1size() lt 9)

return nullMat

fundemental= findFundamentalMat(Mat(points1)Mat(points2)

CV_FM_8POINT)

return F

----------------------------------------------------------------------------------------------------------------------

Callback

void Callback(const ImageConstPtramp msg_rgb

const ImageConstPtramp msg_depth

const CameraInfoConstPtramp cam_info)

cout ltlt lt Callback ltlt endl

Set intrinsic cameras paramenter

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

cv_bridgeCvImageConstPtr cv_ptr_depth

cv_bridgeCvImageConstPtr cv_ptr_rgb

cv_ptr_rgb = cv_bridgetoCvCopy(msg_rgb

sensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth =cv_bridgetoCvCopy(msg_depth

sensor_msgsimage_encodingsTYPE_32FC1)

frameRGB = cv_ptr_rgb-gtimage

if(frameRGBempty())

if (i==0)

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

frameDEPTH1 = cv_ptr_depth-gtimage

sleep(05)

i++

else

frameGray1copyTo(frameGray2)

frameDEPTH1copyTo(frameDEPTH2)

frameDEPTH1 = cv_ptr_depth-gtimage

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

useBriskDetector (frameGray1

frameGray2

frameDEPTH1

fx

fy

cx

cy

matches)

i=0

56

----------------------------------------------------------------------------------------------------------------------

MAIN

int main(int argc char argv)

cout ltlt lt main ltlt endl

rosinit(argc argv image_listener)

rosNodeHandle nh

message_filtersSubscriberltImagegt rgb_sub(nh camerargbimage_raw 1)

message_filtersSubscriberltImagegt depth_sub(nh cameradepth_registeredimage_raw 1)

message_filtersSubscriberltCameraInfogt cam_sub(nh

cameradepth_registeredcamera_info 1)

ifdef APPROXIMATE

typedef sync_policiesApproximateTimeltImage Image CameraInfogt SyncPolicy

endif

ApproximateTime takes a queue size as its constructor argument hence MySyncPolicy(10)

SynchronizerltSyncPolicygt imagesync(SyncPolicy(10) rgb_sub depth_sub cam_sub)

imagesyncregisterCallback(boostbind(ampCallback _1 _2 _3))

rosspin()

return 0

----------------------------------------------------------------------------------------------------------------------

BRISK

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

cout ltlt lt Brisk ltlt endl

Step -1 Define BRISK variables

double t = (double)getTickCount()

vectorltKeyPointgt keypoints_1

keypoints_2

good_matchesclear()

Mat descriptor_1

descriptor_2

Step -2 Define BRISK parameters

int Thresh = 60

int Octave = 4

float PatternScales=10f

BRISK briskDetector(Thresh Octave PatternScales)

Step -3 Create BRISK matcher

briskDetectorcreate(Feature2DBRISK)

Step -4 Detect keypoints using BRISK detector

briskDetectordetect(frameGray1

keypoints_1)

briskDetectordetect(frameGray2

keypoints_2)

Step -5 Calculate descriptors (feature vector)

briskDetectorcompute(frameGray1

keypoints_1

descriptor_1)

briskDetectorcompute(frameGray2

keypoints_2

descriptor_2)

t = ((double)getTickCount() - t)getTickFrequency()

matchermatch(descriptor_1 descriptor_2 matches)

Step -6 Refine Matches

match (keypoints_1

descriptor_1

keypoints_2

descriptor_2

matches)

matches = outMatches

OPTIONAL Step -7 Plot refined matches

cout ltlt lt Drawing correct matches ltlt endl

cout ltlt Brisk Time (seconds) ltlt tltltendl

Mat img_goodmatches

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches img_goodmatches)

imshow( Good Matches BRISK img_goodmatches )

waitKey(30)

vectorltPoint3fgt list_points3d_model

Point3f p

Step -8 Create a list of 3D points

for(int x = 0 x lt frameDEPTH1cols ++x)

for(int y = 0 y lt frameDEPTH1rows ++y)

float d

d = frameDEPTH1atltfloatgt(yx)

if (d gt 0 ampamp d lt 80)

pz = d

px = (float)(x-cx) d (1fx)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

52

2 Remove non-symmetrical matches

vectorltDMatchgt symMatches

symmetryTest(matches1

matches2

symMatches)

3 Validate matches using RANSAC

if(ransacTest_)

refineF = refineF_

F = ransacTest(symMatches keypoints_1 keypoints_2 matches)

else

matches = symMatches

return F

----------------------------------------------------------------------------------------------------------------------

int ratioTest(vectorltvectorltDMatchgt gt ampmatches)

int removed=0

for all matches

for (vectorltvectorltDMatchgt gtiterator matchIterator= matchesbegin()

matchIterator= matchesend() ++matchIterator)

if 2 NN has been identified

if (matchIterator-gtsize() gt 1)

check distance ratio

if ((matchIterator)[0]distance(matchIterator)[1]distance gt ratio)

matchIterator-gtclear() remove match

removed++

else

does not have 2 neighbours

matchIterator-gtclear() remove match

removed++

return removed

----------------------------------------------------------------------------------------------------------------------

Insert symmetrical matches in symMatches vector

void symmetryTest(const vectorltvectorltDMatchgt gtamp matches1 const vectorltvectorltDMatchgt

gtamp matches2vectorltDMatchgtamp symMatches)

for all matches image 1 -gt image 2

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator1= matches1begin()

matchIterator1= matches1end() ++matchIterator1)

ignore deleted matches

if (matchIterator1-gtsize() lt 2)

continue

for all matches image 2 -gt image 1

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator2=

matches2begin() matchIterator2= matches2end() ++matchIterator2)

ignore deleted matches

if (matchIterator2-gtsize() lt 2)

continue

Match symmetry test

if ((matchIterator1)[0]queryIdx ==(matchIterator2)[0]trainIdx ampamp

(matchIterator2)[0]queryIdx == (matchIterator1)[0]trainIdx)

add symmetrical match

symMatchespush_back(DMatch((matchIterator1)[0]queryIdx

(matchIterator1)[0]trainIdx (matchIterator1)[0]distance))

break next match in image 1 -gt image 2

----------------------------------------------------------------------------------------------------------------------

Identify good matches using RANSAC

Return fundemental matrix

Mat ransacTest(const vectorltDMatchgtamp matches const vectorltKeyPointgtamp keypoints_1

const vectorltKeyPointgtamp keypoints_2 vectorltDMatchgtamp outMatches)

Mat nullMat

Convert keypoints into Point2f

vectorltPoint2fgt points1 points2

for (vectorltDMatchgtconst_iterator it= matchesbegin() it= matchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

54

Compute F matrix using RANSAC

if(points1size() lt 10)

return nullMat

vectorltuchargt inliers(points1size()0)

Mat fundemental= findFundamentalMat(Mat(points1)Mat(points2) inliers

CV_FM_RANSAC 30 099) confidence probability

extract the surviving (inliers) matches

vectorltuchargtconst_iterator

itIn= inliersbegin()

vectorltDMatchgtconst_iterator

itM= matchesbegin()

for all matches

for ( itIn= inliersend() ++itIn ++itM)

if (itIn)

it is a valid match

outMatchespush_back(itM)

if (refineF)

The F matrix will be recomputed with

all accepted matches

Convert keypoints into Point2f

for final F computation

points1clear()

points2clear()

for (vectorltDMatchgtconst_iterator it= outMatchesbegin()

it= outMatchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

Compute 8-point F from all accepted matches

if(points1size() lt 9)

return nullMat

fundemental= findFundamentalMat(Mat(points1)Mat(points2)

CV_FM_8POINT)

return F

----------------------------------------------------------------------------------------------------------------------

Callback

void Callback(const ImageConstPtramp msg_rgb

const ImageConstPtramp msg_depth

const CameraInfoConstPtramp cam_info)

cout ltlt lt Callback ltlt endl

Set intrinsic cameras paramenter

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

cv_bridgeCvImageConstPtr cv_ptr_depth

cv_bridgeCvImageConstPtr cv_ptr_rgb

cv_ptr_rgb = cv_bridgetoCvCopy(msg_rgb

sensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth =cv_bridgetoCvCopy(msg_depth

sensor_msgsimage_encodingsTYPE_32FC1)

frameRGB = cv_ptr_rgb-gtimage

if(frameRGBempty())

if (i==0)

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

frameDEPTH1 = cv_ptr_depth-gtimage

sleep(05)

i++

else

frameGray1copyTo(frameGray2)

frameDEPTH1copyTo(frameDEPTH2)

frameDEPTH1 = cv_ptr_depth-gtimage

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

useBriskDetector (frameGray1

frameGray2

frameDEPTH1

fx

fy

cx

cy

matches)

i=0

56

----------------------------------------------------------------------------------------------------------------------

MAIN

int main(int argc char argv)

cout ltlt lt main ltlt endl

rosinit(argc argv image_listener)

rosNodeHandle nh

message_filtersSubscriberltImagegt rgb_sub(nh camerargbimage_raw 1)

message_filtersSubscriberltImagegt depth_sub(nh cameradepth_registeredimage_raw 1)

message_filtersSubscriberltCameraInfogt cam_sub(nh

cameradepth_registeredcamera_info 1)

ifdef APPROXIMATE

typedef sync_policiesApproximateTimeltImage Image CameraInfogt SyncPolicy

endif

ApproximateTime takes a queue size as its constructor argument hence MySyncPolicy(10)

SynchronizerltSyncPolicygt imagesync(SyncPolicy(10) rgb_sub depth_sub cam_sub)

imagesyncregisterCallback(boostbind(ampCallback _1 _2 _3))

rosspin()

return 0

----------------------------------------------------------------------------------------------------------------------

BRISK

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

cout ltlt lt Brisk ltlt endl

Step -1 Define BRISK variables

double t = (double)getTickCount()

vectorltKeyPointgt keypoints_1

keypoints_2

good_matchesclear()

Mat descriptor_1

descriptor_2

Step -2 Define BRISK parameters

int Thresh = 60

int Octave = 4

float PatternScales=10f

BRISK briskDetector(Thresh Octave PatternScales)

Step -3 Create BRISK matcher

briskDetectorcreate(Feature2DBRISK)

Step -4 Detect keypoints using BRISK detector

briskDetectordetect(frameGray1

keypoints_1)

briskDetectordetect(frameGray2

keypoints_2)

Step -5 Calculate descriptors (feature vector)

briskDetectorcompute(frameGray1

keypoints_1

descriptor_1)

briskDetectorcompute(frameGray2

keypoints_2

descriptor_2)

t = ((double)getTickCount() - t)getTickFrequency()

matchermatch(descriptor_1 descriptor_2 matches)

Step -6 Refine Matches

match (keypoints_1

descriptor_1

keypoints_2

descriptor_2

matches)

matches = outMatches

OPTIONAL Step -7 Plot refined matches

cout ltlt lt Drawing correct matches ltlt endl

cout ltlt Brisk Time (seconds) ltlt tltltendl

Mat img_goodmatches

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches img_goodmatches)

imshow( Good Matches BRISK img_goodmatches )

waitKey(30)

vectorltPoint3fgt list_points3d_model

Point3f p

Step -8 Create a list of 3D points

for(int x = 0 x lt frameDEPTH1cols ++x)

for(int y = 0 y lt frameDEPTH1rows ++y)

float d

d = frameDEPTH1atltfloatgt(yx)

if (d gt 0 ampamp d lt 80)

pz = d

px = (float)(x-cx) d (1fx)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

----------------------------------------------------------------------------------------------------------------------

Insert symmetrical matches in symMatches vector

void symmetryTest(const vectorltvectorltDMatchgt gtamp matches1 const vectorltvectorltDMatchgt

gtamp matches2vectorltDMatchgtamp symMatches)

for all matches image 1 -gt image 2

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator1= matches1begin()

matchIterator1= matches1end() ++matchIterator1)

ignore deleted matches

if (matchIterator1-gtsize() lt 2)

continue

for all matches image 2 -gt image 1

for (vectorltvectorltDMatchgt gtconst_iterator matchIterator2=

matches2begin() matchIterator2= matches2end() ++matchIterator2)

ignore deleted matches

if (matchIterator2-gtsize() lt 2)

continue

Match symmetry test

if ((matchIterator1)[0]queryIdx ==(matchIterator2)[0]trainIdx ampamp

(matchIterator2)[0]queryIdx == (matchIterator1)[0]trainIdx)

add symmetrical match

symMatchespush_back(DMatch((matchIterator1)[0]queryIdx

(matchIterator1)[0]trainIdx (matchIterator1)[0]distance))

break next match in image 1 -gt image 2

----------------------------------------------------------------------------------------------------------------------

Identify good matches using RANSAC

Return fundemental matrix

Mat ransacTest(const vectorltDMatchgtamp matches const vectorltKeyPointgtamp keypoints_1

const vectorltKeyPointgtamp keypoints_2 vectorltDMatchgtamp outMatches)

Mat nullMat

Convert keypoints into Point2f

vectorltPoint2fgt points1 points2

for (vectorltDMatchgtconst_iterator it= matchesbegin() it= matchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

54

Compute F matrix using RANSAC

if(points1size() lt 10)

return nullMat

vectorltuchargt inliers(points1size()0)

Mat fundemental= findFundamentalMat(Mat(points1)Mat(points2) inliers

CV_FM_RANSAC 30 099) confidence probability

extract the surviving (inliers) matches

vectorltuchargtconst_iterator

itIn= inliersbegin()

vectorltDMatchgtconst_iterator

itM= matchesbegin()

for all matches

for ( itIn= inliersend() ++itIn ++itM)

if (itIn)

it is a valid match

outMatchespush_back(itM)

if (refineF)

The F matrix will be recomputed with

all accepted matches

Convert keypoints into Point2f

for final F computation

points1clear()

points2clear()

for (vectorltDMatchgtconst_iterator it= outMatchesbegin()

it= outMatchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

Compute 8-point F from all accepted matches

if(points1size() lt 9)

return nullMat

fundemental= findFundamentalMat(Mat(points1)Mat(points2)

CV_FM_8POINT)

return F

----------------------------------------------------------------------------------------------------------------------

Callback

void Callback(const ImageConstPtramp msg_rgb

const ImageConstPtramp msg_depth

const CameraInfoConstPtramp cam_info)

cout ltlt lt Callback ltlt endl

Set intrinsic cameras paramenter

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

cv_bridgeCvImageConstPtr cv_ptr_depth

cv_bridgeCvImageConstPtr cv_ptr_rgb

cv_ptr_rgb = cv_bridgetoCvCopy(msg_rgb

sensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth =cv_bridgetoCvCopy(msg_depth

sensor_msgsimage_encodingsTYPE_32FC1)

frameRGB = cv_ptr_rgb-gtimage

if(frameRGBempty())

if (i==0)

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

frameDEPTH1 = cv_ptr_depth-gtimage

sleep(05)

i++

else

frameGray1copyTo(frameGray2)

frameDEPTH1copyTo(frameDEPTH2)

frameDEPTH1 = cv_ptr_depth-gtimage

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

useBriskDetector (frameGray1

frameGray2

frameDEPTH1

fx

fy

cx

cy

matches)

i=0

56

----------------------------------------------------------------------------------------------------------------------

MAIN

int main(int argc char argv)

cout ltlt lt main ltlt endl

rosinit(argc argv image_listener)

rosNodeHandle nh

message_filtersSubscriberltImagegt rgb_sub(nh camerargbimage_raw 1)

message_filtersSubscriberltImagegt depth_sub(nh cameradepth_registeredimage_raw 1)

message_filtersSubscriberltCameraInfogt cam_sub(nh

cameradepth_registeredcamera_info 1)

ifdef APPROXIMATE

typedef sync_policiesApproximateTimeltImage Image CameraInfogt SyncPolicy

endif

ApproximateTime takes a queue size as its constructor argument hence MySyncPolicy(10)

SynchronizerltSyncPolicygt imagesync(SyncPolicy(10) rgb_sub depth_sub cam_sub)

imagesyncregisterCallback(boostbind(ampCallback _1 _2 _3))

rosspin()

return 0

----------------------------------------------------------------------------------------------------------------------

BRISK

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

cout ltlt lt Brisk ltlt endl

Step -1 Define BRISK variables

double t = (double)getTickCount()

vectorltKeyPointgt keypoints_1

keypoints_2

good_matchesclear()

Mat descriptor_1

descriptor_2

Step -2 Define BRISK parameters

int Thresh = 60

int Octave = 4

float PatternScales=10f

BRISK briskDetector(Thresh Octave PatternScales)

Step -3 Create BRISK matcher

briskDetectorcreate(Feature2DBRISK)

Step -4 Detect keypoints using BRISK detector

briskDetectordetect(frameGray1

keypoints_1)

briskDetectordetect(frameGray2

keypoints_2)

Step -5 Calculate descriptors (feature vector)

briskDetectorcompute(frameGray1

keypoints_1

descriptor_1)

briskDetectorcompute(frameGray2

keypoints_2

descriptor_2)

t = ((double)getTickCount() - t)getTickFrequency()

matchermatch(descriptor_1 descriptor_2 matches)

Step -6 Refine Matches

match (keypoints_1

descriptor_1

keypoints_2

descriptor_2

matches)

matches = outMatches

OPTIONAL Step -7 Plot refined matches

cout ltlt lt Drawing correct matches ltlt endl

cout ltlt Brisk Time (seconds) ltlt tltltendl

Mat img_goodmatches

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches img_goodmatches)

imshow( Good Matches BRISK img_goodmatches )

waitKey(30)

vectorltPoint3fgt list_points3d_model

Point3f p

Step -8 Create a list of 3D points

for(int x = 0 x lt frameDEPTH1cols ++x)

for(int y = 0 y lt frameDEPTH1rows ++y)

float d

d = frameDEPTH1atltfloatgt(yx)

if (d gt 0 ampamp d lt 80)

pz = d

px = (float)(x-cx) d (1fx)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

54

Compute F matrix using RANSAC

if(points1size() lt 10)

return nullMat

vectorltuchargt inliers(points1size()0)

Mat fundemental= findFundamentalMat(Mat(points1)Mat(points2) inliers

CV_FM_RANSAC 30 099) confidence probability

extract the surviving (inliers) matches

vectorltuchargtconst_iterator

itIn= inliersbegin()

vectorltDMatchgtconst_iterator

itM= matchesbegin()

for all matches

for ( itIn= inliersend() ++itIn ++itM)

if (itIn)

it is a valid match

outMatchespush_back(itM)

if (refineF)

The F matrix will be recomputed with

all accepted matches

Convert keypoints into Point2f

for final F computation

points1clear()

points2clear()

for (vectorltDMatchgtconst_iterator it= outMatchesbegin()

it= outMatchesend() ++it)

Get the position of left keypoints

float x= keypoints_1[it-gtqueryIdx]ptx

float y= keypoints_1[it-gtqueryIdx]pty

points1push_back(Point2f(xy))

Get the position of right keypoints

x= keypoints_2[it-gttrainIdx]ptx

y= keypoints_2[it-gttrainIdx]pty

points2push_back(Point2f(xy))

Compute 8-point F from all accepted matches

if(points1size() lt 9)

return nullMat

fundemental= findFundamentalMat(Mat(points1)Mat(points2)

CV_FM_8POINT)

return F

----------------------------------------------------------------------------------------------------------------------

Callback

void Callback(const ImageConstPtramp msg_rgb

const ImageConstPtramp msg_depth

const CameraInfoConstPtramp cam_info)

cout ltlt lt Callback ltlt endl

Set intrinsic cameras paramenter

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

cv_bridgeCvImageConstPtr cv_ptr_depth

cv_bridgeCvImageConstPtr cv_ptr_rgb

cv_ptr_rgb = cv_bridgetoCvCopy(msg_rgb

sensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth =cv_bridgetoCvCopy(msg_depth

sensor_msgsimage_encodingsTYPE_32FC1)

frameRGB = cv_ptr_rgb-gtimage

if(frameRGBempty())

if (i==0)

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

frameDEPTH1 = cv_ptr_depth-gtimage

sleep(05)

i++

else

frameGray1copyTo(frameGray2)

frameDEPTH1copyTo(frameDEPTH2)

frameDEPTH1 = cv_ptr_depth-gtimage

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

useBriskDetector (frameGray1

frameGray2

frameDEPTH1

fx

fy

cx

cy

matches)

i=0

56

----------------------------------------------------------------------------------------------------------------------

MAIN

int main(int argc char argv)

cout ltlt lt main ltlt endl

rosinit(argc argv image_listener)

rosNodeHandle nh

message_filtersSubscriberltImagegt rgb_sub(nh camerargbimage_raw 1)

message_filtersSubscriberltImagegt depth_sub(nh cameradepth_registeredimage_raw 1)

message_filtersSubscriberltCameraInfogt cam_sub(nh

cameradepth_registeredcamera_info 1)

ifdef APPROXIMATE

typedef sync_policiesApproximateTimeltImage Image CameraInfogt SyncPolicy

endif

ApproximateTime takes a queue size as its constructor argument hence MySyncPolicy(10)

SynchronizerltSyncPolicygt imagesync(SyncPolicy(10) rgb_sub depth_sub cam_sub)

imagesyncregisterCallback(boostbind(ampCallback _1 _2 _3))

rosspin()

return 0

----------------------------------------------------------------------------------------------------------------------

BRISK

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

cout ltlt lt Brisk ltlt endl

Step -1 Define BRISK variables

double t = (double)getTickCount()

vectorltKeyPointgt keypoints_1

keypoints_2

good_matchesclear()

Mat descriptor_1

descriptor_2

Step -2 Define BRISK parameters

int Thresh = 60

int Octave = 4

float PatternScales=10f

BRISK briskDetector(Thresh Octave PatternScales)

Step -3 Create BRISK matcher

briskDetectorcreate(Feature2DBRISK)

Step -4 Detect keypoints using BRISK detector

briskDetectordetect(frameGray1

keypoints_1)

briskDetectordetect(frameGray2

keypoints_2)

Step -5 Calculate descriptors (feature vector)

briskDetectorcompute(frameGray1

keypoints_1

descriptor_1)

briskDetectorcompute(frameGray2

keypoints_2

descriptor_2)

t = ((double)getTickCount() - t)getTickFrequency()

matchermatch(descriptor_1 descriptor_2 matches)

Step -6 Refine Matches

match (keypoints_1

descriptor_1

keypoints_2

descriptor_2

matches)

matches = outMatches

OPTIONAL Step -7 Plot refined matches

cout ltlt lt Drawing correct matches ltlt endl

cout ltlt Brisk Time (seconds) ltlt tltltendl

Mat img_goodmatches

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches img_goodmatches)

imshow( Good Matches BRISK img_goodmatches )

waitKey(30)

vectorltPoint3fgt list_points3d_model

Point3f p

Step -8 Create a list of 3D points

for(int x = 0 x lt frameDEPTH1cols ++x)

for(int y = 0 y lt frameDEPTH1rows ++y)

float d

d = frameDEPTH1atltfloatgt(yx)

if (d gt 0 ampamp d lt 80)

pz = d

px = (float)(x-cx) d (1fx)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

----------------------------------------------------------------------------------------------------------------------

Callback

void Callback(const ImageConstPtramp msg_rgb

const ImageConstPtramp msg_depth

const CameraInfoConstPtramp cam_info)

cout ltlt lt Callback ltlt endl

Set intrinsic cameras paramenter

double fx = cam_info-gtK[0]

double fy = cam_info-gtK[4]

double cx = cam_info-gtK[2]

double cy = cam_info-gtK[5]

cv_bridgeCvImageConstPtr cv_ptr_depth

cv_bridgeCvImageConstPtr cv_ptr_rgb

cv_ptr_rgb = cv_bridgetoCvCopy(msg_rgb

sensor_msgsimage_encodingsTYPE_8UC3)

cv_ptr_depth =cv_bridgetoCvCopy(msg_depth

sensor_msgsimage_encodingsTYPE_32FC1)

frameRGB = cv_ptr_rgb-gtimage

if(frameRGBempty())

if (i==0)

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

frameDEPTH1 = cv_ptr_depth-gtimage

sleep(05)

i++

else

frameGray1copyTo(frameGray2)

frameDEPTH1copyTo(frameDEPTH2)

frameDEPTH1 = cv_ptr_depth-gtimage

cvtColor(frameRGB

frameGray1

CV_BGR2GRAY)

useBriskDetector (frameGray1

frameGray2

frameDEPTH1

fx

fy

cx

cy

matches)

i=0

56

----------------------------------------------------------------------------------------------------------------------

MAIN

int main(int argc char argv)

cout ltlt lt main ltlt endl

rosinit(argc argv image_listener)

rosNodeHandle nh

message_filtersSubscriberltImagegt rgb_sub(nh camerargbimage_raw 1)

message_filtersSubscriberltImagegt depth_sub(nh cameradepth_registeredimage_raw 1)

message_filtersSubscriberltCameraInfogt cam_sub(nh

cameradepth_registeredcamera_info 1)

ifdef APPROXIMATE

typedef sync_policiesApproximateTimeltImage Image CameraInfogt SyncPolicy

endif

ApproximateTime takes a queue size as its constructor argument hence MySyncPolicy(10)

SynchronizerltSyncPolicygt imagesync(SyncPolicy(10) rgb_sub depth_sub cam_sub)

imagesyncregisterCallback(boostbind(ampCallback _1 _2 _3))

rosspin()

return 0

----------------------------------------------------------------------------------------------------------------------

BRISK

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

cout ltlt lt Brisk ltlt endl

Step -1 Define BRISK variables

double t = (double)getTickCount()

vectorltKeyPointgt keypoints_1

keypoints_2

good_matchesclear()

Mat descriptor_1

descriptor_2

Step -2 Define BRISK parameters

int Thresh = 60

int Octave = 4

float PatternScales=10f

BRISK briskDetector(Thresh Octave PatternScales)

Step -3 Create BRISK matcher

briskDetectorcreate(Feature2DBRISK)

Step -4 Detect keypoints using BRISK detector

briskDetectordetect(frameGray1

keypoints_1)

briskDetectordetect(frameGray2

keypoints_2)

Step -5 Calculate descriptors (feature vector)

briskDetectorcompute(frameGray1

keypoints_1

descriptor_1)

briskDetectorcompute(frameGray2

keypoints_2

descriptor_2)

t = ((double)getTickCount() - t)getTickFrequency()

matchermatch(descriptor_1 descriptor_2 matches)

Step -6 Refine Matches

match (keypoints_1

descriptor_1

keypoints_2

descriptor_2

matches)

matches = outMatches

OPTIONAL Step -7 Plot refined matches

cout ltlt lt Drawing correct matches ltlt endl

cout ltlt Brisk Time (seconds) ltlt tltltendl

Mat img_goodmatches

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches img_goodmatches)

imshow( Good Matches BRISK img_goodmatches )

waitKey(30)

vectorltPoint3fgt list_points3d_model

Point3f p

Step -8 Create a list of 3D points

for(int x = 0 x lt frameDEPTH1cols ++x)

for(int y = 0 y lt frameDEPTH1rows ++y)

float d

d = frameDEPTH1atltfloatgt(yx)

if (d gt 0 ampamp d lt 80)

pz = d

px = (float)(x-cx) d (1fx)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

56

----------------------------------------------------------------------------------------------------------------------

MAIN

int main(int argc char argv)

cout ltlt lt main ltlt endl

rosinit(argc argv image_listener)

rosNodeHandle nh

message_filtersSubscriberltImagegt rgb_sub(nh camerargbimage_raw 1)

message_filtersSubscriberltImagegt depth_sub(nh cameradepth_registeredimage_raw 1)

message_filtersSubscriberltCameraInfogt cam_sub(nh

cameradepth_registeredcamera_info 1)

ifdef APPROXIMATE

typedef sync_policiesApproximateTimeltImage Image CameraInfogt SyncPolicy

endif

ApproximateTime takes a queue size as its constructor argument hence MySyncPolicy(10)

SynchronizerltSyncPolicygt imagesync(SyncPolicy(10) rgb_sub depth_sub cam_sub)

imagesyncregisterCallback(boostbind(ampCallback _1 _2 _3))

rosspin()

return 0

----------------------------------------------------------------------------------------------------------------------

BRISK

void useBriskDetector(Mat frameGray1

Mat frameGray2

Mat frameDEPTH1

double fx

double fy

double cx

double cy

vectorltDMatchgt matches)

cout ltlt lt Brisk ltlt endl

Step -1 Define BRISK variables

double t = (double)getTickCount()

vectorltKeyPointgt keypoints_1

keypoints_2

good_matchesclear()

Mat descriptor_1

descriptor_2

Step -2 Define BRISK parameters

int Thresh = 60

int Octave = 4

float PatternScales=10f

BRISK briskDetector(Thresh Octave PatternScales)

Step -3 Create BRISK matcher

briskDetectorcreate(Feature2DBRISK)

Step -4 Detect keypoints using BRISK detector

briskDetectordetect(frameGray1

keypoints_1)

briskDetectordetect(frameGray2

keypoints_2)

Step -5 Calculate descriptors (feature vector)

briskDetectorcompute(frameGray1

keypoints_1

descriptor_1)

briskDetectorcompute(frameGray2

keypoints_2

descriptor_2)

t = ((double)getTickCount() - t)getTickFrequency()

matchermatch(descriptor_1 descriptor_2 matches)

Step -6 Refine Matches

match (keypoints_1

descriptor_1

keypoints_2

descriptor_2

matches)

matches = outMatches

OPTIONAL Step -7 Plot refined matches

cout ltlt lt Drawing correct matches ltlt endl

cout ltlt Brisk Time (seconds) ltlt tltltendl

Mat img_goodmatches

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches img_goodmatches)

imshow( Good Matches BRISK img_goodmatches )

waitKey(30)

vectorltPoint3fgt list_points3d_model

Point3f p

Step -8 Create a list of 3D points

for(int x = 0 x lt frameDEPTH1cols ++x)

for(int y = 0 y lt frameDEPTH1rows ++y)

float d

d = frameDEPTH1atltfloatgt(yx)

if (d gt 0 ampamp d lt 80)

pz = d

px = (float)(x-cx) d (1fx)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

Step -3 Create BRISK matcher

briskDetectorcreate(Feature2DBRISK)

Step -4 Detect keypoints using BRISK detector

briskDetectordetect(frameGray1

keypoints_1)

briskDetectordetect(frameGray2

keypoints_2)

Step -5 Calculate descriptors (feature vector)

briskDetectorcompute(frameGray1

keypoints_1

descriptor_1)

briskDetectorcompute(frameGray2

keypoints_2

descriptor_2)

t = ((double)getTickCount() - t)getTickFrequency()

matchermatch(descriptor_1 descriptor_2 matches)

Step -6 Refine Matches

match (keypoints_1

descriptor_1

keypoints_2

descriptor_2

matches)

matches = outMatches

OPTIONAL Step -7 Plot refined matches

cout ltlt lt Drawing correct matches ltlt endl

cout ltlt Brisk Time (seconds) ltlt tltltendl

Mat img_goodmatches

drawMatches(frameGray1 keypoints_1 frameGray2 keypoints_2 matches img_goodmatches)

imshow( Good Matches BRISK img_goodmatches )

waitKey(30)

vectorltPoint3fgt list_points3d_model

Point3f p

Step -8 Create a list of 3D points

for(int x = 0 x lt frameDEPTH1cols ++x)

for(int y = 0 y lt frameDEPTH1rows ++y)

float d

d = frameDEPTH1atltfloatgt(yx)

if (d gt 0 ampamp d lt 80)

pz = d

px = (float)(x-cx) d (1fx)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

58

py = (float)(y-cy) d (1fy)

list_points3d_modelpush_back(Point3f(p))

Step -9 Find out the 2D3D correspondences

container for the model 3D coordinates found in the scene

vectorltPoint3fgt list_points3d_model_match

container for the model 2D coordinates found in the scene

vectorltPoint2fgt list_points2d_scene_match

for(unsigned int match_index = 0 match_index lt matchessize() ++match_index)

3D point from model

Point3f point3d_model = list_points3d_model[ matches[match_index]trainIdx ]

2D point from the scene

Point2f point2d_scene = keypoints_1[ matches[match_index]queryIdx ]pt

list_points3d_model_matchpush_back(point3d_model) add 3D point

list_points2d_scene_matchpush_back(point2d_scene) add 2D point

Step -10 Pose stimation

Step -101 Intrinsic camera parameters

Mat _A_matrix = Matzeros(3 3 CV_64FC1) intrinsic camera parameters

_A_matrixatltdoublegt(0 0) = fx [ fx 0 cx ]

_A_matrixatltdoublegt(1 1) = fy [ 0 fy cy ]

_A_matrixatltdoublegt(0 2) = cx [ 0 0 1 ]

_A_matrixatltdoublegt(1 2) = cy

_A_matrixatltdoublegt(2 2) = 1

Step -102 Estimate the pose given a list of 2D3D correspondences with RANSAC and the

method to use

Mat distCoeffs = Matzeros(4 1 CV_64FC1) vector of distortion coefficients

Mat rvec = Matzeros(3 1 CV_64FC1) output rotation vector

Mat tvec = Matzeros(3 1 CV_64FC1) output translation vector

bool useExtrinsicGuess = false if true the function uses the provided

rvec and tvec values as

initial approximations of the rotation and translation vectors

Mat inliers

int flags= ITERATIVE

Step -103 Solve PnPRansac

RANSAC parameters

int iterationsCount = 1000 number of Ransac iterations

float reprojectionError = 80 maximum allowed distance to consider it an inlier

float confidence = 099 RANSAC successful confidence

solvePnPRansac( list_points3d_model_match

list_points2d_scene_match

_A_matrix

distCoeffs

rvec

tvec

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

useExtrinsicGuess

iterationsCount

reprojectionError

confidence

inliers

flags )

Rodrigues(rvec Ra_matrix)

Ta_matrix = tvec converts Rotation Vector to Matrix

Step -11 Matrix Concatenation

Step -111 Construct T_matrix Convertimos la matriz Ta (31) en la matriz (4 1)

for (int y=0 y lt Ta_matrixrows ++y)

T_matrixatltdoublegt(y0)= Ta_matrixatltdoublegt(y0)

T_matrixatltdoublegt(30)= 10 Antildeadimos un 1 en la posicioacuten T_matrix(30)

[ T00 ]

T = [ T10 ]

[ T20 ]

[ 1 ]

Step -112 Construct R_matrix

for (int y=0 y lt Ra_matrixrows ++y)

for (int x=0 x lt Ra_matrixcols ++x)

R_matrixatltdoublegt(yx)= Ra_matrixatltdoublegt(yx)

R_matrixatltdoublegt(32)= 00 Antildeadimos un 1 a la en la posicioacuten R_matrix(32)

[ R00 R01 R02 ]

R= [ R10 R11 R12 ]

[ R20 R21 R22 ]

[ 0 0 0 ]

cout ltlt Calculo R= ltlt R_matrixltlt endl

Step -113 Primera Iteracioacuten

if (k == 0)

cout ltlt Primera Iteracioacutenltltnltltendl

Rt_matrix= R_matrix

Tt_matrix= T_matrix

hconcat(Rt_matrix Tt_matrix Tt_matrix)

cout ltlt Primera Iteracioacuten Traslacioacuten total= ltlt endl ltlt Tt_matrix ltlt endl

k++

[ R00 R01 R02 T03]

Tt_matrix= [ R10 R11 R12 T13]

[ R20 R21 R22 T23]

[ 0 0 0 1 ]

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

60

Step -114 Iteraciones Consecutivas

T=[R T] - Tt=[Rt Tt] - Tt= Ttinversa(T)

else

hconcat(R_matrix T_matrix Temp_matrix)

Temp2_matrix= Temp_matrixinv()

cout ltlt inversa= ltlt Temp2_matrixltlt endl

Temp3_matrix= Tt_matrixTemp2_matrix

Tt_matrix=Temp3_matrix

cout ltlt Calculo Tt= ltlt endl ltlt Tt_matrix ltlt endl

Rt_matrix= Rt_matrixRa_matrix

cout ltlt Calculo Rt= ltlt endl ltlt Rt_matrix ltlt endl

Step -12 Save Tt_matrix

cout ltlt lt Pose Saved ltltendl

ofstream filename

filenameopen(tray0txt iosapp)

filenameltltTt_matrixatltdoublegt(03)ltlt n ltltTt_matrixatltdoublegt(13)ltlt n

ltltTt_matrixatltdoublegt(23)ltlt n

filenameclose()

73 Anexo C Plotm

Apertura del fichero ldquotrayectoriatxtrdquo

file = fopen (trayectoriatxt r)

Lectura de los datos del fichero

point= fscanf(file f57)

Cierre del fichero

fclose(file)

Definimos las primeras posiciones del fichero

px=1 En la liacutenea 1 del fichero se encuentra la posicioacuten x

py=2 En la liacutenea 1 del fichero se encuentra la posicioacuten y

pz=3 En la liacutenea 1 del fichero se encuentra la posicioacuten z

Definimos el vector ldquoxrdquo de una fila y 19 columnas Realizamos la misma operacioacuten para los vectores

ldquoyrdquo y ldquozrdquo

x=zeros(1 19)

y=zeros(1 19)

z=zeros(1 19)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)

Realizamos un bucle ldquoforrdquo para leer todas las posiciones del fichero e ir guardando cada punto en su

respective vector

for i= 119

x(1 i)=point (px 1)

y(1 i)=point (py 1)

z(1 i)=point (pz 1)

px=px+3

py=py+3

pz=pz+3

endfor

Ploteamos los vectores

figure(1)

plot3(x y z--omlineWidth2)

grid on

xlabel (X axis)

ylabel (Y axis)

zlabel (Z axis)

title (UAV trajectory)