P oyec o Fin de Ca e a
Ingenie ía de Telecomunicación
Fo ma o de Publicación de la Escuela Técnica
Supe io de Ingenie ía
Au o : F. Ja ie Payán Some
Tu o : Juan José Mu illo Fuen es
Dep. Teo ía de la Señal y Comunicaciones
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Se illa, 2013
T abajo Fin de G ado
G ado en Ingenie ía Elec ónica, Robó ica y Meca-
ónica
Es udio de écnicas de odome ía isuo-
ine cial. Algo i mo VINS-Mono
Au o : Ma ía Rod íguez Rod íguez
Tu o : Manuel Va gas Villanue a
Dp o. en Ingenie ía de Sis emas y Au omá ica
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Se illa, 2020
T abajo Fin de G ado
G ado en Ingenie ía Elec ónica, Robó ica y Meca ónica
Es udio de écnicas de odome ía isuo-ine cial.
Algo i mo VINS-Mono
Au o :
Ma ía Rod íguez Rod íguez
Tu o :
Manuel Va gas Villanue a
P o eso Ti ula
Dp o. en Ingenie ía de Sis emas y Au omá ica
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Se illa, 2020
T abajo Fin de G ado:
Es udio de écnicas de odome ía isuo-ine cial. Algo i mo VINS-
Mono
Au o : Ma ía Rod íguez Rod íguez
Tu o : Manuel Va gas Villanue a
El ibunal nomb ado pa a juzga el abajo a iba indicado, compues o po los siguien es p o eso es:
P esiden e:
Vocal/es:
Sec e a io:
acue dan o o ga le la cali icación de:
El Sec e a io del T ibunal
Fecha:

Ag adecimien os
En p ime luga , quie o ag adece a mi amilia que siemp e ha con iado en mi.
A mi u o , Manuel, po ayuda me a desa olla es e abajo.
Po úl imo, a mis amigos de la ca e a, po hace es os años an especiales y habe compa ido
an os momen os.
Ma ía Rod íguez Rod íguez
Se illa, 2020
I
Resumen
El
p esen e abajo ecoge una écnica de es imación de la posición y la o ien ación de un ehículo
a pa i de la usión senso ial de una unidad de medida ine cial (IMU) y un sis ema de isión
monocula . El algo i mo u ilizado pa a ello ha sido VINS-Mono.
Se ha es udiado el undamen o eó ico de dicho algo i mo, y ambién se han ealizado una se ie de
simulaciones. Pa a una de las simulaciones ealizadas se ha u ilizado un da ase público de EuRoC
MAV que con iene imágenes cap u adas po un d on y medidas omadas po su IMU. Po o o lado,
ambién se han ealizado a ias simulaciones en Gazebo. Una de ellas es sob e el uelo de un d on,
a pa i de un paque e de ROS ya c eado. La segunda simulación, en cambio, es sob e un ehículo
e es e modelado desde ce o en Gazebo. Es e ehículo lle a inco po ado una IMU y una cáma a,
con el obje i o de es ima la ayec o ia seguida en iempo eal. Pa a ello se ha u ilizado el so wa e
especialis a en obó ica, ROS.
III
XÍndice
4.3.1 Componen es de Gazebo 29
A chi os del mundo 29
A chi os del modelo 30
Plugins 33
5 Algo i mo VINS-Mono: Simulaciones 39
5.1 A chi o de con igu ación de VINS-Mono 39
5.2 EuRoC MAV da ase s 41
5.3 Vehículo aé eo Gazebo 49
5.4 Vehiculo e es e Gazebo 53
6 Conclusión 63
7 Anexo 65
7.1 my obo .wo ld 65
7.2 my obo .xac o 73
7.3 my obo .gazebo 80
7.4 A chi o de con igu ación eu oc_con ig.yaml del algo i mo VINS-Mono 83
7.4.1 Expe imen o EuRoC MAV Da ase 83
7.4.2 Expe imen o ehículo aé eo Gazebo 86
7.4.3 Expe imen o ehículo e es e Gazebo 88
7.5 Código de Ma lab pa a ob ene la ep esen ación de la posición y o ien ación ela i a
en e la cáma a y la IMU 90
7.6 Código de Ma lab pa a calcula e o es en la es imación de la ayec o ia del expe i-
men o del ehículo e es e 91
Índice de Figu as 95
Índice de Tablas 99
Índice de Códigos 101
Bibliog a ía 103
Índice al abé ico 105
Glosa io 107

No ación y Siglas
ROS Robo Ope a ing Sys em. Sis ema Ope a i o Robó ico
SDF
Simula ion Desc ip ion Fo ma . Fo ma o Dec ipción Si-
mulación
VINS Visual Ine ial Sys em. Sis ema isuo-ine cial
IMU Ine ial Measu emen Uni . Unidad de medida ine cial
VIO Visual Ine ial Odome y. Odome ía isuo-ine cial
KLT Kanade Lucas Tomasi
RANSAC
Random Sample Consesus. Consenso de mues a alea o-
ia
SLAM
Simul aneous Localiza ion And Mapping. T azado de
mapas y localización simul ánea
S M S uc u e om Mo ion
PnP Pe spec i e n Poin . Pe spec i a desde n pun os
GPS
Global Posi ioning Sys em. Sis ema de Posicionamien o
Global
DOF Deg ees O F eedom. G ados de Libe ad
(·)wSis ema de e e encia del mundo
(·)bSis ema de e e encia del cue po
(·)cSis ema de e e encia de la cáma a
ˆ
(·)Es imación de una can idad
RMa iz de o ación
qCua e nio
qw
bRo ación del ma co del cue po al ma co del mundo
pw
bT aslación del ma co del cue po al ma co del mundo
baSesgo del acele óme o
bwSesgo del gi oscopio
bkMa co del cue po mien as se oma la imagen k h
∆ kDu ación del in e alo de iempo [ k, k+1]
gwVec o de g a edad en el ma co del mundo
bk
bk
Velocidad espec o el sis ema de e e encia del cue po
mien as se oma la imagen k h
s
Escala que con ie e S M monocula a unidades mé icas
xk
Es ado de la IMU es el momen o en el que se oma la
imagen k h
λlIn e sa de la dis ancia de la ca ac e ís ica l h
XI
XII No ación y Siglas
B(ˆ
zbk
bk+1,X)Residuos de i ados de la IMU
C(ˆ
zcj
l,X)Residuos de i ados de la isión
BConjun o de las medidas de la IMU
C
Conjun o de las ca ac e ís icas que han sido al menos
obse adas dos eces en la en ana deslizan e ac ual
1 In oducción
1.1 Mo i ación
Ac ualmen e
, cada ez es más común libe a a los se es humanos de a eas a pa i de la apa-
ición de los obo s au ónomos, los cuales pe ciben el medio que les odea y pueden ope a
sin la supe isión de una pe sona. Juegan un papel impo an e en aplicaciones in eligen es como
explo ación, conducción au ónoma, ealidad i ual (VR) y ealidad aumen ada (AR). Es e abajo
se cen a en ehículos au ónomos, an o aé eos como e es es.
La p esencia de los Vehículos Aé eos No T ipulados (UAV) es cada ez mayo . Se a an de na es
que uelan sin ipulación y las cuales eje cen su unción de o ma emo a. Su comienzo ue en
aplicaciones mili a es en los Vehículos Aé eos de Comba e No T ipulados (UCAV), pe o su ama
hizo que el campo en el que se u ilizaban uese cada ez más amplio y muchas emp esas los han
in oducido en su abajo. En los úl imos años, ambién se han u ilizado an o en áb icas como pa a
en e enimien o, g acias al descenso de sus p ecios.
Po o o lado, al y como se ha mencionado, ambién los obo s e es es au ónomos se han
implan ado en la ac ualidad; un ejemplo de ello son los conocidos obo s de limpieza. O os que
siguen siendo muy es udiados son los ehículos au ónomos, los cuales de ás ya no sólo ienen es u-
dios de in es igación de ingenie ía pa a su uncionamien o, sino ambién exhaus i os es udios é icos.
(a) Vehículo Aé eo de Comba e No T ipulado (UCAV). (b) D on en el in e io de una áb ica.
Figu a 1.1 Ejemplos de ehículos aé eos.
1
2Capí ulo 1. In oducción
(a) Robo de limpieza. (b) Vehículo au ónomo.
Figu a 1.2 Ejemplos de ehículos e es es.
Todos es os obo s deben se capaces de desen ol e se en en o nos desconocidos y, pa a ello, es
necesa ia la p esencia de senso es, de o ma que puedan mo e se y o ien a se sin ayuda de se es
humanos.
1.2 Obje i o
En Robó ica, pa a la na egación au ónoma es necesa io que el obo enga conocimien o de su
posición. Pa a ob ene una buena es imación de la localización exis en muchos senso es y écnicas,
como los encode s, GPS, sis emas de na egación ine ciales y odome ía ine cial. El p oblema es
que los senso es siemp e ienen limi aciones: los encode s pueden ene e o es ele ados debido
al deslizamien o, el GPS puede ene p oblemas cuando se encuen a en espacios ce ados, y el
senso lase es muy dependien e del ma e ial y o ien ación de las supe icies. Nues o obje i o en
es e abajo es p opo ciona una écnica de es imación de la ayec o ia álida an o en espacios
in e io es como ex e io es. Además, la es imación de la ayec o ia se ealiza espec o al pun o de
pa ida (a di e encia del GPS, que es un senso que p opo ciona coo denadas absolu as, como son
las coo denadas geog á icas).
Pa a es ima la posición de un obo se u iliza la odome ía. Además, en es a écnica exis en
di e en es senso es pa a consegui el obje i o comen ado an e io men e: odome ía po encode s
en las uedas (en el caso de ehículos e es es), senso es de ba ido lase o simila es, senso es
isuales es e eoscópicos y odome ía isuo-ine cial. Es a úl ima es la écnica que se a a desa olla
en es e abajo, la cual usiona senso es ine ciales y senso es isuales. Hay que ene en cuen a
dos aspec os: po un lado, al y como se ha comen ado, los senso es siemp e ienen cie o e o y,
po an o, es a écnica no iene g an p ecisión; po o o lado, las posiciones es imadas siemp e son
ela i as al pun o de inicio. Aunque se aya a u iliza la odome ía isuo-ine cial en es e p oyec o,
se a a explica cómo se llegó has a ella.
La odome ía con encional se basaba inicialmen e en la suposición de que las e oluciones de
las uedas se pueden aduci en un desplazamien o lineal con elación al suelo. P opo ciona una
buena p ecisión a co o plazo, de bajo cos e y pe mi e al as elocidades de mues eo. Además, si se
usiona con medidas de posición absolu as, como el GPS, se mejo a la es imación. Pe o alguno de
los e o es que apa ecen son: mal alineamien o de las uedas, los diáme os de ambas uedas no son
iguales, pa inaje de las uedas...
1.2 Obje i o 3
Figu a 1.3
Encode inc emen al. Senso u ilizado pa a de e mina la posición, elocidad o di ección.
Una al e na i a es la odome ía isual (VO), basada en la es imación de la posición y o ien ación
de un obo a pa i de una secuencia de imágenes omadas po una o a ias cáma as. Es a se enca ga
de examina los cambios en las imágenes cap u adas, debidos al mo imien o. En la ac ualidad,
se ha is o aumen ado el uso de la odome ía isual monocula debido, en e o as causas, a su
equilib io en e cos e y p ecisión. La cáma a iene un bajo cos e y su p ecisión suele se mayo que
écnicas de odome ía con encionales como, po ejemplo, el encode ( igu a 1.3). Po o o lado, si
se compa a con o as écnicas más ac uales, como el GPS, ambién iene sus en ajas como su buen
uncionamien o en luga es donde es e úl imo no se ía capaz.
VO se puede di idi en dos ca ego ías según el ipo de cáma a que u iliza: es é eoscópica o
monocula . La p ime a iene dos len es y es capaz de p opo ciona p o undidad. Po o o lado, las
cáma as monocula es educen los e o es de calib ación y son cada ez más usadas en aplicaciones
de odome ía isual. El p incipal p oblema de es e ipo de cáma as es que no p opo ciona la escala
mé ica.
Pa a soluciona es e p oblema, en es e abajo se a a u iliza ambién un senso ine cial o IMU.
VINS, o sis ema isuo-ine cial monocula , ue c eado pa a elimina los e o es p oducidos po la
ausencia de la escala. El sis ema mínimo necesa io pa a odome ía isuo-ine cial (VIO) es aquel
o mado po una IMU y una cáma a monocula . Las cáma as e IMUs son complemen a ios (la
cáma a p opo ciona secuencias de imágenes, mien as que la IMU p opo ciona acele aciones y
elocidades angula es a una mayo ecuencia). El senso IMU es un senso de bajo cos e y su
p incipal en aja es que posee escala mé ica. Además, la IMU p opo ciona alo es de los ángulos
oll,pi ch yyaw (ángulos de alabeo, cabeceo y guiñada, espec i amen e). La di e encia en e es os
ángulos se explica a con inuación median e el ejemplo de un a ión pa a acili a su en endimien o:
• oll: Ro ación espec o al eje que une el mo o y la cola del a ión.
•pi ch: Ro ación espec o al eje que une pun a a pun a las alas del a ión.
•
yaw: Ro ación espec o al eje que pasa po el cen o de g a edad y es pe pendicula a los dos
ejes mencionados an e io men e.

