Trabajo Fin de Máster - Universidad de...
Transcript of Trabajo Fin de Máster - Universidad de...
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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
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)
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)
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)