4Capí ulo 1. In oducción
(a) Cáma a es e eo. (b) Cáma a monocula .
Figu a 1.4 Tipos de cáma a en odome ía isual.
Figu a 1.5 Ejes del d on y mo imien o sob e ellos.
El senso ine cial ambién compensa la al a de iqueza de in o mación (con as es o ex u as) en
las imágenes, la cual pe mi e ex ae ca ac e ís icas pa a que pos e io men e sean as eadas.
Como se obse a á más adelan e, las ecuaciones de es imación de mo imien o que pueden plan-
ea se a pa i de la in o mación isuo-ine cial incluye ue es no linealidades. Como la escala no
es obse able desde la cáma a monocula , es di ícil usiona las medidas de la cáma a y la IMU
di ec amen e. De ahí, la necesidad de unos buenos alo es iniciales. Se e á en el p oyec o cómo la
e apa decisi a pa a que la es imación de la ayec o ia de un obo sea buena se á la inicialización. La
exis encia de dos senso es hace que sea muy impo an e la calib ación de los pa áme os ex ínsecos
que desc iben la posición y o ien ación ela i a IMU-cáma a.
Pa a abo da odos es os p oblemas, en es e abajo es udio en de alle VINS-Mono, un es imado
de es ados 6 DOF isuo-ine cial p opues o po un g upo de obó ica aé ea en la Uni e sidad de
Ciencia y Tecnología de Hong Kong en 2017. Puede se u ilizado en ehículos e es es, MAVs,
sma phones y o as pla a o mas in eligen es. Es e es imado hace uso sólamen e de un senso
ine cial y una cáma a monocula .
1.3 Es uc u a del es o del documen o 5
Figu a 1.6
Senso es IMU y cáma a es e eoscópica u ilizados en el da ase de EuRoC MAV. Es e
da ase se u iliza en el p ime expe imen o del algo i mo de VINS-Mono.
Figu a 1.7 Es imación de la ayec o ia de un d on a pa i de un da ase de EuRoC MAV.
Con el obje i o de p oba el algo i mo VINS-Mono se ealiza una demos ación con un da ase
público de EuRoC MAV. Además, pa a demos a su uncionamien o en iempo eal, se han ealiza-
do un pa de simulaciones en Gazebo. A a és del so wa e de obó ica ROS, se a a ejecu a el
algo i mo en iempo eal con dos modelos de ehículos di e en es (un d on y un obo e es e). Se
pueden maneja y con ola los ehículos po eclado y, a la ez, se puede obse a la es imación de
la ayec o ia ob enida con el algo i mo en un isualizado 3D denominado RViz.
1.3 Es uc u a del es o del documen o
Es e p oyec o es á p esen ado de o ma que se comienza con la explicación del undamen o eó ico
del algo i mo VINS-Mono, se con inúa con los equisi os obliga o ios que se han de ene ins alados
pa a lanza el algo i mo, y se e mina con a ias demos aciones del uncionamien o del mismo.
•Capí ulo 2: Es ado del a e
6Capí ulo 1. In oducción
Clasi icación de algo i mos isuo-ine ciales según el mé odo u ilizado y el ni el de acopla-
mien o. P esen ación de algo i mos capaces de usiona las medidas de di e en es conjun os
de senso es.
•Capí ulo 3: Algo i mo VINS-Mono : Fundamen o Teó ico
Base eó ica sob e el algo i mo y explicación de las di e en es e apas que se pueden obse a
en su desa ollo: inicialización, p ep ocesamien o de medidas isuales e ine ciales, odome ía
isuo-ine cial, elocalización y op imización.
•Capí ulo 4: He amien as necesa ias
Desc ipción de los p og amas y lib e ías necesa ias pa a el buen uncionamien o de VINS-
Mono. Explicación de los comandos y he amien as más u ilizadas de ROS y Gazebo.
•Capí ulo 5: Algo i mo VINS-Mono : Simulaciones
Desc ipción de los expe imen os ealizados. El algo i mo ha sido p obado an o en un ehículo
aé eo como uno e es e. También se ha p obado en iempo eal, en la simulacion c eada de
Gazebo. Exposición y compa ación de los esul ados.
•Capí ulo 6: Conclusiones
Re lexión sob e los esul ados ob enidos, así como posibles ampliaciones del abajo.
2 Es ado del a e
En
la na u aleza, el sis ema de localización humano es á basado en un senso ine cial biológico
y los ojos. Los ó ganos esponsables del sen ido del equilib io, localizados en nues o oído
medio-in e no, pueden de ec a gi os y acele aciones lineales. Es a es la misma in o mación que
pe ciben los gi oscopios y acele óme os, ípicamen e in eg ados en las llamadas IMUs. Es o puede
se aslado en el campo de la ingenie ía a VINS (Sis emas de na egación isuo-ine ciales). Se
puede e , po an o, que a ni el biológico, nues o ce eb o ambién ealiza es a usión isuo-ine cial.
Es o queda pa en e po el hecho de que, cuando no exis e cohe encia en e lo pe cibido po el sen ido
del equilib io y de la is a (como cuando amos en un ehículo y ijamos la is a en el in e io del
mismo), podemos sen i males a o ma eo.
En la ingenie ía, al y como se comen ó en el apa ado 1.2, las es imaciones de la localización
ienen e o es debido a las ince idumb es de los senso es. Si se u ilizan múl iples senso es, es os
e o es se pueden limi a y educi . La IMU, po ejemplo, siemp e iene una cie a ince idumb e.
En la ac ualidad, se u ilizan cáma as pa a complemen a la in o mación de la IMU y así educi
los e o es. Los sis emas de na egación isuo-ine ciales basados en la usión de la in o mación
p oceden e de una cáma a y una IMU ecibe el nomb e de VINS. Es os son capaces de es ima la
posición y o ien ación de un ehículo desen ol iéndose en un en o no desconocido a p io i, además
de ecupe a la escala mé ica.
VINS se puede clasi ica gene almen e en sis emas de usión es echamen e o débilmen e aco-
plados [
17
]. En los sis emas débilmen e acoplados hay dos a ian es: la in o mación de la IMU se
inco po a como medidas independien es pa a op imiza las es imaciones ealizadas con las medidas
isuales, o bien en el il o de Kalman Ex endido se u ilizan las medidas de la IMU en la ase de
p edicción de es ados, mien as que las isuales sólo en la e apa de ac ualización. Po o o lado,
en los sis emas es echamen e acoplados, se op imiza de o ma conjun a las medidas de odos los
senso es, p opo cionando una mayo p ecisión. Es e úl imo sis ema es el oco p incipal de es udio
g acias a los a ances de la ecnología de compu ación.
VINS ambién puede di idi se según el mé odo en el que se base: en il o, en op imización o
en deep lea ning [
21
]. Los mé odos basados en il os no malmen e u ilizan el il o de Kalman
ex endido (EKF). Las medidas del senso ine cial de al a ecuencia son usadas en la p opagación de
es ados, mien as que las medidas isuales son usadas pa a la e apa de ac ualización. Po ejemplo,
el algo i mo MSCKF (Mul i-S a e Cons ain Kalman Fil e ) es á basado en el EKF, y consis e
en man ene poses es imadas an e io es y medidas isuales de una misma ca ac e ís ica desde
di e en es pun os de is a. Los mé odos basados en il os son sensibles a la sinc onización en el
iempo, ya que cualquie medida que llegue a de causa p oblemas debido a que los es ados no
7
14 Capí ulo 3. Algo i mo VINS-Mono: Fundamen o eó ico
medidas IMU. Es o ocu e po ejemplo cuando se ajus an las poses en la e apa de op imización.
El obje i o es e i a epe idas in eg aciones. Si cambiamos el ma co de e e encia del mundo al
local
bk
, en la ecuación (3.5) se obse a que
α
,
β
y
γ
dependen de los sesgos de la IMU, pe o no
de los es ados de
bk
y
bk+1
. Po an o, cuando la es imación de los sesgos a ía poco, se pueden
ajus a
α
,
β
y
γ
; en caso con a io, se uel e a ealiza la ep opagación. Es o educe el cos e
compu acional.
Rbk
wpw
bk+1=Rbk
wpw
bk+ w
bk∆ k−1
2gw∆ 2
k+α
α
αbk
bk+1
Rbk
w w
bk+1=Rbk
w( w
bk−gw∆ k)+β
β
βbk
bk+1
qbk
w⊗qw
bk+1=γ
γ
γbk
bk+1(3.4)
donde
α
α
αbk
bk+1=ZZ ∈[ k, k+1]
Rbk
(ˆ
a −ba −na)d 2
β
β
βbk
bk+1=Z ∈[ k, k+1]
Rbk
(ˆ
a −ba −na)d
γ
γ
γbk
bk+1=Z ∈[ k, k+1]
1
2Ω
Ω
Ω(ˆ
ω
ω
ω −bw −nw)γ
γ
γbk
d .(3.5)
α
α
αbk
bk+1,β
β
βbk
bk+1yγ
γ
γbk
bk+1son los é minos inc emen ales esul an es de la p ein eg ación IMU.
3.2 Inicialización
Como ya se ha comen ado du an e es e abajo, la odome ía isuo-ine cial monocula es un sis ema
al amen e no lineal: la escala no es obse able a pa i de la cáma a monocula y, po an o, es
muy di ícil usiona medidas de ambos senso es di ec amen e (son necesa ios unos buenos alo es
iniciales). Es o p o oca que la inicialización sea la ase más delicada pa a es e algo i mo. Además,
es a complejidad aumen a si la IMU iene ele ados e o es y sesgos.
3.2.1 Ven ana deslizan e sólo isión S M
Se emplea un mé odo débilmen e acoplado pa a la usión senso ial, que comienza con un S uc u e
om Mo ion (S M) [
20
] en una en ana deslizan e. Con un sis ema sólo isual es posible ob ene
los alo es iniciales, pe o si luego se alinea con las medidas de la IMU se puede ob ene ambién
los alo es de la g a edad, elocidad, sesgos y escala.
Es a en ana de o og amas S M se u iliza pa a educi complejidad. En p ime luga , se calcula la
posición ela i a en e dos imágenes po el algo i mo de los 5 pun os cuando se cumpla la siguien e
condición: exis en más de 30 pun os ca ac e ís icos comunes y su icien e pa alaje en e un o og ama
an e io y el úl imo. El o og ama an iguo se con ie e en el de e e encia
{C0}
(es e es el ma co
de e e encia de la cáma a en la p ime a posición). En el caso de que no se cumpla la condición
an e io , se añade el o og ama a la en ana y se espe an nue os o og amas. Si el algo i mo de
5 pun os iene éxi o, se es ablece la escala y se iangulan odas las ca ac e ís icas en e los 2
o og amas y, po úl imo, se aplica un ajus e pa a minimiza los e o es de p oyección. Pos e io -
men e, se es ima á la pose del es o de o og amas de la en ana. Todos es os alo es ob enidos son

3.2 Inicialización 15
Figu a 3.4 Esquema esumen e apa de inicialización.
posiciones y o aciones, siemp e espec o
{C0}
debido a que no se conoce sob e el ma co del cue po.
Conocidos los pa áme os ex ínsecos, se pueden ans o ma las poses espec o la cáma a al
ma co del cue po (IMU) en la ecuación (3.6). El pa áma o ses la escala, y es un ac o undamen al
pa a pode alinea las medidas isuales con las ine ciales, y así ob ene una buena alinealización.
qc0
bk=qc0
ck×qb
c−1
s¯
pc0
bk=s¯
pc0
ck−Rc0
bkpb
c(3.6)
3.2.2 Alineación isuo-ine cial
En es a e apa, median e la alineación de las medidas isuales e ine ciales se puede ob ene la escala,
la g a edad, la elocidad e incluso el sesgo. El sesgo del acele óme o se igno a en la inicialización
debido a que es á acoplado con la g a edad, cuyo alo es mucho mayo y p o oca que el sesgo sea
di ícil de obse a .
En p ime luga , se calib a el sesgo del gi oscopio, minimizando la unción (3.7). En es a ecuación
se puede obse a cómo se u ilizan an o las es imaciones isuales ob enidas de S M (
qc0
bk+1−1
,
qc0
bk
),
como el é mino inc emen al de la p ein eg ación de la IMU de o ien ación (ˆ
γ
γ
γbk
bk+1).
m´
ın
δbw
∑
k∈B

qc0
bk+1−1⊗qc0
bk⊗γ
γ
γbk
bk+1


2
γ
γ
γbk
bk+1≈ˆ
γ
γ
γbk
bk+1⊗1
1
2Jγ
bwδbw(3.7)
A pa i de es a es imación, con las ecuaciones de p ein eg ación de la IMU de la ec. (3.5), se
uel en a p opaga las a iables inc emen ales
α
,
β
, y
γ
, que como se comen ó an e io men e
dependen de los sesgos de la IMU.
16 Capí ulo 3. Algo i mo VINS-Mono: Fundamen o eó ico
Una ez inalizada la inicialización del sesgo el gi oscopio, se p ocede a inicializa los es ados de
las elocidades, g a edad y escala mé ica:
XI=h b0
b0, b1
b1,··· bn
bn,gc0,si(3.8)
bk
bk
es la elocidad espec o el ma co del cue po mien as se oma la imagen
k h
,
gc0
es el ec o
de g a edad espec o el ma co {C0}, y ses la escala mé ica.
A pa i de las a iables del ec o con enido en (3.8), se plan ea la ecuación (3.9), donde los
é minos inc emen ales
α
α
αbk
bk+1
y
β
β
βbk
bk+1
se exp esan en unción de la in o mación ob enida de la isión.
Las a iables a calcula son las elocidades espec o al ma co del cue po, la g a edad espec o
{C0}
y el pa áme o de escala ( a iables del ec o de es ados). Dichos alo es son es imados al
sus i ui los alo es conocidos de la in o mación ob enida de la S M (
Rc0
bk
,
Rc0
bk+1
,
¯
pc0
bk
,
¯
pc0
bk+1)
y al
sus i ui
α
α
αbk
bk+1
y
β
β
βbk
bk+1
po sus alo es ac ualizados.
∆ k
es el in e alo de iempo en e dos ames
consecu i os.
α
α
αbk
bk+1=Rbk
c0(s(¯
pc0
bk+1−¯
pc0
bk)+ 1
2gc0∆ 2
k−Rc0
bk bk
bk∆ k)
β
β
βbk
bk+1=Rbk
c0(Rc0
bk+1 bk+1
bk+1+gc0∆ k−Rc0
bk bk
bk).(3.9)
Una ez conocidas
bn
bn,gc0
y
s
, el siguien e obje i o es mejo a el ec o de g a edad ob enido.
Un ec o en el espacio idimensional depende de su magni ud y p oyección en los es ejes. Como
la magni ud de la g a edad es conocida, el núme o de g ados de libe ad de la g a edad se educe a 2.
T as es e ajus e, al calcula el ec o de la g a edad
gc0
, se puede ob ene la o ación
qw
c0
en e
el ma co de la cáma a y el mundo y, po an o, ans o ma odas las a iables y elocidades del
sis ema de e e encia de la cáma a
{C0}
al del mundo
{W}
. Las medidas isuales ambién se án
escalados a la escala mé ica. Todos es os alo es se án necesa ios pa a la siguien e e apa de VIO.
3.3 VIO monocula es echamen e acoplado
El ec o de es ados comple o de la en ana deslizan e en es e VIO es:
X=x0,x1, . . . xn,xb
c,λ0,λ1, . . . λm
xk=hpw
bk, w
bk,qw
bk,ba,bgi,k∈[0,n]
xb
c=pb
c,qb
c(3.10)
El ec o de es ados
X
con iene los es ados de la IMU cuando se cap u a el ame
bk
(
xn
), la
posición y o ien ación de la cáma a espec o a la IMU (
xb
c
), y la in e sa de la p o undidad de la
l h
ca ac e ís ica, is a desde su p ime a obse ación (
λl
). En p ime luga , los es ados de la IMU
ienen in o mación sob e la posición , elocidad, o ación y sesgos de dicho senso . Po o o lado, se
u iliza la in e sa de la dis ancia pa a p opo ciona mayo es abilidad. Además, nes el núme o o al
de key ames ymse co esponde con el núme o o al de ca ac e ís icas en la en ana deslizan e que
han sido obse adas al menos dos eces.
Las obse aciones se pueden medi con los senso es (la cáma a e IMU), las cuales se suponen con
uido. Se iene un modelo de obse ación que, a pa i del es ado, p opo ciona las obse acioness
3.3 VIO monocula es echamen e acoplado 17
ideales, el cual apa ece en la ecuación (3.11). Po o o lado, las obse aciones eales se deno an
como ˆz.
z=h(X)(3.11)
Po un lado, las obse aciones del senso IMU deben medi in o mación sob e la posición, elo-
cidad, o ien ación, y sesgos en la acele ación y en la o ien ación. Es os da os se plan ean de o ma
inc emen al en e 2 ames. El inc emen o de sesgo no se puede medi po su na u aleza, de o ma
que se ue za su inc emen o a ce o.








ˆ
α
α
αbk
bk+1
ˆ
β
β
βbk
bk+1
ˆ
γ
γ
γbk
bk+1
0
0








=






Rbk
w(pw
bk+1−pw
bk+1
2gw∆ 2
k− w
bk∆ k)
Rbk
w( w
bk+1+gw∆ k− w
bk)
qw−1
bk⊗qw
bk+1
babk+1−babk
bwbk+1−bwbk







.(3.12)
Po o o lado, las obse aciones de la cáma a consis en en la posición obse ada de las ca ac-
e ís icas en las imágenes, las cuales se usan como e e encia isual.
ˆ
zci
l
es la obse ación de una
ca ac e ís ica po p ime ez en el ame i, mien as que
ˆ
zcj
l
es la obse ación de dicha ca ac e ís ica
en un ame pos e io j.
ˆ
zcj
l=ˆucj
l
ˆ cj
l
ˆ
zci
l=ˆuci
l
ˆ ci
l(3.13)
La ob ención de las medidas an o de la IMU como de la cáma a se explicó de alladamen e en el
apa ado 3.1.
También se debe menciona qué son los esiduos. Es os son la di e encia en e la obse ación
eal y la ideal (es deci , el alo p edicho po el modelo de obse ación).
z=ˆ
z+δz⇒δz=z−ˆ
z=h(X)−ˆ
z(3.14)
Gene almen e, los esiduos, an o de la IMU como de la cáma a se suelen deno a de o ma gené ica
como
y siguen una dis ibución no mal, de media nula y de co a ianza P(ma iz de co a ianza
del esiduo).
∼N(φ,P)(3.15)
P( ) = 1
p(2π)N||P||e−1
2( −φ)TP−1( −φ)(3.16)
La maximización de es a p obabilidad es equi alen e a la minimización de la dis ancia de
Mahalanobis co espondien e. El p oblema de minimización se plan ea como un ajus e isuo-
ine cial ( isual-ine ial bundle adjus men ).
maxP( )∼min|| ||2
P=min||h(X)−ˆz||2
P(3.17)
El p oblema de es imación ambién se puede exp esa en é minos p obabilís icos, haciendo e e en-
cia a algunas de las siguien es unciones de p obabilidad:
18 Capí ulo 3. Algo i mo VINS-Mono: Fundamen o eó ico
•
P(Z | X): P obabilidad condicionada de la obse ación. P obabilidad de obse a el alo zen
el ec o de obse ación, supues o un es ado X.
•
P(X | Z): P obabilidad a pos e io i del es ado. P obabilidad de que el es ado sea X, a la is a
de un obse ación.
•
P(X): P obabilidad del es ado a p io i. P obabilidad de que el ec o de que el ec o de
es ados enga un cie o alo .
Exis en a ias o mas de es ima el es ado: MLE (Maximum Likehood Es ima ion) o MAP
(Maximum A pos e io i Es ima ion). La úl ima mencionada es la que u iliza VINS-Mono y, po
an o, la es imación del es ado es igual que el cálculo de la dis ibución de p obabilidad condicionada
a pa i de obse aciones conocidas.
P(X|Z)(3.18)
A endiendo al eo ema de Bayes:
P(X|Z) = P(Z|X)·P(X)
P(Z)∝P(Z|X)·P(X)(3.19)
En VINS-Mono, la in o mación a p io i p ocede de la inicialización, y du an e el uncionamien o
pos e io , del esul ado de la ma ginalización. La ince idumb e de las medidas se conside an como
una dis ibución gaussiana z∼N(z,Q). Po lo an o,
X∗=minX{P(z|x)}=minX∑||z−h(x)||Q(3.20)
Finalmen e se llega a que pa a la es imación del ec o (3.10), se minimiza la siguien e unción.
Se u iliza un ajus e isuo-ine cial pa a ob ene la mejo es imación posible:
m´
ın
X(
 p−HpX

2+∑
k∈B

 B(ˆ
zbk
bk+1,X)


2
Pbk
bk+1
+∑
(l,j)∈C
ρ(
 C(ˆ
zcj
l,X)

2
Pcj
l
))(3.21)
donde la no ma de Hube es de inida como
ρ(s) = s s ≤1
2√s−1s>1.(3.22)
B
es el conjun o de odas las medidas de la IMU, y
C
es el conjun o de las ca ac e ís icas que
se han obse ado al menos dos eces en la en ana ac ual.
p
y
Hp
es la in o mación p e ia de
ma ginalización.
3.3 VIO monocula es echamen e acoplado 19
En la ecuación (3.21) apa ecen an o los esiduos ine ciales como los isuales. En el caso de los
esiduos ine ciales end íamos:
B(ˆ
zbk
bk+1,X) = 






δα
α
αbk
bk+1
δβ
β
βbk
bk+1
δθ
θ
θbk
bk+1
δba
δbg







=









Rbk
w(pw
bk+1−pw
bk+1
2gw∆ 2
k− w
bk∆ k)−ˆ
α
α
αbk
bk+1
Rbk
w( w
bk+1+gw∆ k− w
bk)−ˆ
β
β
βbk
bk+1
2hqw−1
bk⊗qw
bk+1⊗(ˆ
γ
γ
γbk
bk+1)−1ixyz
babk+1−babk
bwbk+1−bwbk









(3.23)
Los esiduos isuales ienen la unción de minimiza el e o en la es imación de la posición
de una misma ca ac e ís ica. Se de ine la medida esidual de la cáma a en una es e a unidad. La
óp ica de la mayo ía de cáma as se puede modela como un ayo unidad conec ando la supe icie de
una es e a unidad. Es os esiduos isuales conside an las coo denadas de la ca ac e ís ica
l h
en
el p ime ame en el que se de ec ó, y las coo denadas en la imagen pos e io en la que se sigue
obse ando dicha ca ac e í ica.
C(ˆ
zcj
l,X) = b1b2T· ˆ
¯
Pcj
l−Pcj
l
kPcj
lk!
ˆ
¯
Pcj
l=πc−1 ˆucj
l
ˆ cj
l
Pcj
l=Rc
b Rbj
w Rw
bi Rb
c
1
λl
πc−1 ˆuci
l
ˆ ci
l
+pb
c!+pw
bi−pw
bj!−pb
c!(3.24)
La unción
π−1
c
ans o ma la localización de un píxel en un ec o unidad a pa i de los pa áme os
in ínsecos. Como el ec o esidual iene dos g ados de libe ad, se puede p oyec a en el plano
angen e ( igu a 3.5). Du an e es a e apa, la dis ancia in e sa de las ca ac e ís icas, la posición y
elocidad de cada ame, los pa áme os ex ínsecos y los sesgos de la IMU se op imizan jun os.
3.3.1 Ma ginalización
La ma ginalización es un p ocedimien o que ue o iginalmen e desa ollado pa a odome ía isuo-
ine cial [
22
], y se enca ga de limi a la complejidad compu acional. Es o es debido a que es a
complejidad aumen a cuando se obse an en o nos desconocidos y, po an o, el ec o de es ados
iende a c ece . El esul ado p opues o es educi es e cos e ma ginalizando pa e de los es ados
p e ios.
El p ocedimien o de la igu a 3.6 se basa en mi a el penúl imo o og ama cla e de la en ana:
•
Si es un o og ama cla e, se man iene en la en ana y el o og ama más an iguo de la en ana
se ma ginaliza (es deci , se desecha).

20 Capí ulo 3. Algo i mo VINS-Mono: Fundamen o eó ico
Figu a 3.5 Imagen del esiduo isual ep esen ado en una es e a unidad.
Figu a 3.6 P ocedimien o de ma ginalización.
•
Po o o lado, si no es un o og ama cla e, se elimina el o og ama y sus medidas isuales,
pe o se man ienen las medidas ine ciales.
O a unción de es e paso es man ene espaciados los o og amas cla e de la en ana: po un
lado, asegu a que ienen su icien e pa alaje pa a la iangulación de ca ac e ís icas, y po o o lado,
maximiza la p obabilidad de man ene las medidas del acele óme o con mayo exci ación. Con
la ma ginalización se cons uyen unas nue as condiciones a p io i del p oblema, basadas en las
medidas ma ginalizadas del es ado eliminado.
3.3.2 De ección de allos y ecupe ación
VIO es muy obus o an e cambios de en o nos. Posee un módulo de de ección de allos que obse a
que las salidas del es imado no sean inusuales. Se conside a que exis e allo cuando el núme o de
ca ac e í icas en común con el ame an e io es á po debajo de un lími e, cuando exis e un acío en
o ación o aslación en la salida del esimado , o cuando hay un cambio muy g ande en la es imación
3.4 Relocalización 21
de los pa áme os ex ínsecos o sesgos. Es ine i able que se p oduzcan allos en el sis ema, pe o se
pueden iden i ica y co egi . Po ello, VINS-Mono uel e a la ase de inicialización y uel e a
c ea un nue o segmen o pa a la ayec o ia seguida.
3.4 Relocalización
La complejidad compu acional se limi a g acias a la en ana deslizan e y a la ma ginalización, pe o
es o ambién p o oca que se acumulen de i as. Es as de i as ienen luga en la posición espec o a
los es ejes (x,y,z) y en la o ación al ededo de la di ección de la g a edad (yaw). Pa a elimina las
se u iliza es e módulo de elocalización. La obse ación epe ida de las mismas ca ac e ís icas es
u ilizada en la elocalización, mejo ando la es imación y la exac i ud. El p ocedimien o seguido es
el siguien e:
•De ección de lazos:
Se u iliza DBoW2 [
13
], que es una lib e ía mejo ada de DBoW, la cual implemen a un á bol
je áquico pa a ap oxima ecinos ce canos en el espacio de ca ac e ís icas de la imagen.
Pa a aumen a la asa de ecupe ación po cie e de lazo se de ec an 500 esquinas más con
el desc ip o BRIEF [
9
]. Se man ienen odos los desc ip o es de las ca ac e ís icas pe o sin
gua da las imágenes pa a aho a memo ia.
•Recupe ación de ca ac e ís icas:
Cuando se de ec a un bucle, se es ablece la conexión en e la en ana ac ual y el candida o de
cie e de bucle. Las co espondencias de ca ac e ís icas son encon adas po el desc ip o
BRIEF, pe o pueden exis i alo es a ípicos. Exis en di e sos pasos pa a elimina los, y as
es e p ocedimien o si el núme o de inlie s es mayo que el lími e es ablecido, se conside a un
bucle y se ealiza la elocalización.
•Relocalización es echamen e acoplada:
En es e paso, se alinea la en ana ac ual con las poses pasadas. La op imización de la en ana
deslizan e se ealiza u ilizando medidas de la IMU, medidas isuales y co espondencias
del bucle en el cie e de lazo. Cuando se es ablecen muchos cie es de bucles en la en ana
ac ual, se op imizan las co espondencias de las ca ac e ís icas de cie e de bucle de odos
los ames a la ez [18].
3.5 Op imización de g a o de poses global
Hay que esal a que la op imización y la elocalización se lle an a cabo en di e en es hilos de o ma
asínc ona: es o pe mi e u iliza en la elocalización la op imización del g a o de poses en el ins an e
en el que es é disponible.
G acias a la medida ine cial de la g a edad, los ángulos pi ch y oll son o almen e obse ables.
Es os son es ados absolu os espec o el ma co del mundo, mien as que x,y,zyyaw son es imaciones
ela i as espec o el ma co de e e encia. La de i a acumulada solo ocu e en es os 4 úl imos es ados
y la op imización sólo se ealiza de es os 4 g ados de libe ad.
22 Capí ulo 3. Algo i mo VINS-Mono: Fundamen o eó ico
Figu a 3.7 Pasos seguidos po el módulo de elocalización.
ˆ
pi
i j =ˆ
Rw−1
i(ˆ
pw
j−ˆ
pw
i)
ˆ
ψi j =ˆ
ψj−ˆ
ψi.(3.24)
3.5.1 Fusiona g a o de poses
El g a o de poses op imiza el mapa ac ual y ambién es capaz de usiona el mapa ac ual con un mapa
an e io ya cons uido y gua dado. Si hemos ca gado un mapa an e io y se de ec an conexiones
an e ambos, se puede p oduci una usión.
El amaño del g a o de poses puede c ece sin lími es cuando la dis ancia del iaje aumen a,
3.5 Op imización de g a o de poses global 23
Figu a 3.8
T ayec o ia es imada del ehículo e es e con algo i mo VINS-Mono al al usiona la
con una ayec o ia gua dada de un expe imen o an e io .
limi ando el iempo eal del sis ema a la go plazo. Po an o, se implemen a un p oceso de educción
de la mues a: odos los key ames con es icciones de cie e de lazo se man ienen, pe o sus ecinos
que se encuen en muy ce ca o engan o ien aciones muy pa ecidas se eliminan.
30 Capí ulo 4. He amien as u ilizadas
Figu a 4.4 He amien a q _g aph que mues a conexiones en e nodos al ejecu a VINS-Mono.
</wo ld>
</sd >
El código 4.2 es un ejemplo de un a chi o con ex ensión .wo ld. En él, se añade un modelo de
una casa ya c eado de Gazebo denominado house_1, y se ha indicado el nomb e, la posición y
la o ien ación deseada den o de la simulación. Es o es lo que se ha hecho pa a c ea el en o no
de la simulación de Gazebo del úl imo expe imen o ealizado sob e VINS-Mono: se han añadido
di e en es modelos ya c eados como ca e e as, a olas, semá o os, pa ques, casas... El código
comple o del a chi o del mundo c eado se puede encon a en el código 7.1.
(a) Vis a on al casa. (b) Vis a gi ada casa.
Figu a 4.5 Di e en es is as de la casa ya c eada de Gazebo.
A chi os del modelo
Es e ipo de a chi os ambién ienen o ma o SDF. Cada a chi o sólo con iene un modelo y una ez
c eado puede in oduci se en el a chi o del mundo. Es o hace que el modelo se pueda eu iliza y
ambién simpli ica el a chi o del mundo. Al igual que exis en ya ejemplos de mundos c eados en
Gazebo, ambién hay muchos modelos.

4.3 Gazebo 31
Los modelos se c ean a pa i de a iculaciones, enlaces, plugins...Es necesa io indica odos
es os elemen os pa a pode c ea el modelo. El modelo c eado en es e p oyec o ha sido un ehículo
de 4 uedas ( igu a 4.6). El código comple o necesa io pa a su c eación se puede e en el anexo
7.2, donde se pueden obse a odos los elemen os que se deben especi ica . Aquí solo se in oduce
la sección del código donde se modela la ueda delan e a de echa, con el obje i o de explica una
pa e del código más de alladamen e.
El ehículo es á compues o po 4 uedas, el chasis, una unidad de medida ine cial y 2 cáma as
(una on al y una la e al, que es la usada pa a oma las imágenes que se u iliza án en la es imación
del algo i mo VINS-Mono). Pa a cada componen e se ha de especi ica el nomb e, el elemen o
isual de colisión e ine cial, y ambién la a iculación que lo une a o o componen e del modelo.
(a) Vis a on al ehículo. (b) Vis a gi ada ehículo.
Figu a 4.6 Di e en es is as del ehículo c eado en Gazebo.
El siguien e código con iene un ejemplo de los elemen os que deben especi ica se pa a cada
componen e del coche. Es e, en conc e o, modela la ueda delan e a de echa ( igu a 4.7):
Código 4.3 F agmen o de código del modelo del ehículo. Rueda delan e a de echa.
<!--Rueda de echa delan e a-->
<link name=" on _ igh _wheel">
< isual>
<o igin py="1.57079632679 0 0 " xyz="0 0.15 0"/>
<geome y>
<mesh scale ="0.9 0.9 0.9" ilename="model://ca _wheel/meshes/
ca _wheel.dae"/>
</geome y>
<ma e ial name="Da kG ay"/>
</ isual>
<collision>
<o igin py="1.57079632679 0 0.2 " xyz="0 0.15 0"/>
<geome y>
<cylinde leng h="0.1" adius="0.2"/>
</geome y>
</collision>
<ine ial>
<mass alue="${wheel_mass}"/>
<o igin xyz="0 0.08 0"/>
32 Capí ulo 4. He amien as u ilizadas
<ine ia ixx="${0.25*wheel_mass*0.2*0.2}" ixy="0" ixz="0" iyy="$
{0.5*wheel_mass*0.2*0.2}" iyz="0" izz="${0.25*wheel_mass
*0.2*0.2}"/>
</ine ial>
</link>
<join name=" on _ igh _wheel_join " ype="con inuous">
<pa en link="chassis"/>
<child link=" on _ igh _wheel"/>
<o igin py="0 0 0" xyz="0.25 0.3 -0.14"/>
<axis py="0 0 0" xyz="0 1 0"/>
<limi e o ="100" eloci y="100"/>
<join _p ope ies damping="0.0" ic ion="0.0"/>
</join >
Se pueden obse a dos pa es o almen e di e enciadas: la ueda y la a iculación que une la
ueda con el es o del ehículo, en con e o, con el chasis. A con inuación, se a a hace una b e e
explicación de los elemen os del código más impo an es:
•Link: Aquí se de allan las p opiedades ísicas del enlace en el modelo.
– name: Se indica el nomb e del enlace, el cual debe se único.
– collision
: Se especi ican las p opiedades de colisión de un enlace. Es as pueden se
di e en es a las p opiedades isuales.
– isual
: Las p opiedades isuales del enlace (la posición y o ien ación de los ejes del
enlace, y la o ma del obje o).
Se puede obse a en el código que se u iliza un modelo 3D ya c eado llamado ca _-
wheel.dae en luga de usa una igu a simple como pod ía se un cilind o pa a imi a la
o ma de una ueda.
– ine ial
: Las p opiedades ine ciales del enlace (la masa, el o igen del cen o de masas y
los elemen os de la ma iz de ine cias).
•Join :
Se desc ibe la a iculación que a a uni dos componen es del modelo (en es e caso se
une la ueda y el chasis).
– name: Nomb e de la a iculación, el cual debe se único.
– ype
: Se especi ica el ipo de a iculación del que se a a. En es e caso es de ipo
con inuous po que o a espec o a un único eje con un ango con inuo de mo imien o.
– pa en link
y
child link
: Se especi ican los componen es que se unen a a és de dicha
a iculación.
– axis
: Se indican los pa áme os de los ejes de o ación o de los ejes de aslación,
según sea una a iculación de e olución o p ismá ica. La ueda es una a iculación de
e olución, y o a al ededo del eje y.
– o igin
: Es el desplazamien o y la o ación de los ejes del child link espec o pa en link.
Es deci , los ejes de las uedas ienen igual o ación que los ejes del chasis pe o iene un
cie o desplazamien o.
Exis en muchos más elemen os que se pueden de ini a la ho a de c ea los componen es de un
modelo, pe o sólo he hecho un b e e inciso sob e aquellos que se han u ilizado en el modelo del
ehículo y cuyo código se encuen a en su o alidad en 7.2.
4.3 Gazebo 33
Figu a 4.7 Modelo de una ueda de coche ya c eado po Gazebo.
Plugins
Los plugins p opo cionan un mecanismo simple pa a in e ac ua con Gazebo. Son muy ú iles
po que:
•Pe mi en a los desa ollado es con ola casi cualquie aspec o de Gazebo.
•Pueden se inse ados y eliminados de un sis ema en ejecución.
•Son u inas independien es que se compa en ácilmen e.
Exis en di e en es ipos: mundo, modelo, senso , sis ema, isual y GUI. Cada ipo es ges ionado
po un componen e di e en e de Gazebo y son elegidos según la uncionalidad deseada. En la
simulación c eada con Gazebo pa a el expe imen o de VINS-Mono se han u ilizado plugins pa a
los senso es que lle a inco po ado el ehículo: la IMU y las dos cáma as. En ambas cáma as se ha
u ilizado el mismo plugin, con las mismas ca ac e ísicas.
Código 4.4 Plugin de la cáma a.
<gazebo e e ence="came a2_link">
<senso ype="came a" name="came a_la ">
< isualize> ue</ isualize>
<upda e_ a e>60</upda e_ a e>
<came a name="came a_la ">
<ho izon al_ o >1.413</ho izon al_ o >
<image>
<wid h>640</wid h>
34 Capí ulo 4. He amien as u ilizadas
<heigh >360</heigh >
< o ma >R8G8B8</ o ma >
</image>
<clip>
<nea >0.01</nea >
< a >100</ a >
</clip>
</came a>
<plugin name="came a_con olle " ilename="libgazebo_ os_came a.
so">
<came aName>cocheci o/came a_la </came aName>
<alwaysOn> ue</alwaysOn>
<upda eRa e>60</upda eRa e>
<imageTopicName>/cocheci o/came a_la /image_ aw</imageTopicName
>
<came aIn oTopicName>/cocheci o/came a_la /came a_in o</
came aIn oTopicName>
< ameName>cocheci o_base_cam_la </ ameName>
</plugin>
</senso >
< u nG a i yO > ue</ u nG a i yO >
</gazebo>
En p ime luga , se an a desc ibi los di e en es pa áme os que se de inen en el agmen o de
código an e io , que an a modela la cáma a la e al (es aquella que p opo ciona las imágenes que
pos e io men e se p ocesan pa a es ima la ayec o ia del ehículo). Pos e io men e, en el capí ulo
5, se e á cómo se á necesa io ajus a en un iche o de con igu ación del algo i mo VINS-Mono los
pa áme os in e nos elacionados an o con la cáma a como con la IMU. Es o es necesa io pa a que
la ayec o ia que es ima sea buena. También se á necesa io conoce la posición ela i a de un senso
espec o a o o (es o ambién se explica á más de enidamen e en el capí ulo 5, de simulaciones).
Vol iendo al plugin de la cáma a, se a a desc ibi b e emen e el obje i o que pe sigue el código:
• e e ence: Se indica el enlace en el cual se ins ala el plugin.
• ype
: Tipo de senso . En es e caso, se a a de una cáma a pe o ambién puede se GPS, IMU,
LIDAR, al íme o, cáma a 3D...
•name: Nomb e que se le asigna al senso , el cual debe se único.
• isualize
: Indica si el senso es isualizado en la GUI. Cuando es á ijado a T ue, en la
simulación de Gazebo se e án las imágenes que cap a la cáma a. Se puede obse a en la
igu a 4.8.
•upda eRa e
: Se especi ica la ecuencia a la que el senso publica la in o mación (en es e
caso, núme o de ames po segundo).
•came a: Se indican los elemen os especí icos de una cáma a:
– name: Nomb e de la cáma a.
– ho izon al_ o : Campo de isión o el ángulo aba cable po el senso .
– image: Ancho, al o y o ma o de las imágenes cap u adas po la cáma a.
– clip: Dis ancia mínima y máxima a la que la cáma a es capaz de cap a obje os.
4.3 Gazebo 35
Figu a 4.8
Las imágenes cap adas po la cáma a son isualizadas en la simulación ( isualize=T ue).
•plugin
– alwaysOn
: Indica si el plugin puede ac i a se o desac i a se en cualquie momen o. Al
es a a T ue, se o dena que siemp e es é ac i o.
– upda eRa e: F ecuencia a la que publica la in o mación.
– imageTopicName: Tópico en el que publica la in o mación.
– came aIn oTopicName
: Tópico en el que publica da os sob e la in o mación que pu-
blica. En es e caso, al a a se de una cáma a los da os que publica son el al o y ancho
de las imágenes, y el ipo de o ma o.
Código 4.5 Plugin de la IMU.
<gazebo e e ence="imu_link">
<g a i y> ue</g a i y>
<senso name="imu_senso " ype="imu">
<always_on> ue</always_on>
<upda e_ a e>200</upda e_ a e>
< opic>__de aul _ opic__</ opic>
<plugin ilename="libgazebo_ os_imu_senso .so" name="imu_plugin">
< opicName>imu0</ opicName>
<bodyName>imu_link</bodyName>
<upda eRa eHZ>200.0</upda eRa eHZ>
<gaussianNoise>0</gaussianNoise>
<xyzO se >0 0 0</xyzO se >
< pyO se >0 0 0</ pyO se >

36 Capí ulo 4. He amien as u ilizadas
< ameName>imu_link</ ameName>
<accelD i >0.5 0.5 0.5</accelD i >
<accelGaussianNoise>0.35 0.35 0.3</accelGaussianNoise>
< a eD i >0.0 0.0 0.0</ a eD i >
< a eGaussianNoise>0.00 0.00 0.00</ a eGaussianNoise>
<headingD i >0.0</headingD i >
<headingGaussianNoise>0.00</headingGaussianNoise>
</plugin>
</senso >
</gazebo>
En el código 4.5, se pueden obse a cómo los pa áme os elacionados con el senso IMU son
los mismos ( opicname,upda eRa eHZ...) que en el plugin de la cáma a. Pe o, en cambio, hay o os
pa áme os del plugin que si a ían. Se han especi icado unos pa áme os e e idos al uido de la
IMU: la des iación es ánda de los uidos gausianos y la des iación es ánda de los e o es de de i a.
Jus amen e es os da os son muy impo an es po que es necesa io conoce los pa a pos e io men e
indica los en el iche o de con igu ación del algo i mo VINS-Mono.
En la igu a 4.9 se pueden obse a las conexiones exis en es en la simulación c eada del ehículo
e es e en Gazebo y los ópicos de ROS donde se publica in o mación o se lee. Po un lado, se
publica in o mación en los ópicos e e idos a ambas cáma as (/cocheci o/cama a_la y/coche-
ci o/cama a_ on ) y a la IMU(/imu0). Po o o lado, a uno de los ópicos a los que se susc ibe
es /cmd_ el (cuya in o mación p o iene del nodo con el que se maneja el ehículo y le indica la
elocidad y acele ación en cada ins an e).
Resal a que aunque el ehículo enga inco po adas dos cáma as, las imagenes u ilizadas pos e-
io men e en el algo i mo VINS-Mono son las cap adas po la cáma a la e al. La cáma a on al
sólo se ha incluido po que es más ípico ene una cáma a que ome o os en la pa e delan e a del
ehículo, pe o no iene ninguna unción a la ho a de ealiza la es imación de la ayec o ia.
4.3 Gazebo 37
Figu a 4.9
Nodos y ópicos c eados con la simulación de Gazebo. Imagen omada a pa i de la
he amien a q _g aph.
5 Algo i mo VINS-Mono: Simulaciones
Pa a ealiza expe imen os del algo i mo VINS-Mono se ha decidido u iliza di e en es en o nos y
obo s. Po un lado, se han u ilizado a ios da ase s públicos de EuRoC MAV [
8
], cuyas medidas son
omadas po senso es exis en es en un d on. Po o o lado, se han ealizado un pa de simulaciones
en Gazebo en iempo eal (una de ellas es ima la ayec o ia de un coche, mien as que la o a es ima
la ayec o ia de un d on).
5.1 A chi o de con igu ación de VINS-Mono
Pa a hace unciona el algo i mo con las simulaciones an e io es es necesa io modi ica un a chi o
de con igu ación, como ya se comen ó an e io men e. En es e se indican algunos da os necesa ios
pa a una buena es imación, en e los que se encuen an los pa áme os in ínsecos de los senso es
( an o de la IMU como de la cáma a), además de los pa áme os ex ínsecos que e lejan la posición y
o ien ación ela i a en e ambos. A pa e, ambién se especi ican o os pa áme os elacionados con
la isualización, con la con igu ación de cie e de bucle, e c. A con inuación, se an a explica más
de enidamen e odos es os aspec os, incluyendo dis in as pa es del código. El código comple o se
puede encon a en el anexo 7.4, donde se han incluido los iche os de con igu ación de cada una de
las simulaciones ealizadas. El iche o de con igu ación u ilizado pa a los expe imen os basados en
los da ase s de EuRoC MAV es siemp e el mismo po que el da ase se ob iene del mismo ehículo
aé eo y los pa áme os no a ían (po an o, sólo se añade en el anexo el a chi o u ilizado en el
p ime da ase ). La única di e encia se encuen a en el di ec o io donde se gua dan los da os de salida.
En p ime luga , en el codigo 5.1 se indican los ópicos a los que se susc ibe el algo i mo pa a
lee la in o mación ob enida de los senso es, an o de la cáma a monocula como de la IMU (en
es os ópicos publican las medidas los da ase s de EuRoC MAV o las simulaciones de Gazebo,
según el expe imen o en el que nos encon emos). Un equisi o pa a el buen uncionamien o de
es e algo i mo es que la ecuencia a la que publica medidas la cáma a y la IMU sea de mínimo de
20Hz y 100Hz, espec i amen e. También se indica el di ec o io de salida donde más adelan e se
gua da á la ayec o ia es imada.
Código 5.1 Pa áme os comunes.
imu_ opic: "/imu0"
image_ opic: "/cam0/image_ aw"
ou pu _pa h: "/home/ma ia od/Esc i o io/TFG/expe imen os/da ase _EUROC1
/"
39
46 Capí ulo 5. Algo i mo VINS-Mono: Simulaciones
(a) MH_01_easy.
(b) MH_02_easy.
(c) MH_03_medium.
(d) MH_04_di icul .
Figu a 5.6 Imágenes u ilizadas en los da ase s EuRoC MAV.
<< mp_Q.y() << " "
<< mp_Q.z() << " "
<< mp_Q.w() << endl;
Código 5.7 Modi ica unción upda ePa h() en a chi o pose_g aph.cpp.
o s eam loop_pa h_ ile(VINS_RESULT_PATH, ios::app);
loop_pa h_ ile.se (ios:: ixed, ios:: loa ield);

5.2 EuRoC MAV da ase s 47
loop_pa h_ ile.p ecision(0);
loop_pa h_ ile << (*i )-> ime_s amp << " ";
loop_pa h_ ile.p ecision(5);
loop_pa h_ ile << P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << endl;
Código 5.8 Modi ica unción addKeyF ame() en a chi o pose_g aph.cpp.
o s eam loop_pa h_ ile(VINS_RESULT_PATH, ios::app);
loop_pa h_ ile.se (ios:: ixed, ios:: loa ield);
loop_pa h_ ile.p ecision(0);
loop_pa h_ ile << cu _k -> ime_s amp << " ";
loop_pa h_ ile.p ecision(5);
loop_pa h_ ile << P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << endl;
Código 5.9 Modi ica unción main() en a chi o pose_g aph_node.cpp.
VINS_RESULT_PATH = VINS_RESULT_PATH + "/ ins_ esul _loop. x ";
Una ez se ealizan odas las modi icaciones necesa ias, se uel e a lanza el algo i mo pa a
ob ene el a chi o de salida ins_ esul _loop. x . Po an o, ya se iene la ayec o ia es imada en
el o ma o co ec o pa a e alua el algo i mo con e o. Po o o lado, los da os g ound u h ienen
o ma o .cs y se ienen que ans o ma a TUM. Pa a ello, se u iliza la o den mos ada en 5.10.
Pos e io men e, ya se puede ejecu a la he amien a e o_ aj pa a ob ene las g á icas compa a i as
de las ayec o ias, de las posiciones espec o los es ejes, y de los ángulos de Eule . La compa a-
ción en e la ayec o ia eal y es imada se puede obse a en la imagen 5.7, en la cual apa ece la
ayec o ia es imada de odos los da ase s. Las g á icas de las posiciones espec o los es ejes y
espec o los ángulos de na egación son como las mos adas en 5.8 (solo se han añadido las ob enidas
a pa i del p ime da ase pa a mos a cómo son las g á icas ob enidas). En es as se pueden e las
líneas co espondien es a la es imación y a las medidas g ound_ u h casi supe pues as (es un poco
complicado di e encia las). Po o o lado, la línea ec a que apa ece en odas las g á icas sólo iene
la uncion de uni el p ime alo con el úl imo alo del expe imen o (es deci , no iene nada que
e con la ayec o ia eal o es imada).
Código 5.10 T ans o ma medidas g ound u h a o ma o TUM.
e o_ aj eu oc da a.cs --sa e_as_ um
48 Capí ulo 5. Algo i mo VINS-Mono: Simulaciones
(a) MH_01_easy. (b) MH_02_easy.
(c) MH_03_medium. (d) MH_04_di icul .
Figu a 5.7
G á icas ob enidas a pa i de la he amien a e o_ aj. Con ienen la ayec o ia es imada
y la ayec o ia eal de los cua o da ase s .
Tabla 5.1 E o es calculados a pa i de he amien a e o_ape (en me os).
Max Mean Median Min RMSE SSE S d
MH_01_easy 0.90279 0.20720 0.20457 0.00937 0.22785 94.23017 0.09478
MH_02_easy 0.83487 0.19371 0.13849 0.01478 0.24065 86.64175 0.14279
MH_03_medium 1.30522 0.32323 0.23857 0.02071 0.40105 210.70204 0.23740
MH_04_di icul 1.5956 0.42322 0.39195 0.04747 0.47774 222.52977 0.22162
Código 5.11 U iliza he amien a e o_ aj.
e o_ aj um ../../../ ins_ esul _loop. x -- e =da a. um -p --
plo _mode=xyz align -co ec _scale
Pe o, pa a pode compa a la e aluación de VINS-Mono de los cua o da ase s, se an a calcula
los e o es de cada expe imen o con la he amien a e o_ape y se an a mos a en la abla 5.1. En
es a abla apa ece di e en e in o mación sob e el e o come ido: e o máximo, e o mínimo, e o
medio, la mediana, RMSE (e o cuad á ico medio), SSE (suma de los cuad ados de los e o es), y
STD (e o es ánda de la media).
Voy a explica a con inuación algunos de los e o es que apa ecen en la abla y cuyos alo es
se ob ienen con la he amien a e o_ape. El RMSE mide la can idad de e o que hay en e dos
conjun os de da os, compa ando un alo p edicho y un alo obse ado o conocido. Po o o lado, el
SSE ambién mide la di e encia en e el alo obse ado y el p edicho. Los alo es de es os e o es
aumen an cuando ambién lo hace la complejidad de los da ase s, debido a que las es imaciones son
menos pa ecidas a los da os obse ados. Po úl imo, el e o es ánda de la media es la des iación
es ánda de una mues a es adís ica (es deci , la a iación en e la media de la población y la media
5.3 Vehículo aé eo Gazebo 49
(a) Compa ación posiciones x,y,z.
(b) Compa ación ángulos oll,pi ch yyaw.
Figu a 5.8 G á icas ob enidas a pa i de la he amien a e o_ aj (pa a el da ase MH_01_easy).
que se conside a conocida). Po an o, se puede obse a en la abla 5.1 que el e o p oducido en la
es imación de la ayec o ia con VINS-Mono aumen a a medida que aumen a la complejidad de los
da ase s.
También se han ob enido unos mapas de e o es pa a los cua o da ase s, los cuales indican en
qué zonas de la ayec o ia se come e un mayo e o en la es imación. Es os se han ecogido en la
igu a 5.9.
Exis en más he amien as en el paque e e o pa a la e aluación de algo i mos SLAM, pe o he
conside ado e o_ aj ye o_ape son aquellas que o ecen una in o mación más desc ip i a pa a
pode e alua VINS-Mono.
5.3 Vehículo aé eo Gazebo
Es e expe imen o se ha ealizado a pa i de un paque e de ROS llamado um_simula o [
1
]. Es e se
u iliza pa a simula el obo aé eo A d one en Gazebo. Po an o, es o o expe imen o del algo i mo
ealizado en un d on, ya que en es e ipo de ehículos es donde más se u iliza VINS-Mono debido a
que el sis ema isuo-ine cial u ilizado (una cáma a monocula e IMU) supone bajo peso y poco
50 Capí ulo 5. Algo i mo VINS-Mono: Simulaciones
(a) MH_01_easy. (b) MH_02_easy.
(c) MH_03_medium. (d) MH_04_di icul .
Figu a 5.9
G á icas ob enidas a pa i de la he amien a e o_epo. Con ienen la ayec o ia es imada
jun o con el ni el de e o come ido.
Figu a 5.10 A d one 2.0.
consumo de po encia.
Pa a con ola el A d one 2.0 en Gazebo se ha u ilizado un paque e denominado a d one_au opilo
[
2
]. Con es e paque e es posible despega y a e iza el d on, aslada lo en odas las di ecciones,
o a y cambia de cáma a. En la simulación, debido a las limi aciones de Gazebo, no es án mode-
lados odos los senso es del A d one 2.0 al 100%. Es án implemen adas un pa de cáma as (una
on al y o a di igida hacia el suelo), un senso de al i ud y un senso IMU.
Pa a pode ejecu a el algo i mo VINS-Mono y es ima los es ados del d on al mismo iempo que
es e se con ola en la simulación de Gazebo es necesa io conoce , en p ime luga , los ópicos donde
se publican las medidas de los senso es que nos in e esan. Po un lado, las medidas de la IMU son
publicadas en a d one imu. En cuan o al ópico donde se publican las imágenes cap adas po las
cáma as, se a an de dos ópicos di e en es debido a que iene una cáma a on al y o a di igida
5.3 Vehículo aé eo Gazebo 51
Figu a 5.11 En o no de A d one 2.0 en Gazebo.
hacia el suelo. Pa a el algo i mo VINS-Mono se u iliza la cáma a on al debido a que cap a más
iqueza de elemen os en el en o no y mejo a á la es imación. El ópico de dicha cáma a es a d one
on image_mono.
En es e expe imen o, se a a supone que la posición y o ien ación de la cáma a espec o la IMU
son desconocidas, po lo que el alo del pa áme o es ima e_ex insic se ija a 2 ( ealmen e se puede
ob ene a pa i de los a chi os de con igu ación desca gados con el paque e um_simula o ). Es o
signi ica que al comienzo de la simulación se ealiza á la calib ación de los pa áme os ex ínsecos
cáma a-IMU y se á necesa io o a y desplaza el d on en odas las di ecciones du an e unos
segundos. En lo que espec a a los pa áme os in e nos de la cáma a, los cuales son necesa ios pa a
modi ica el a chi o de con igu ación de VINS-Mono, se pueden consul a en el ópico a d one
on image_mono came a_in o. En es e ópico, los pa áme os son publicados po el simulado de
Gazebo, como se puede obse a en los mensajes publicados en 5.12. Gene almen e la in o mación
sob e los pa áme os de la cáma a se publican en un ópico en el que apa ezca "came a_in o".
Código 5.12 Mensaje publicado en el ópico a d one on image_mono came a_in o.
heade :
seq: 8726
s amp:
secs: 872
nsecs: 745000000
ame_id: "a d one_base_ on cam"
heigh : 360
wid h: 640
dis o ion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [374.6706070969281, 0.0, 320.5, 0.0, 374.6706070969281, 180.5, 0.0,
0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [374.6706070969281, 0.0, 320.5, -0.0, 0.0, 374.6706070969281, 180.5,
0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
oi:
x_o se : 0
y_o se : 0

52 Capí ulo 5. Algo i mo VINS-Mono: Simulaciones
heigh : 0
wid h: 0
do_ ec i y: False
Como se puede e en el código, apa ecen odos los pa áme os necesa ios sob e la cáma a que
se comen a on an e io men e: al o y ancho de la imagen, pa áme os de dis o sión y pa áme os de
p oyección.
O a o ma de ob ene los pa áme os de la cáma a es busca la con igu ación del senso den o
de los a chi os desca gados del paque e de Gazebo. En es os a chi os se puede encon a el en o no
que se u iliza, el modelo del d on, los senso es que posee, los plugins que u iliza, e c. Es jus amen e
la in o mación de los plugins la que nos in e esa pa a conoce los pa áme os an o de la cáma a
como de la IMU. Es a in o mación se puede ob ene de los códigos 5.13 y 5.14, espec i amen e.
Código 5.13 Con igu ación de la cáma a en el A d one 2.0.
<xac o:include ilename="$( ind c g_sim_gazebo)/u d /senso s/
gene ic_came a.u d .xac o" />
<xac o:gene ic_came a name=" on " sim_name="a d one" pa en ="
base_link" upda e_ a e="60" es_x="640" es_y="360" image_ o ma
="R8G8B8" h o ="${81*M_PI/180}">
<o igin xyz="0.21 0.0 0.01" py="0 0 0"/>
</xac o:gene ic_came a>
------------------------------------------
<plugin name="${name}_came a_con olle " ilename="
libgazebo_ os_came a.so">
<came aName>/${sim_name}/${name}</came aName>
<alwaysOn> ue</alwaysOn>
<upda eRa e>${upda e_ a e}</upda eRa e>
<imageTopicName>/${sim_name}/${name}/image_ aw</imageTopicName>
<came aIn oTopicName>/${sim_name}/${name}/came a_in o</
came aIn oTopicName>
< ameName>a d one_base_${name}cam</ ameName>
</plugin>
Código 5.14 Con igu ación de la IMU en el A d one 2.0.
<plugin name="quad o o _imu_sim" ilename="libhec o _gazebo_ os_imu.so">
<alwaysOn> ue</alwaysOn>
<upda eRa e>100.0</upda eRa e>
<bodyName>base_link</bodyName>
< ameId>a d one_base_link</ ameId>
< opicName>/a d one/imu</ opicName>
< pyO se s>0 0 0</ pyO se s> <!-- dep eca ed -->
<gaussianNoise>0</gaussianNoise> <!-- dep eca ed -->
<accelD i >0.5 0.5 0.5</accelD i >
<accelGaussianNoise>0.35 0.35 0.3</accelGaussianNoise>
< a eD i >0.0 0.0 0.0</ a eD i >
5.4 Vehiculo e es e Gazebo 53
< a eGaussianNoise>0.00 0.00 0.00</ a eGaussianNoise>
<headingD i >0.0</headingD i >
<headingGaussianNoise>0.00</headingGaussianNoise>
</plugin>
Una ez modi icado el a chi o de con igu ación, se ealiza la es imación de la ayec o ia con el
algo i mo VINS-Mono. Pa a ello, se u ilizan los comandos obse ados en 5.15:
•La p ime a línea lanza la simulación del d on del paque e A d one 2.0.
•
La segunda se u iliza pa a pode con ola el d on a a és del eclado y mo e lo po el en o no
con el paque e desca gado de a d one_au opilo .
•
La e ce a línea se u iliza pa a modi ica el ipo de o ma o de imagen que ienen las imágenes
cap adas po la cáma a on al. Es e o ma o es RBG, el cual no es compa ible con el algo i mo
VINS-Mono. De es a o ma, se co ige ans o mando las imágenes a o ma o monoc omá ico.
Pa a ello se u iliza el paque e image_p oc [4].
•
Las úl imas líneas ya se ie on en el expe imen o del da ase de EuRoC MAV, pues son los
comandos necesa ios pa a lanza el algo i mo VINS-Mono y pode ab i el simulado R iz
(es as líneas son comunes en odos los expe imen os).
Código 5.15 Lanza expe iemn o A d one 2.0.
oslaunch c g_gazebo a d one_ es wo d.launch
oslaunch a d one_au opilo au opilo .launch
ROS_NAMESPACE=a d one/ on os un image_p oc image_p oc
oslaunch ins_es ima o eu oc.launch
slaunch ins_es ima o ins_ iz.launch
Los esul ados ob enidos se pueden obse a 5.12. Es a imagen se ha omado del isualizado
RViz. Además de la ayec o ia es imada del d on, se pueden obse a las imágenes que cap a
la cáma a y ambién las ca ac e ís icas ex aídas de las imágenes. Es as son sólo algunas de las
opciones que o ece RViz, ya que se puede isualiza la in o mación de cualquie ópico de la
simulación.
El a chi o comple o de con igu ación u ilizado pa a lanza es a simulación apa ece comple o en
7.5.
5.4 Vehiculo e es e Gazebo
El úl imo expe imen o ealizado pa a mos a los esul ados del algo i mo VINS-Mono iene como
obje i o es ima la ayec o ia de un ehículo e es e, no como en los casos an e io es que se
a aban de ehículos aé eos (los cuales son los ehículos más comunes en los que se suele u iliza
es e algo i mo). Pa a es e expe imen o no sólo se ha lanzado el algo i mo y se ha modi icado el
a chi o de con igu ación, sino que se ha cons uido la simulación desde ce o.
Tal y como se comen ó en el apa ado 4.3, se ha c eado an o el ehículo como el en o no po el
que se mue e. En p ime luga , el en o no se ha o mado po elemen os ya c eados po Gazebo:
calles, pa ques, casas, es au an es, a olas, señales de á ico...simulando un en o no eal. Cuan os
más elemen os con enga el en o no, mayo se á la iqueza de ca ac e ís icas en las imágenes omadas
54 Capí ulo 5. Algo i mo VINS-Mono: Simulaciones
Figu a 5.12 T ayec o ia ob enida de A d one 2.0 a pa i del algo i mo VINS-MONO.
Figu a 5.13 Vehículo e es e modelado en Gazebo.
po las cáma as, y mejo se á la es imación. Es a es la azón p incipal po la que la cáma a u ilizada
pa a el algo imo VINS-Mono es la cáma a la e al y no la cáma a on al: la cáma a la e al pe cibe
muchos más elemen os del en o no (la cáma a on al pe cibe sob e odo la ca e e a, mien as que
la la e al cap a odos los elemen os a ambos lados de la calle).
Además del en o no, ambién se ha c eado el ehículo e es e. Es e es á o mado po cua o
uedas y el chasis ( igu a 5.13). De cada elemen o se han indicado sus dimensiones, ine cias y o as
p opiedades (po ejemplo, en las uedas, ambién se especi ica el coe icien e de ozamien o). Pa a
ob ene medidas ú iles que puedan u iliza se pa a la es imación de la ayec o ia en VINS-Mono,
se han añadido plugins pa a los senso es de las cáma as y de la IMU. El código de es os ya ue
explicado en el capí ulo 4.3.1.
Además de los plugins de la IMU y de las cáma as, se ha añadido un plugin pa a el con ol del
ehículo (código 5.16). Es e plugin ecibe los mensajes de las elocidades lineales espec o xey, y
la elocidad angula espec o z(a a és del ópico cmd_ el). Po o o lado, publica en el ópico
odom la posición eal (o medidas g ound_ u h) que sigue el ehículo, la cual se u iliza á más
adelan e pa a calcula el e o come ido en la es imación de la ayec o ia.
5.4 Vehiculo e es e Gazebo 55
Código 5.16 Plugin de con ol.
<gazebo>
<plugin name="objec _con olle " ilename="libgazebo_ os_plana _mo e.
so">
<commandTopic>cmd_ el</commandTopic>
<odome yTopic>odom</odome yTopic>
<odome yF ame>odom</odome yF ame>
<odome yRa e>50.0</odome yRa e>
< obo BaseF ame>base_ oo p in </ obo BaseF ame>
<publishTF> alse</publishTF>
</plugin>
</gazebo>
Pa a con ola el ehículo e indica las elocidades mencionadas an e io men e que necesi a el
plugin de con ol pa a mo e el ehíchulo se u iliza un paque e de ROS que ecibe el nomb e de
eleop_ wis _keyboa d [
3
]. Es e pe mi e con ola el ehículo a a és del eclado (desplaza el
ehículo, o a , aumen a elocidad de desplazamien o y de gi o). Es e paque e se ejecu a a la ez
que se lanza la simulación de Gazebo, sin necesidad de ejecu a lo en un e minal di e en e. Es o
nos da la libe ad de con ola el ehículo en iempo eal.
Una ez c eado an o el en o no como el modelo del ehículo, y ya se es án publicando las
medidas de la IMU y de la cáma a la e al en sus co espondien es ópicos, se p ocede a modi ica el
a chi o de con igu ación del algo i mo pa a ealiza la es imación de la ayec o ia.
En p ime luga , los ópicos donde se publican las medidas se han elegido al c ea el modelo del
coche, y se han especi icado en el plugin del senso co espondien e. En es e caso, los nomb es
de los ópicos son /imu0 y/cocheci o/came a_la /image_mono. Po o o lado, como el ehículo
ha sido c eado desde ce o, ambién se conoce pe ec amen e la ma iz de pa áme os ex ínsecos
en e la cáma a (la e al) y el senso IMU. La posición ela i a en e ambos senso es se puede e en
la imagen 5.14. Es a igu a se ha ob enido a pa i del p og ama Ma lab. A pa i de la ma iz de
pa áme os ex ínsecos calculada y los ejes de la IMU, se han ob enido los ejes de la cáma a la e al.
Es á cla o que si se desease u iliza la cáma a on al, la ma iz de pa áme os ex ínsecos debe ía
se calculada de nue o.
La ma iz de pa áme os ex ínsecos dependen an o de la o ación como de la aslación en e los
senso es. Se deben ob ene los pa áme os ex ínsecos de la cáma a espec o la IMU y, po an o,
pa a calcula los se deben de gi a y aslada los ejes de la IMU has a que coincidan con la cáma a.
En el código 5.17 se pueden obse a el ángulo que se gi a espec o cada eje y la aslación necesa ia.
El obje i o p incipal de ob ene el g á ico en Ma lab es la comp obación de que los pa áme os
ex ínsecos que se han calculado son co ec os. Es o se puede comp oba al e la posición ela i a
en e la cáma a y la IMU con los pa áme os calculados. Po úl imo, ema ca que el eje zde la
cáma a es aquel que apun a hacia las imágenes que se oman.
Código 5.17 Ob ención g á ico de la posición ela i a en e la IMU y la cáma a.
he a_ini=180;
phi_ini=0;
alpha_ini=90;
he a= he a_ini*(2*pi/360);%angulo que gi a al ededo de z

6 Conclusión
El p oblema de VINS ha conseguido bas an es p og esos du an e los úl imos 20 años. El obje i o
p incipal que VINS pe sigue es consegui la mayo p ecisión posible en sus es imaciones.
Se debe esal a la impo ancia de VINS en los obo s au ónomos (se necesi a conoce la locali-
zación de es os obo s en odo momen o). Pa a odas sus aplicaciones, es necesa io conoce dónde
se encuen an pa a pode sabe cuál se á el siguien e mo imien o necesa io. Además, una en aja
del algo i mo VINS-Mono es que unciona en luga es donde las medidas GPS son inaccesibles.
Como se ha comen ado a lo la go de es e abajo, VINS-Mono es un es imado de es ados 6
DOF isuo-ine cial, el cual es á basado en el mé odo de op imización (consiguen mayo p ecisión
y equie en menos memo ia). Es e algo i mo ealiza la es imación de la ayec o ia a pa i una
cáma a monocula y una IMU. Pa a e alua es e algo i mo se han ealizado di e en es expe imen os
y se han mos ado an o las es imaciones ob enidas como los e o es p oducidos en es as.
Los expe imen os ealizados han sido a iados pa a pode e alua el algo i mo. Po un lado, se ha
p obado VINS-Mono en ehículos aé eos (se co esponde al expe imen o del da ase EuRoC MAV
y al expe imen o del A d one 2.0), y en ehículos e es es (se co esponde con la simulación de
Gazebo c eada desde ce o). Además, ambién se ha p obado an o en iempo eal como a pa i de
una bolsa de imágenes (o osbag). De cada expe imen o se han calculado los e o es pa a pode e
la calidad de la es imación.
G acias a los expe imen os ealizados con los da ase s EuRoC MAV se ha podido e alua el
algo imo en si uaciones con di e en e complejidad. Se ha obse ado que la ayec o ia es imada po
VINS-Mono su e mayo e o cuando la di icul ad aumen a, pe o igualmen e ob iene muy buenos
esul ados.
Se an a ema ca algunos de los aspec os más impo an es a ene en cuen a pa a que VINS-
Mono ealice una buena es imación de la ayec o ia. Po un lado, se encuen a la calib ación
de los pa áme os ex ínsecos IMU-cáma a (de odas o mas, si no se iene es a in o mación, el
algo i mo iene la en aja de pode calcula los al comienzo del expe imen o y gua da esos alo es
pa a expe imen os u u os). Po o o lado, ambién es muy impo an e la iqueza en ex u as y
elemen os del en o no. El algo i mo es capaz de de ec a escenas epe idas g acias a la obse ación
de ca ac e ís icas comunes en e las imágenes. Po an o, si las imágenes cap adas po las cáma as
ienen bas an es ca ac e ís icas a ex ae , se á mucho más ácil encon a simili udes en e imágenes.
O o aspec o impo an e es la ecuencia mínima a la que deben publica las medidas la IMU y la
cáma a, la cual es 100 Hz y 20Hz, espec i amen e (es o es un equisi o undamen al).
63
64 Capí ulo 6. Conclusión
Aunque los algo i mos VINS han alcanzado bas an e madu ez, oda ía exis en muchos en es
abie os pa a u u as in es igaciones. A eces se pueden da condiciones del en o no di íciles o
degene adas. Una de las posibles mejo as pa a la es imación puede se la inclusión de más senso es
con el in de minimiza los e o es, ó con el in de mejo a el uncionamien o en en o nos y si ua-
ciones donde la combinación cáma a-IMU no dan una solución óp ima.
7 Anexo
7.1 my obo .wo ld
Código 7.1 A chi o del mundo.
<?xml e sion="1.0" ?>
<sd e sion="1.4">
<wo ld name="de aul ">
<include>
<u i>model://g ound_plane</u i>
</include>
<include>
<u i>model://sun</u i>
</include>
<!--CARRETERAS-->
< oad name="my_ oad1">
<wid h>2</wid h>
<poin >-5 0 0</poin >
<poin >5 0 0</poin >
</ oad>
< oad name="my_ oad2">
<wid h>2</wid h>
<poin >4 1 0</poin >
<poin >4 11 0</poin >
</ oad>
< oad name="my_ oad3">
<wid h>2</wid h>
<poin >-6 -1 0</poin >
<poin >-6 11 0</poin >
</ oad>
< oad name="my_ oad4">
<wid h>2</wid h>
<poin >-5 8 0</poin >
65
66 Capí ulo 7. Anexo
<poin >3 8 0</poin >
</ oad>
<!--ACERAS-->
< oad name="Pedes ian1">
<wid h>1</wid h>
<poin >-5 1.5 0</poin >
<poin >3 1.5 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Pedes ian</name>
</sc ip >
</ma e ial>
</ oad>
< oad name="Pedes ian2">
<wid h>1</wid h>
<poin >-8 -1.5 0</poin >
<poin >5 -1.5 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Pedes ian</name>
</sc ip >
</ma e ial>
</ oad>
< oad name="Pedes ian3">
<wid h>1</wid h>
<poin >2.5 1.5 0</poin >
<poin >2.5 11 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Pedes ian</name>
</sc ip >
</ma e ial>
</ oad>
< oad name="Pedes ian5">
<wid h>1</wid h>
<poin >5.5 -2 0</poin >
<poin >5.5 11 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Pedes ian</name>
</sc ip >
</ma e ial>
7.1 my obo .wo ld 67
</ oad>
< oad name="Pedes ian6">
<wid h>1</wid h>
<poin >-4.5 2 0</poin >
<poin >-4.5 11 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Pedes ian</name>
</sc ip >
</ma e ial>
</ oad>
< oad name="Pedes ian7">
<wid h>1</wid h>
<poin >-7.5 -2 0</poin >
<poin >-7.5 11 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Pedes ian</name>
</sc ip >
</ma e ial>
</ oad>
< oad name="Pedes ian8">
<wid h>1</wid h>
<poin >-4 6.5 0</poin >
<poin >2 6.5 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Pedes ian</name>
</sc ip >
</ma e ial>
</ oad>
< oad name="Pedes ian9">
<wid h>1</wid h>
<poin >-4 9.5 0</poin >
<poin >2 9.5 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Pedes ian</name>
</sc ip >
</ma e ial>
</ oad>

68 Capí ulo 7. Anexo
< oad name="g ass0"><!--cesped columpio-->
<wid h>4</wid h>
<poin >-4 4 0</poin >
<poin >2 4 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/G ass</name>
</sc ip >
</ma e ial>
</ oad>
< oad name="g ass1"><!--cesped as ood-->
<wid h>6</wid h>
<poin >-10 -5 0</poin >
<poin >6 -5 0</poin >
<ma e ial>
<sc ip >
<u i> ile://media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/G ass</name>
</sc ip >
</ma e ial>
</ oad>
<!-- lechas-->
<include>
<u i>model://a ow_ ed</u i>
<name>a ow_0</name>
<pose>1 0 0.01 -2.5 -1.57 0.94</pose>
</include>
<include>
<u i>model://a ow_ ed</u i>
<name>a ow_1</name>
<pose>3.10 0 0.01 -2.5 -1.57 0.94</pose>
</include>
<include>
<u i>model://a ow_ ed</u i>
<name>a ow_2</name>
<pose>4 1.7 0.01 0 -1.57 0.0</pose>
</include>
<include>
<u i>model://a ow_ ed</u i>
<name>a ow_3</name>
<pose>4 3.68 0.01 0 -1.57 0.0</pose>
</include>
<include>
<u i>model://a ow_ ed</u i>
<name>a ow_4</name>
<pose>4 5.68 0.01 0 -1.57 0.0</pose>
</include>
<include>
7.1 my obo .wo ld 69
<u i>model://a ow_ ed</u i>
<name>a ow_5</name>
<pose>4 7.68 0.01 0 -1.57 0.0</pose>
</include>
<include>
<u i>model://a ow_ ed</u i>
<name>a ow_6</name>
<pose>2.31 8 0 1.57 -1.57 0.0</pose>
</include>
<include>
<u i>model://a ow_ ed</u i>
<name>a ow_7</name>
<pose>0.31 8 0 1.57 -1.57 0.0</pose>
</include>
<include>
<u i>model://a ow_ ed</u i>
<name>a ow_8</name>
<pose>-1.66 8 0 1.57 -1.57 0.0</pose>
</include>
<include>
<u i>model://a ow_ ed</u i>
<name>a ow_9</name>
<pose>-3.66 8 0 1.57 -1.57 0.0</pose>
</include>
<!--CASAS-->
<include>
<u i>model://house_1</u i><!--se han modi icado los .dae pa a que
las casas sean mas pequenas-->
<name>House1_0</name>
<pose>8.32 4.7 0 0 0 -1.57</pose>
</include>
<include>
<u i>model://house_1</u i>
<name>House1_1</name>
<pose>8.32 9.95 0 0 0 -1.57</pose>
</include>
<include>
<u i>model://house_3</u i>
<name>House3_0</name>
<pose>-8.92 0.78 0 0 0 1.57</pose>
</include>
<include>
<u i>model://house_3</u i>
<name>House3_1</name>
<pose>-8.92 5 0 0 0 1.57</pose>
</include>
<include>
70 Capí ulo 7. Anexo
<u i>model://house_3</u i>
<name>House3_2</name>
<pose>-8.76 9.23 0 0 0 1.57</pose>
</include>
<include>
<u i>model://house_2</u i>
<name>House2_0</name>
<pose>2.38 -4 0 0 0 0</pose>
</include>
<include>
<u i>model://playg ound</u i>
<name>playg ound</name>
<pose>-1.06 3.84 0 0 0 0</pose>
</include>
<include>
<u i>model:// oun ain</u i>
<name> oun ain</name>
<pose>-6.4 -2.7 0 0 0 0</pose>
</include>
<include>
<u i>model://pos _o ice</u i>
<name>pos _o ice</name>
<pose>7.27 -0.55 0 0 0 1.57</pose>
</include>
<include>
<u i>model://school</u i>
<name>school</name>
<pose>-1.02 14 0 0 0 0</pose>
</include>
<!--RESTAURANTE COMIDA RAPIDA Y MESAS-->
<include>
<u i>model:// as _ ood</u i>
<name> as _ ood</name>
<pose>-2.79 -4.69 0 0 0 0</pose>
</include>
<include>
<u i>model://ca e_ able</u i>
<name>ca e_ able_0</name>
<pose>-6.69 -5.29 0 0 0 0</pose>
</include>
<include>
<u i>model://ca e_ able</u i>
<name>ca e_ able_1</name>
<pose>-8.08 -5.29 0 0 0 0</pose>
7.1 my obo .wo ld 71
</include>
<include>
<u i>model://ca e_ able</u i>
<name>ca e_ able_2</name>
<pose>-8.08 -6.68 0 0 0 0</pose>
</include>
<include>
<u i>model://ca e_ able</u i>
<name>ca e_ able_3</name>
<pose>-6.69 -6.68 0 0 0 0</pose>
</include>
<include>
<u i>model://pe son_s anding</u i>
<name>pe son_s anding_0</name>
<pose>5.81 1.74 0 0 0 -1.57</pose>
</include>
<!--PERSONAS ESPERANDO COMIDA RAPIDA-->
<include>
<u i>model://pe son_s anding</u i>
<name>pe son_s anding_1</name>
<pose>-2.44 -1.48 0 0 0 0</pose>
</include>
<include>
<u i>model://pe son_s anding</u i>
<name>pe son_s anding_2</name>
<pose>-1.48 -1.56 0 0 0 -1.57</pose>
</include>
<include>
<u i>model://pe son_s anding</u i>
<name>pe son_s anding_3</name>
<pose>-0.77 -1.57 0 0 0 -1.57</pose>
</include>
<include>
<u i>model://pe son_s anding</u i>
<name>pe son_s anding_4</name>
<pose>-0 -1.58 0 0 0 3.11</pose>
</include>
<include>
<u i>model://pe son_s anding</u i>
<name>pe son_s anding_5</name>
<pose>0.66 -1.54 0 0 0 -1.82</pose>
</include>
<include>
<u i>model://pe son_walking</u i>
<name>pe son_walking_0</name>
<pose>-1.22 1.51 0 0 0 1.57</pose>
</include>
<include>
<u i>model://pe son_walking</u i>
78 Capí ulo 7. Anexo
<o igin xyz="0.4 0 ${heigh /2}" py="0 0 0"/><!-- he es ado
cambiando el alo de xyz-->
<pa en link="chassis"/>
<child link="came a_link"/>
</join >
<link name="came a_link">
<collision>
<o igin xyz="0 0 0.0" py="0 0 0"/>
<geome y>
<box size="${came a_link} ${came a_link} ${came a_link}"/>
</geome y>
</collision>
< isual>
<o igin xyz="0 0 0.0" py="0 0 0"/>
<geome y>
<mesh scale ="0.5 0.5 0.5" ilename="model://kinec /meshes/kinec .
dae"/>
</geome y>
<ma e ial name="black"/>
</ isual>
<ine ial>
<mass alue="1e-5" />
<o igin xyz="0 0 0" py="0 0 0"/>
<ine ia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"
/>
</ine ial>
</link>
<!-- Came a la e al-->
<join name="came a2_join " ype=" ixed">
<axis xyz="0 1 0" />
<o igin xyz="0 -0.35 ${heigh /2}" py="0 0 -1.57"/><!-- se pone en
un lado del coche-->
<pa en link="chassis"/>
<child link="came a2_link"/>
</join >
<link name="came a2_link">
<collision>
<o igin xyz="0 0 0.0" py="0 0 0"/>
<geome y>
<box size="${came a_link} ${came a_link+0.1} ${came a_link}"/>
</geome y>
</collision>
< isual>

7.2 my obo .xac o 79
<o igin xyz="0 0 0.0" py="0 0 0"/>
<geome y>
<box size="${came a_link} ${came a_link+0.1} ${came a_link}"/>
<!-- <mesh scale ="0.5 0.5 0.5" ilename="model://kinec /meshes/
kinec .dae"/>-->
</geome y>
<ma e ial name="black"/>
</ isual>
<ine ial>
<mass alue="1e-5" />
<o igin xyz="0 0 0" py="0 0 0"/>
<ine ia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"
/>
</ine ial>
</link>
<!--IMU-->
<join name="imu_join " ype=" ixed">
<axis xyz="1 0 0"/> <!-- 0 1 0 -->
<o igin xyz="0 0 0"/>
<pa en link="chassis"/>
<child link="imu_link"/>
</join >
<link name="imu_link">
<ine ial>
<mass alue="0.001"/>
<o igin py="0 0 0" xyz="0 0 0"/>
<ine ia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz
="0.0001"/>
</ine ial>
< isual>
<o igin py="0 0 0" xyz="0 0 0"/>
<geome y>
<box size="0.1 0.1 0.1"/>
</geome y>
</ isual>
<collision>
<o igin py="0 0 0" xyz="0 0 0"/>
<geome y>
<box size="0.1 0.1 0.1"/>
</geome y>
</collision>
</link>
80 Capí ulo 7. Anexo
</ obo >
7.3 my obo .gazebo
Código 7.3 A chi o que con iene los plugins del ehículo e es e.
<?xml e sion="1.0"?>
< obo >
<gazebo e e ence="base">
< u nG a i yO > alse</ u nG a i yO >
</gazebo>
<!--Rueda de echa delan e a-->
<gazebo e e ence=" on _ igh _wheel">
<mu1 alue="1.0"/>
<mu2 alue="1.0"/>
<kp alue="10000000.0"/>
<kd alue="1.0"/>
< di 1 alue="1 0 0"/>
<!--<ma e ial>Gazebo/G ey</ma e ial>-->
< u nG a i yO > alse</ u nG a i yO >
</gazebo>
<!--Rueda izquie da delan e a-->
<gazebo e e ence=" on _le _wheel">
<mu1 alue="1.0"/>
<mu2 alue="1.0"/>
<kp alue="10000000.0"/>
<kd alue="1.0"/>
< di 1 alue="1 0 0"/>
<!-- <ma e ial>Gazebo/G ey</ma e ial>-->
< u nG a i yO > alse</ u nG a i yO >
</gazebo>
<!--Rueda de echa ase a-->
<gazebo e e ence="back_ igh _wheel">
<mu1 alue="1.0"/>
<mu2 alue="1.0"/>
<kp alue="10000000.0"/>
<kd alue="1.0"/>
< di 1 alue="1 0 0"/>
<!--<ma e ial>Gazebo/G ey</ma e ial>-->
< u nG a i yO > alse</ u nG a i yO >
</gazebo>
<!--Rueda izquie da ase a-->
7.3 my obo .gazebo 81
<gazebo e e ence="back_le _wheel">
<mu1 alue="1.0"/>
<mu2 alue="1.0"/>
<kp alue="10000000.0"/>
<kd alue="1.0"/>
< di 1 alue="1 0 0"/>
<!-- <ma e ial>Gazebo/G ey</ma e ial>-->
< u nG a i yO > alse</ u nG a i yO >
</gazebo>
<!-- came a delan e a PLUGIN-->
<gazebo e e ence="came a_link">
<senso name="came a" ype="dep h">
<upda e_ a e>30</upda e_ a e><!-- iene que se 20 ps como min-->
<came a>
<ho izon al_ o >1.047198</ho izon al_ o >
<image>
<wid h>752</wid h><!--<wid h>640</wid h>--><!--cambiado pa a
ue coincida con da os del eu oc_con ig-->
<heigh >480</heigh >
< o ma >R8G8B8</ o ma >
</image>
<clip>
<nea >0.05</nea >
< a >3</ a >
</clip>
</came a>
<plugin name="came a_plugin" ilename="libgazebo_ os_openni_kinec .so
">
<baseline>0.2</baseline>
<alwaysOn> ue</alwaysOn>
<!-- Keep his ze o, upda e_ a e in he pa en <senso > ag
will con ol he ame a e. -->
<upda eRa e>0.0</upda eRa e><!--nume o de imagenes que oma la
cama a po segundo(si es a a ce o la eq es a misma que la
del pa en senso -->
<came aName>came a_i </came aName>
<imageTopicName>/cam0/colo /image_ aw</imageTopicName>
<came aIn oTopicName>/cam0/colo /came a_in o</
came aIn oTopicName>
<dep hImageTopicName>/cam0/dep h/image_ aw</dep hImageTopicName
>
<dep hImageCame aIn oTopicName>/cam0/dep h/came a_in o</
dep hImageCame aIn oTopicName>
<poin CloudTopicName>/cam0/dep h/poin s</poin CloudTopicName>
< ameName>came a_link</ ameName>
<poin CloudCu o >0.5</poin CloudCu o ><!--min y max dis ancia
en e pun os-->
<poin CloudCu o Max>3.0</poin CloudCu o Max>
<Cx>0</Cx>
82 Capí ulo 7. Anexo
<Cy>0</Cy>
< ocalLeng h>0</ ocalLeng h>
<hackBaseline>0</hackBaseline>
</plugin>
</senso >
</gazebo>
<!-- came a la e al PLUGIN-->
<gazebo e e ence="came a2_link">
<senso ype="came a" name="came a_la ">
< isualize> ue</ isualize>
<upda e_ a e>60</upda e_ a e>
<came a name="came a_la ">
<ho izon al_ o >1.413</ho izon al_ o >
<image>
<wid h>640</wid h>
<heigh >360</heigh >
< o ma >R8G8B8</ o ma >
</image>
<clip>
<nea >0.01</nea >
< a >100</ a >
</clip>
</came a>
<plugin name="came a_con olle " ilename="libgazebo_ os_came a.
so">
<came aName>cocheci o/came a_la </came aName>
<alwaysOn> ue</alwaysOn>
<upda eRa e>60</upda eRa e>
<imageTopicName>/cocheci o/came a_la /image_ aw</imageTopicName
>
<came aIn oTopicName>/cocheci o/came a_la /came a_in o</
came aIn oTopicName>
< ameName>cocheci o_base_cam_la </ ameName>
</plugin>
</senso >
< u nG a i yO > ue</ u nG a i yO >
</gazebo>-->
<!--IMU PLUGIN-->
<gazebo e e ence="imu_link">
<g a i y> ue</g a i y>
<senso name="imu_senso " ype="imu">
<always_on> ue</always_on>
<upda e_ a e>200</upda e_ a e>
< opic>__de aul _ opic__</ opic>
7.4 A chi o de con igu ación eu oc_con ig.yaml del algo i mo VINS-Mono 83
<plugin ilename="libgazebo_ os_imu_senso .so" name="imu_plugin">
< opicName>imu0</ opicName>
<bodyName>imu_link</bodyName>
<upda eRa eHZ>200.0</upda eRa eHZ>
<gaussianNoise>0</gaussianNoise>
<xyzO se >0 0 0</xyzO se >
< pyO se >0 0 0</ pyO se >
< ameName>imu_link</ ameName>
<accelD i >0.5 0.5 0.5</accelD i >
<accelGaussianNoise>0.35 0.35 0.3</accelGaussianNoise>
< a eD i >0.0 0.0 0.0</ a eD i >
< a eGaussianNoise>0.00 0.00 0.00</ a eGaussianNoise>
<headingD i >0.0</headingD i >
<headingGaussianNoise>0.00</headingGaussianNoise>
</plugin>
</senso >
</gazebo>
<!--CONTROL-->
<gazebo>
<plugin name="objec _con olle " ilename="libgazebo_ os_plana _mo e.
so">
<commandTopic>cmd_ el</commandTopic>
<odome yTopic>odom</odome yTopic>
<odome yF ame>odom</odome yF ame>
<odome yRa e>50.0</odome yRa e>
< obo BaseF ame>base_ oo p in </ obo BaseF ame>
<publishTF> alse</publishTF>
</plugin>
</gazebo>
7.4 A chi o de con igu ación eu oc_con ig.yaml del algo i mo VINS-Mono
7.4.1 Expe imen o EuRoC MAV Da ase
Código 7.4 eu oc_con ig.yaml pa a el expe imen o de EuRoC MAV Da ase .
%YAML:1.0
#common pa ame e s
imu_ opic: "/imu0"
image_ opic: "/cam0/image_ aw"
ou pu _pa h: "/home/ma ia od/Esc i o io/TFG/expe imen os/da ase _EUROC4
/"

84 Capí ulo 7. Anexo
#came a calib a ion
model_ ype: PINHOLE
came a_name: came a
image_wid h: 752
image_heigh : 480
dis o ion_pa ame e s:
k1: -2.917e-01
k2: 8.228e-02
p1: 5.333e-05
p2: -1.578e-04
p ojec ion_pa ame e s:
x: 4.616e+02
y: 4.603e+02
cx: 3.630e+02
cy: 2.481e+02
# Ex insic pa ame e be ween IMU and Came a.
es ima e_ex insic: 0 # 0 Ha e an accu a e ex insic pa ame e s. We will
us he ollowing imu^R_cam, imu^T_cam, don’ change i .
# 1 Ha e an ini ial guess abou ex insic
pa ame e s. We will op imize a ound you
ini ial guess.
# 2 Don’ know any hing abou ex insic pa ame e s
. You don’ need o gi e R,T. We will y o
calib a e i . Do some o a ion mo emen a
beginning.
#I you choose 0 o 1, you should w i e down he ollowing ma ix.
#Ro a ion om came a ame o imu ame, imu^R_cam
ex insicRo a ion: !!openc -ma ix
ows: 3
cols: 3
d : d
da a: [0.0148655429818, -0.999880929698, 0.00414029679422,
0.999557249008, 0.0149672133247, 0.025715529948,
-0.0257744366974, 0.00375618835797, 0.999660727178]
#T ansla ion om came a ame o imu ame, imu^T_cam
ex insicT ansla ion: !!openc -ma ix
ows: 3
cols: 1
d : d
da a: [-0.0216401454975,-0.064676986768, 0.00981073058949]
# ea u e ake pap ame e s
max_cn : 150 # max ea u e numbe in ea u e acking
min_dis : 30 # min dis ance be ween wo ea u es
eq: 10 # equence (Hz) o publish acking esul . A
leas 10Hz o good es ima ion. I se 0, he equence will be same
as aw image
F_ h eshold: 1.0 # ansac h eshold (pixel)
show_ ack: 1 # publish acking image as opic
7.4 A chi o de con igu ación eu oc_con ig.yaml del algo i mo VINS-Mono 85
equalize: 1 # i image is oo da k o ligh , un on equalize
o ind enough ea u es
isheye: 0 # i using isheye, un on i . A ci cle mask will
be loaded o emo e edge noisy poin s
#op imiza ion pa ame e s
max_sol e _ ime: 0.04 # max sol e i a ion ime (ms), o gua an ee eal
ime
max_num_i e a ions: 8 # max sol e i a ions, o gua an ee eal ime
key ame_pa allax: 10.0 # key ame selec ion h eshold (pixel)
#imu pa ame e s The mo e accu a e pa ame e s you p o ide, he be e
pe o mance
acc_n: 0.08 # accele ome e measu emen noise s anda d de ia ion.
#0.2 0.04
gy _n: 0.004 # gy oscope measu emen noise s anda d de ia ion.
#0.05 0.004
acc_w: 0.00004 # accele ome e bias andom wo k noise s anda d
de ia ion. #0.02
gy _w: 2.0e-6 # gy oscope bias andom wo k noise s anda d de ia ion.
#4.0e-5
g_no m: 9.81007 # g a i y magni ude
#loop closu e pa ame e s
loop_closu e: 1 # s a loop closu e
load_p e ious_pose_g aph: 1 # load and euse p e ious pose g aph;
load om ’pose_g aph_sa e_pa h’
as _ elocaliza ion: 1 # use ul in eal- ime and la ge p ojec
pose_g aph_sa e_pa h: "/home/ma ia od/Esc i o io/TFG/expe imen os/
da ase _EUROC4/" # sa e and load pa h
#unsynch oniza ion pa ame e s
es ima e_ d: 0 # online es ima e ime o se be ween
came a and imu
d: 0.0 # ini ial alue o ime o se . uni : s.
eaded image clock + d = eal image clock (IMU clock)
# olling shu e pa ame e s
olling_shu e : 0 # 0: global shu e came a, 1: olling
shu e came a
olling_shu e _ : 0 # uni : s. olling shu e ead ou
ime pe ame ( om da a shee ).
# isualiza ion pa ame e s
sa e_image: 1 # sa e image in pose g aph o
isualiza ion p upose; you can close his unc ion by se ing 0
isualize_imu_ o wa d: 0 # ou pu imu o wa d p opoga ion o achie e
low la ency and high equence esul s
isualize_came a_size: 0.4 # size o came a ma ke in RVIZ
86 Capí ulo 7. Anexo
7.4.2 Expe imen o ehículo aé eo Gazebo
Código 7.5 eu oc_con ig.yaml pa a el expe imen o del A d one 2.0 en Gazebo.
%YAML:1.0
#common pa ame e s
imu_ opic: "/a d one/imu"
image_ opic: "/a d one/ on /image_mono"
ou pu _pa h: "/home/ma ia od/Esc i o io/TFG/expe imen os/a d one"
#came a calib a ion
model_ ype: PINHOLE
came a_name: came a
image_wid h: 640
image_heigh : 360
dis o ion_pa ame e s:
k1: 0
k2: 0
p1: 0
p2: 0
p ojec ion_pa ame e s:
x: 3.7467e+02
y: 3.7467e+02
cx: 3.205e+02
cy: 1.805e+02
# Ex insic pa ame e be ween IMU and Came a.
es ima e_ex insic: 2 # 0 Ha e an accu a e ex insic pa ame e s. We will
us he ollowing imu^R_cam, imu^T_cam, don’ change i .
# 1 Ha e an ini ial guess abou ex insic
pa ame e s. We will op imize a ound you
ini ial guess.
# 2 Don’ know any hing abou ex insic pa ame e s
. You don’ need o gi e R,T. We will y o
calib a e i . Do some o a ion mo emen a
beginning.
#I you choose 0 o 1, you should w i e down he ollowing ma ix.
#Ro a ion om came a ame o imu ame, imu^R_cam
ex insicRo a ion: !!openc -ma ix
ows: 3
cols: 3
d : d
da a: [0.0148655429818, -0.999880929698, 0.00414029679422,
0.999557249008, 0.0149672133247, 0.025715529948,
-0.0257744366974, 0.00375618835797, 0.999660727178]
#T ansla ion om came a ame o imu ame, imu^T_cam
ex insicT ansla ion: !!openc -ma ix
ows: 3
7.4 A chi o de con igu ación eu oc_con ig.yaml del algo i mo VINS-Mono 87
cols: 1
d : d
da a: [-0.0216401454975,-0.064676986768, 0.00981073058949]
# ea u e ake pap ame e s
max_cn : 150 # max ea u e numbe in ea u e acking
min_dis : 30 # min dis ance be ween wo ea u es
eq: 10 # equence (Hz) o publish acking esul . A
leas 10Hz o good es ima ion. I se 0, he equence will be same
as aw image
F_ h eshold: 1.0 # ansac h eshold (pixel)
show_ ack: 1 # publish acking image as opic
equalize: 1 # i image is oo da k o ligh , un on equalize
o ind enough ea u es
isheye: 0 # i using isheye, un on i . A ci cle mask will
be loaded o emo e edge noisy poin s
#op imiza ion pa ame e s
max_sol e _ ime: 0.04 # max sol e i a ion ime (ms), o gua an ee eal
ime
max_num_i e a ions: 8 # max sol e i a ions, o gua an ee eal ime
key ame_pa allax: 10.0 # key ame selec ion h eshold (pixel)
#imu pa ame e s The mo e accu a e pa ame e s you p o ide, he be e
pe o mance
acc_n: 0.08 # accele ome e measu emen noise s anda d de ia ion.
#0.2 0.04
gy _n: 0.004 # gy oscope measu emen noise s anda d de ia ion.
#0.05 0.004
acc_w: 0.00004 # accele ome e bias andom wo k noise s anda d
de ia ion. #0.02
gy _w: 2.0e-6 # gy oscope bias andom wo k noise s anda d de ia ion.
#4.0e-5
g_no m: 9.81007 # g a i y magni ude
#loop closu e pa ame e s
loop_closu e: 1 # s a loop closu e
load_p e ious_pose_g aph: 1 # load and euse p e ious pose g aph;
load om ’pose_g aph_sa e_pa h’
as _ elocaliza ion: 0 # use ul in eal- ime and la ge p ojec
pose_g aph_sa e_pa h: "/home/ma ia od/Esc i o io/TFG/expe imen os/
a d one" # sa e and load pa h
#unsynch oniza ion pa ame e s
es ima e_ d: 1 # online es ima e ime o se be ween
came a and imu
d: 0.0 # ini ial alue o ime o se . uni : s.
eaded image clock + d = eal image clock (IMU clock)
# olling shu e pa ame e s

Índice de Figu as
1.1 Ejemplos de ehículos aé eos 1
(a) Vehículo Aé eo de Comba e No T ipulado (UCAV) 1
(b) D on en el in e io de una áb ica 1
1.2 Ejemplos de ehículos e es es 2
(a) Robo de limpieza 2
(b) Vehículo au ónomo 2
1.3 Encode inc emen al. Senso u ilizado pa a de e mina la posición, elocidad o di ección 3
1.4 Tipos de cáma a en odome ía isual 4
(a) Cáma a es e eo 4
(b) Cáma a monocula 4
1.5 Ejes del d on y mo imien o sob e ellos 4
1.6 Senso es IMU y cáma a es e eoscópica u ilizados en el da ase de EuRoC MAV. Es e
da ase se u iliza en el p ime expe imen o del algo i mo de VINS-Mono 5
1.7 Es imación de la ayec o ia de un d on a pa i de un da ase de EuRoC MAV 5
2.1 Clasi icación VINS 8
2.2 F amewo k p opues o pa a la es imación de es ados que sopo a múl iples conjun os de
senso es 9
3.1 Esquema gene al VINS-Mono 11
3.2 De ección de ca ác e ís icas en una imagen 12
(a) Imagen o iginal 12
(b) Imagen con ca ac e ís icas encon adas 12
3.3 Esquema de la p ein eg ación IMU 13
3.4 Esquema esumen e apa de inicialización 15
3.5 Imagen del esiduo isual ep esen ado en una es e a unidad 20
3.6 P ocedimien o de ma ginalización 20
3.7 Pasos seguidos po el módulo de elocalización 22
3.8 T ayec o ia es imada del ehículo e es e con algo i mo VINS-Mono al al usiona la con
una ayec o ia gua dada de un expe imen o an e io 23
4.1 Comunicación en el en o no ROS 27
4.2 He amien a RViz que mues a la ayec o ia seguida po un obo en 3D 29
4.3 He amien a q _bag al ep oduci una osbag que con iene da os de ipo imagen 29
4.4 He amien a q _g aph que mues a conexiones en e nodos al ejecu a VINS-Mono 30
4.5 Di e en es is as de la casa ya c eada de Gazebo 30
(a) Vis a on al casa 30
95
96 Índice de Figu as
(b) Vis a gi ada casa 30
4.6 Di e en es is as del ehículo c eado en Gazebo 31
(a) Vis a on al ehículo 31
(b) Vis a gi ada ehículo 31
4.7 Modelo de una ueda de coche ya c eado po Gazebo 33
4.8 Las imágenes cap adas po la cáma a son isualizadas en la simulación ( isualize=T ue) 35
4.9 Nodos y ópicos c eados con la simulación de Gazebo. Imagen omada a pa i de la
he amien a q _g aph 37
5.1 Helicóp e o de 6 hélices u ilizado pa a la ecogida de los da os del da ase EuRoC MAV 42
5.2 Imágenes omadas po la cáma a izquie da del helicóp e o en ins an es alea o ios 43
5.3 T ayec o ia es imada VINS-Mono a pa i del da ase EuRoC MAV de di icul ad baja, jun o
con la ayec o ia eal (los colo es son e de y ojo, espec i amen e) 44
5.4 T ayec o ia gua dada VINS-Mono del da ase EuRoC MAV de di icul ad baja 44
5.5 T ayec o ia es imada VINS-Mono a pa i del da ase EuRoC MAV de di icul ad baja, jun-
o con una ayec o ia an e io ya gua dada y la ayec o ia eal (los colo es son e de,
na anja y ojo, espec i amen e 45
5.6 Imágenes u ilizadas en los da ase s EuRoC MAV 46
(a) MH_01_easy 46
(b) MH_02_easy 46
(c) MH_03_medium 46
(d) MH_04_di icul 46
5.7 G á icas ob enidas a pa i de la he amien a e o_ aj. Con ienen la ayec o ia es imada
y la ayec o ia eal de los cua o da ase s 48
(a) MH_01_easy 48
(b) MH_02_easy 48
(c) MH_03_medium 48
(d) MH_04_di icul 48
5.8 G á icas ob enidas a pa i de la he amien a e o_ aj (pa a el da ase MH_01_easy) 49
(a) Compa ación posiciones x,y,z49
(b) Compa ación ángulos oll,pi ch yyaw 49
5.9 G á icas ob enidas a pa i de la he amien a e o_epo. Con ienen la ayec o ia es imada
jun o con el ni el de e o come ido 50
(a) MH_01_easy 50
(b) MH_02_easy 50
(c) MH_03_medium 50
(d) MH_04_di icul 50
5.10 A d one 2.0 50
5.11 En o no de A d one 2.0 en Gazebo 51
5.12 T ayec o ia ob enida de A d one 2.0 a pa i del algo i mo VINS-MONO 54
5.13 Vehículo e es e modelado en Gazebo 54
5.14 Posición ela i a en e los senso es cáma a-IMU 56
5.15 Ejes de la cáma a e IMU sob e el ehículo 56
5.16 En o no de simulación c eado en Gazebo 57
(a) 57
(b) 57
5.17 T ayec o ia es imada del ehículo e es e con algo i mo VINS-Mono 58
5.18 T ayec o ia es imada del ehículo e es e con algo i mo VINS-Mono al u iliza una a-
yec o ia gua dada de un expe imen o an e io 58
Índice de Figu as 97
5.19 T ayec o ia inal gua dada del ehículo e es e con algo i mo VINS-Mono al u iliza una
ayec o ia gua dada de un expe imen o an e io 59
5.20 Compa ación en e la ayec o ia es imada y la ayec o ia eal del ehículo e es e de Gazebo 60
5.21 E o es espec o a los ejes x,y,z61
5.22 E o cuad á ico de la es imación de la ayec o ia del ehículo e es e 61
Índice de Tablas
5.1 E o es calculados a pa i de he amien a e o_ape (en me os) 48
99

Índice de Códigos
4.1 Comandos necesa ios pa a ins alación de paque e VINS-Mono 26
4.2 Ejemplo de un a chi o del mundo 29
4.3 F agmen o de código del modelo del ehículo. Rueda delan e a de echa 31
4.4 Plugin de la cáma a 33
4.5 Plugin de la IMU 35
5.1 Pa áme os comunes 39
5.2 Calib ación de la cáma a 40
5.3 Pa áme os ex ínsecos en e cáma a e IMU 40
5.4 Pa áme os IMU 41
5.5 Lanza expe imen o da ase EuRoC MAV 43
5.6 Modi ica unción pubOdome y() en a chi o isualiza ion.cpp 45
5.7 Modi ica unción upda ePa h() en a chi o pose_g aph.cpp 46
5.8 Modi ica unción addKeyF ame() en a chi o pose_g aph.cpp 47
5.9 Modi ica unción main() en a chi o pose_g aph_node.cpp 47
5.10 T ans o ma medidas g ound u h a o ma o TUM 47
5.11 U iliza he amien a e o_ aj 48
5.12 Mensaje publicado en el ópico a d one on image_mono came a_in o 51
5.13 Con igu ación de la cáma a en el A d one 2.0 52
5.14 Con igu ación de la IMU en el A d one 2.0 52
5.15 Lanza expe iemn o A d one 2.0 53
5.16 Plugin de con ol 54
5.17 Ob ención g á ico de la posición ela i a en e la IMU y la cáma a 55
5.18 Laza expe imen o ehículo e es e 57
5.19 Gua da in o mación del expe imen o en una osbag 58
5.20 Código de Ma lab donde se a an los da os de la osbag 59
5.21 Código de Ma lab donde se a an los da os de la osbag 60
7.1 A chi o del mundo 65
7.2 C eación del modelo del ehículo e es e 73
7.3 A chi o que con iene los plugins del ehículo e es e 80
7.4 eu oc_con ig.yaml pa a el expe imen o de EuRoC MAV Da ase 83
7.5 eu oc_con ig.yaml pa a el expe imen o del A d one 2.0 en Gazebo 86
7.6 eu oc_con ig.yaml pa a el expe imen o del ehículo e es e en Gazebo 88
7.7 Código Ma lab pa a ep esen a la disposición en e dos senso es 90
7.8 Código Ma lab pa a e alua VINS-Mono en el expe imen o del ehículo e es e en Gazebo 91
101
Bibliog a ía
[1] Paque e a d one,h p:// wiki. os.o g/ um_simula o .
[2] Paque e con ol a d one,h ps:// gi hub.com/ Ba ahlush/ a d one_au opilo .
[3] Paque e con ol ehiculo e es e,h p:// wiki. os.o g/ eleop_ wis _keyboa d.
[4] Paque e image p oc,h p:// wiki. os.o g/ image_p oc.
[5] Ros wiki,h p:// wiki. os.o g/ es.
[6] Samee Aga wal, Kei Mie le, and O he s, Ce es sol e ,h p:// ce es-sol e .o g.
[7] J.-Y. Bougue , Py amidal implemen a ion o he lucas kanade ea u e acke , 1999.
[8]
Michael Bu i, Janosch Nikolic, Pascal Gohl, Thomas Schneide , Joe n Rehde , Sammy Oma i,
Ma kus W Ach elik, and Roland Siegwa , The eu oc mic o ae ial ehicle da ase s, The
In e na ional Jou nal o Robo ics Resea ch (2016).
[9]
Michael Calonde , Vincen Lepe i , Ch is oph S echa, and Pascal Fua, B ie : Bina y obus
independen elemen a y ea u es, ol. 6314, 09 2010, pp. 778–792.
[10]
Ondřej Chum, Jiří Ma as, and Jose Ki le , Locally op imized ansac, Join Pa e n Recogni ion
Symposium, Sp inge , 2003, pp. 236–243.
[11]
Je ey Delme ico and Da ide Sca amuzza, A benchma k compa ison o monocula isual-
ine ial odome y algo i hms o lying obo s, 2018 IEEE In e na ional Con e ence on Robo ics
and Au oma ion (ICRA), IEEE, 2018, pp. 2502–2509.
[12]
Ch is ian Fo s e , Luca Ca lone, F ank Dellae , and Da ide Sca amuzza, Imu p ein eg a ion
on mani old o e icien isual-ine ial maximum-a-pos e io i es ima ion, Geo gia Ins i u e o
Technology, 2015.
[13]
Do ian Gál ez-López and J. D. Ta dós, Bags o bina y wo ds o as place ecogni ion in
image sequences, IEEE T ansac ions on Robo ics 28 (2012), no. 5, 1188–1197.
[14]
Michael G upp, e o: Py hon package o he e alua ion o odome y and slam.,h ps:// gi hub.
com/ MichaelG upp/ e o, 2017.
[15] Gaël Guennebaud, Benoî Jacob, e al., Eigen 3, h p://eigen. ux amily.o g, 2010.
[16]
Todd Lup on and Salah Sukka ieh, Visual-ine ial-aided na iga ion o high-dynamic mo ion
in buil en i onmen s wi hou ini ial condi ions, IEEE T ansac ions on Robo ics
28
(2011),
no. 1, 61–76.
103