DISEÑO DE UN PROTOTIPO DE SISTEMA DE ODOMETRÍA VISUAL ...
77
DISEÑO DE UN PROTOTIPO DE SISTEMA DE ODOMETRÍA VISUAL CARLOS ANDRÉS GUERRA HERNÁNDEZ LUIS SEBASTIÁN HIGUITA PALACIO UNIVERSIDAD DE SAN BUENAVENTURA MEDELLÍN FACULTAD DE INGENIERÍAS INGENIERÍA ELECTRÓNICA MEDELLÍN 2017
Transcript of DISEÑO DE UN PROTOTIPO DE SISTEMA DE ODOMETRÍA VISUAL ...
DISEÑO DE UN PROTOTIPO DE SISTEMA DE ODOMETRÍA VISUAL
CARLOS ANDRÉS GUERRA HERNÁNDEZ
LUIS SEBASTIÁN HIGUITA PALACIO
UNIVERSIDAD DE SAN BUENAVENTURA MEDELLÍN
FACULTAD DE INGENIERÍAS
INGENIERÍA ELECTRÓNICA
MEDELLÍN
2017
DISEÑO DE UN PROTOTIPO DE SISTEMA DE ODOMETRÍA VISUAL
CARLOS ANDRÉS GUERRA HERNÁNDEZ
LUIS SEBASTIÁN HIGUITA PALACIO
Monografía presentada para optar al título de Ingeniero Electrónico
Asesor: Camilo Andrés Flórez Velásquez, Ingeniero Eléctrico.
UNIVERSIDAD DE SAN BUENAVENTURA MEDELLÍN
FACULTAD DE INGENIERÍAS
INGENIERÍA ELECTRÓNICA
MEDELLÍN
2017
Dedicatoria
A Flor Dary Hernández y Jaime Guerra por la formación que me dieron, por ser la base
fundamental en mi vida, por el apoyo incondicional que me brindaron para poder cumplir
este logro.
Agradecimientos
Gracias Camilo Flórez por todo el apoyo brindado a lo largo de la carrera, por su tiempo y
amistad, por los conocimientos que nos transmitiste, orientaciones, forma de trabajar,
persistencia, paciencia y su motivación fueron fundamentales para el desarrollo de este
trabajo y de nosotros como profesionales, muy a tu manera te ganaste nuestra admiración y
nos sentimos en deuda contigo por tanto que recibimos durante la elaboración de este trabajo,
muchísimas gracias.
A Edison Guerra por la colaboración incondicional brindada durante toda la elaboración del
proyecto.
Agradecemos a nuestros compañeros de clase, amigos y familiares que de una u otra forma
ayudaron a que este proyecto se llevara a cabo.
Contenido
Resumen ............................................................................................................................... 10
Introducción .......................................................................................................................... 14
1. Objetivos ........................................................................................................................... 17
1.1. Objetivo General........................................................................................................ 17
1.2. Objetivos Específicos ................................................................................................ 17
2. Estimación de las coordenadas en dos (2) dimensiones de un agente robótico móvil
aplicando la metodología de matrices de cuerpo rígido ....................................................... 18
2.1 Definición de matriz de transformación de cuerpo rígido relativa ............................. 20
2.2. Captura y extracción de características en un par de imágenes estereoscópicas ....... 23
2.3. Emparejamiento de puntos característicos 2D presentes en las imágenes izquierda y
derecha .............................................................................................................................. 26
2.4 Correspondencia entre píxeles usando correlación y geometría epipolar .................. 29
2.5 Estimación de las coordenadas 3D de un punto característico ................................... 32
2.6 Emparejamiento de puntos característicos 3D existentes en las imágenes
estereoscópicas en los tiempos K y K-1 ........................................................................... 36
2.7 Estimación de la matriz de transformación de cuerpo rígido relativa ........................ 37
3. Metodología experimental ................................................................................................ 41
3.1 Implementación del sistema de visión artificial estereoscópica bicameral ................ 42
3.2 Experimento de calibración de las cámaras y obtención del modelo matemático para la
estimación de las coordenadas 3D .................................................................................... 45
3.3 Aplicación del algoritmo de Shi – Tomasi para la detección de esquinas presentes en
una imagen ........................................................................................................................ 47
3.4 Implementación de algoritmo de emparejamiento de puntos característicos 2D ....... 48
3.5 Implementación de algoritmo de emparejamiento de puntos característicos 3D de
imágenes en los instantes de tiempo K-1 y K ................................................................... 55
3.6 Estimación de las coordenadas 3D de los puntos característicos emparejados en las
imágenes izquierda y derecha ........................................................................................... 56
3.7 Implementación de algoritmo de Arun para la estimación de la matriz de
transformación de cuerpo rígido relativa .......................................................................... 57
3.8 Diseño e implementación de un algoritmo para la estimación de la matriz de pose de
las cámaras ........................................................................................................................ 59
3.9 Diseño de experimento para la estimación de la trayectoria del agente robótico móvil,
y de la medida del error cometido por la técnica de odometría ........................................ 60
4. Resultados y análisis ......................................................................................................... 62
4.1 Calibración de las cámaras y obtención del modelo matemático para la estimación de
las coordenadas 3D ........................................................................................................... 62
4.2 Aplicación del algoritmo de Shi – Tomasi para la detección de esquinas presentes en
una imagen ........................................................................................................................ 65
4.3 Algoritmo de emparejamiento de puntos característicos 2D ...................................... 66
4.4 algoritmo de emparejamiento de puntos característicos de imágenes en los instantes de
tiempo K-1 y K ................................................................................................................. 69
4.5 Implementación de algoritmo de ARUN para la estimación de la matriz de
transformación de cuerpo rígido relativa y matriz de pose. ............................................. 71
5. Conclusiones ..................................................................................................................... 75
Referencias .......................................................................................................................... 76
Lista de Tablas
Tabla 1. Datos de las esquinas elegidas para la calibración de las cámaras ......................... 62
Tabla 2. Matriz de calibración estimada para cámara izquierda .......................................... 63
Tabla 3. Matriz de calibración estimada para cámara derecha ............................................. 64
Tabla 4. Calculo del error relativo de las coordenadas 3D estimadas con el algoritmo de
matrices de calibración, con respecto a las coordenadas reales del sistema de balizas ........ 65
Tabla 5. Coordenadas de los puntos característicos presentes en las imágenes capturadas por
las cámaras izquierda y derecha emparejados. ..................................................................... 68
Tabla 6. Coordenadas de los puntos característicos presentes en las Imágenes capturadas por
la cámara izquierda en los tiempos K-1 y K. ........................................................................ 70
Tabla 7. Coordenadas de los puntos característicos emparejados en las imágenes izquierdas
capturadas en tiempos consecutivos en el experimento de movimiento recto de 30 cm...... 71
Tabla 8. Coordenadas de los puntos característicos emparejados en las imágenes izquierdas
capturadas en tiempos consecutivos en el experimento de movimiento curvo .................... 72
Tabla 9. Coordenadas X y Y, extraídas de la matriz de translación, estimadas por el algoritmo
de cálculo de la matriz de calibración de cuerpo rígido ....................................................... 73
Tabla 10. Coordenadas extraídas de la matriz de pose, estimada a partir del algoritmo de arun
.............................................................................................................................................. 73
Lista de Figuras
Figura 1. Pasos metodológicos sugeridos para la estimación de las coordenadas absolutas 3D
de un agente robótico móvil propuesta por (Scaramuzza & Fraundorfer, 2011). ................ 19
Figura 2. Un punto P de 3 dimensiones medido respecto a un sistema de referencia estático
〖<x〗_w,y_w,〖 z〗_w>, y respecto a un sistema de referencia móvil 〖<x〗_M,y_M,〖
z〗_M>. ................................................................................................................................ 20
Figura 3. Posibles ángulos de giro del sistema de referencia móvil respecto al estático.
(Zapata Herrera & Silva Narvaez, 2015). Reconstrucción de entornos 3D mediante un
sistema de visión artificial estereoscópico, para aplicaciones en navegación robótica). ..... 21
Figura 4. Geometría epipolar existente entre 2 cámaras. ..................................................... 30
Figura 5. Ejemplo de graficación de 5 rectas epipolares en la imagen derecha a partir de los
5 puntos seleccionados en la imagen izquierda (Barros Gallego & Agudelo, 2015). .......... 32
Figura 6. Diagrama de bloques donde se evidencian los pasos metodológicos para
implementar un sistema de visión artificial. ......................................................................... 42
Figura 7. Sistema estereoscópico propuesto. Nótese que el sistema de referencia cartesiano
de la cámara derecha esta trasladado respecto al sistema de referencia absoluto ubicado en la
cámara izquierda. Imagen elaborada por los autores. ........................................................... 43
Figura 8. Disposición del sistema de cámaras sobre el agente robótico móvil enseñando uno
de los elementos empleados de la plantilla de calibración de las cámaras. .......................... 45
Figura 9. Código de Matlab empleado para detectar y almacenar las esquinas ................... 48
Figura 10. Sistema de referencia de la cámara izquierda ilustrando 3 puntos colineales con el
centro de la lente, que es el centro del sistema de referencia. .............................................. 49
Figura 11. Metodología para estimar la correspondencia entre puntos característicos 2D
presentes en las imágenes derecha e izquierda ..................................................................... 54
Figura 12. Metodología propuesta por el equipo de trabajo para estimar la correspondencia
entre puntos característicos presentes en las imágenes capturadas en los tiempos K-1 y K. 56
Figura 13. Pasos metodológicos necesarios para la estimación de la matriz de rotación de
cuerpo rígido. ........................................................................................................................ 58
Figura 14. Sistema de referencia absoluto empleado durante las pruebas. En particular el
vehículo se movió en la trayectoria del eje Y_W. ................................................................ 60
Figura 15. . Toma de Experimento 2: Trayectoria curvilínea seleccionada para el móvil. .. 61
Figura 16. Matriz para la calibración de la cámara izquierda .............................................. 63
Figura 17. Matriz para la calibración de la cámara izquierda .............................................. 64
Figura 18. Aplicación de algoritmo de estimación de esquinas sobre una imagen .............. 66
Figura 19. Metodología propuesta para el cálculo de correlación entre sub – imágenes de la
cámara izquierda y derecha .................................................................................................. 67
Figura 20. Imágenes capturadas por la cámara derecha e izquierda respectivamente, en donde
se muestra el emparejamiento estimado por el algoritmo. ................................................... 68
Figura 21. Imágenes capturadas por la cámara izquierda en los tiempos K-1 y K, en donde se
muestra el emparejamiento estimado por el algoritmo ......................................................... 70
Figura 22. Grafica de la trayectoria estimada en el experimento 1 ...................................... 74
10
Resumen
La odometría visual ha sido uno de las técnicas más ampliamente utilizadas en asocio con
sensores cinemáticos (gps, IMUs, cámara cenital, encoders entre otros) para la estimación de
las coordenadas de la trayectoria de un agente robótico móvil, información de relevancia a la
hora de usar la metodología SLAM para la navegación en entornos estructurados o
semiestructurados en dos o tres dimensiones. En particular (Scaramuzza & Fraundorfer,
2011) propone la fundamentación matemática necesaria para la estimación de la trayectoria
de un móvil sobre un sistema de referencia absoluto. Los resultados obtenidos al combinarlos
con filtros estimadores de Kalman y el algoritmo de correspondencia entre puntos
característicos RANSAC, ilustran el gran potencial de la técnica para soportar la navegación
de agentes robóticos, pero la robustez de estos algoritmos provocan un gran consumo de
recursos computacionales que se traducen en largos tiempos de procesamiento de la
información visual, inhabilitando el mismo para ser empleado en sistemas de navegación de
tiempo real (Rodríguez-Aragón, 2015).
Por tal motivo el grupo de trabajo propone el diseño de un prototipo de sistema de odometría
visual estereoscópica para la estimación de trayectorias 2D, reemplazando el algoritmo
RANSAC por el uso de estimadores estadísticos más simples que sirvan para establecer
correspondencias entre puntos característicos de forma más eficiente que el mencionado
algoritmo. Para tal fin se implementará un sistema de visión artificial estereoscópica sobre
un agente robótico con tracción diferencial, que empleará las matrices de calibración de las
cámaras y los conceptos de la geometría epipolar para la estimación de las coordenadas 3D
de los puntos característicos de las imágenes muestreadas y capturadas en tiempos
consecutivos, k y k-1. Para establecer la correspondencia entre puntos característicos
existentes entre el par de imágenes estereoscópicas se utilizará la metodología propuesta por
(Kumar Singh, Shi-Tao Huang, & Blostein, 1987), en asocio con el uso de la técnica de
correlación entre dos imágenes. A partir de la información obtenida en los instantes de tiempo
k y k-1 se procederá a calcular la matriz de transformación de cuerpo rígido, que a su vez se
usará para determinar la matriz de pose del sistema de visión que corresponde a la pose del
agente robótico. Para poner a prueba el sistema elaborado, se procederá a mover el robot por
11
diversos tipos de trayectorias arbitrarias establecidas previamente, y se medirán las normas
euclidianas o de Frobenius existentes entre las coordenadas de las trayectorias estimadas y
las reales medidas respecto a un sistema de referencia absoluto.
Los resultados obtenidos (sistema de odometría visual) se emplearan para una próxima etapa
proyectual, en la cual se reconstruirá un entorno 2D a partir de la información visual
capturada por un par estereoscópico presente en un agente robótico.
Palabras clave: Odometría Visual, Geometría Epipolar, Matriz Fundamental, Matriz de
Transformación de cuerpo rígido, Matriz de Pose del Agente Robótico, Técnica de
Correspondencia 3D – 3D de Arun.
12
Abstract
Visual odometry has been one of the techniques most widely used in association with
kinematic sensors (gps, IMUs, zenith, encoders and others) for the estimation of the
coordinates of the trajectory of a mobile robotic agent, information of relevance to the time
to use the SLAM methodology for navigation in structured or semi-structured environments
in two or three dimensions. In particular, (Scaramuzza & Fraundorfer, 2011) proposes the
mathematical foundation necessary for the estimation of the trajectory of a mobile over an
absolute reference system. The results obtained when combined with Kalman estimator
filters and the RANSAC characteristic matching algorithm, illustrate the great potential of
the technique to support the navigation of robotic agents, but the robustness of these
algorithms causes a great consumption of computational resources that are Translate into
long processing times of visual information, disabling it for use in real-time navigation
systems (Rodríguez-Aragón, 2015).
For this reason the working group proposes the design of a prototype of a stereoscopic visual
odometry system for the estimation of 2D trajectories, replacing the RANSAC algorithm
with the use of simpler statistical estimators that serve to establish correspondences between
characteristic points more efficiently Than said algorithm. For this purpose a stereoscopic
artificial vision system will be implemented on a robotic agent with differential traction,
which will use the camera calibration matrices and the concepts of the epipolar geometry for
the estimation of the 3D coordinates of the characteristic points of the sampled images And
captured in consecutive times, k and k-1. In order to establish the correspondence between
characteristic points between the pair of stereoscopic images, the methodology proposed by
(Kumar Singh, Shi-Tao Huang, & Blostein, 1987), will be used in association with the use
of the correlation technique between two images. From the information obtained at time
instants k and k-1, the rigid body transformation matrix will be calculated, which in turn will
be used to determine the pose matrix of the vision system corresponding to the agent's pose
Robotic In order to test the elaborated system, the robot will be moved by various types of
previously established arbitrary trajectories, and the Euclidean or Frobenius norms existing
between the coordinates of the estimated trajectories and the actual measurements with
respect to a reference system absolute.
13
The results obtained (visual odometry system) will be used for a next design stage, in which
a 2D environment will be reconstructed from the visual information captured by a
stereoscopic pair present in a robotic agent.
Keywords: Visual Odometry, Epipolar Geometry, Fundamental Matrix, Rigid Body
Transformation Matrix, Robotic Agent Pose Matrix, 3D Arun - 3D Correspondence
Technique.
14
Introducción
La metodología SLAM (Simultaneus Localization and Mapping) sirve para obtener de forma
precisa las coordenadas de la ubicación de un agente robótico móvil respecto a un sistema de
referencia arbitrario absoluto en un instante de tiempo k, y con dicha información generar de
forma iterativa la trayectoria y el mapa del entorno donde navega (Durrant-Whyte & Bailey,
2012). Con el mapa reconstruido el robot puede seleccionar la ruta más óptima que minimice
alguna función de coste planteada por el diseñador (consumo de energía, seguimiento de la
trayectoria más corta entre dos puntos, la trayectoria planificada entre otras), (Paniagua,
2014). Algunas aplicaciones de la metodología SLAM son por ejemplo el vehículo autónomo
diseñado y fabricado por la empresa Google (Thrun, 2010), algunos satélites de exploración
de otros planetas (Nasa, 2014), los vehículos con función de parqueo automático diseñados
en la Universidad Politécnica de Valencia (Paniagua, 2014) y otras universidades, y otros
agentes robóticos diseñados por el Instituto Carnegie – Mellon (Kehagias, Djugash, & Singh,
2006).
Algunas técnicas empleadas para estimar las coordenadas de la ubicación de un agente
robótico móvil sobre un entorno 2D o 3D son las técnicas incrementales (Scaramuzza &
Fraundorfer, 2011) que hacen uso del modelo cinemático del móvil para calcular de forma
iterativa la posición del robot, entre las cuales se destacan la odometría usando encoders, la
odometría visual en asocio con filtros de Kalman extendidos o Uncested Kalman Filter; o
técnicas de posicionamiento global que determinan las coordenadas de la posición del móvil
sin necesidad de procesos iterativos fundamentados en algún modelo cinemático, usando por
ejemplo GPS, cámaras cenitales y radio transceptores (Paniagua, 2014), balizas ultrasónicas
(Gómez Gómez, 2015), entre otros.
Las técnicas de posicionamiento global por su naturaleza funcional no propagan la
incertidumbre de las medidas de los sensores en los cálculos iterativos de la posición global
(Paniagua, 2014), pero presentan una serie de dificultares técnicas que limitan su aplicación
en diversos tipos de entornos, por ejemplo la técnica de localización por GPS sólo es
aplicable si el radio presente en el agente puede establecer enlace con el satélite, lo que limita
su campo de aplicación a exteriores en asocio con las incertidumbres propias del sistema de
15
geo-posicionamiento (2.5 a 5 metros), y las técnicas de cámara cenital y balizas ultrasónicas
están restringidas a espacios donde sea factible instalar estos tipos de sensores (típicamente
en interiores o lugares donde no hayan ondas ultrasónicas generadas por vibraciones
mecánicas externas al sensor). La técnica de estimación incremental de las coordenadas de
la posición de un agente robótico móvil presenta como defecto, un incremento gradual de la
incertidumbre de dichas coordenadas a medida que el número de iteraciones crece en el
tiempo, siendo más notable este problema en robots que usan sensores propioceptivos tipo
encoders para ruedas por efecto de rodaduras y desniveles presentes en el terreno
(Scaramuzza & Fraundorfer, 2011). Una excelente opción para mitigar la incertidumbre
generada por los encoders para ruedas es el uso de sistemas de odometría visual, que son
insensibles a efectos de las rodaduras de las ruedas del agente y a los desniveles presentes en
el entorno (Agudelo España, Silva Lopez, & Gil Lopez, 2013), y en asocio con la información
del modelo cinemático del agente robótico y el uso de filtros de Kalman es posible disminuir
la incertidumbre en la estimación de la posición; además pueden ser empleados en conjunto
con otros sensores de posición para reducir los errores en la estimación de la posición
(Scaramuzza & Fraundorfer, 2011). La técnica propuesta por (Scaramuzza & Fraundorfer,
2011) usa los puntos característicos de 2 y/o de 3 dimensiones existentes entre 2 imágenes
capturadas en tiempos consecutivos, con la finalidad de estimar la matriz de transformación
de cuerpo rígido, y luego de forma iterativa calcular la matriz de pose del agente robótico
móvil. En particular el autor ilustra tres técnicas matemáticas para el cálculo de la matriz de
transformación: Las técnicas 3D – 3D, 2D – 2D y 3D – 2D, siendo la más liviana a nivel
algorítmico la técnica 3D – 3D, ya que las otras técnicas exigen una mayor cantidad de
cálculos matemáticos que incluyen resolución de problemas como la estimación de la matriz
esencial usando descomposición en valores singulares, generando una mayor carga
computacional que limita las posibles aplicaciones del sistema de odometría en entornos que
requieren mayor velocidad de reacción del computador, como la conducción por carretera
entre otras. (Rodríguez-Aragón, 2015) muestra que entre los algoritmos necesarios para la
estimación de la posición del móvil, es necesario emplear un algoritmo de correspondencia
para los puntos característicos existentes entre imágenes capturadas en tiempos consecutivos
para la estimación de la matriz de cuerpo rígido, como por ejemplo el algoritmo RANSAC
(Cruz, 2013); bastante efectivo para resolver el problema de correspondencia pero con un
16
alto costo computacional, que nuevamente repercute en altos tiempo de ejecución, que
limitan la aplicaciones de tiempo real del sistema de odometría visual.
Adicionalmente en la base de datos de patentes y registros industriales de Superintendencia
de Industria y Comercio de Colombia se hallaron muy pocas patentes y registros industriales
con las palabras claves “odometría”, “robótica”, “móvil”, “visión”, “visión artificial” o
“visión por computador”:
(Aperador Chaparro, Plaza Torres, & Cifuentes Bernal, 2014). Silla de ruedas eléctrica con
un dispositivo de imágenes térmicas para el control de desplazamiento en personas invidentes
y con limitaciones de movilidad.
Por último, El Gobierno de la República de Colombia sugiere a través de Colciencias en su
“Plan nacional de CTI para el desarrollo de los sectores de Electrónica – tecnologías de la
información y las comunicaciones” (Colciencias, 2013) y en su Plan de Desarrollo 2014 –
2018 (República de Colombia, 2014), abordar los temas de investigación más importantes
para apoyar el desarrollo tecnológico del país, entre los cuales se encuentran la robótica
industrial y sus aplicaciones en el sector de navegación reactiva o planificada en ambientes
industriales.
Se puede concluir de la información presentada de la problemática de la estimación de las
coordenadas de la trayectoria de un agente robótico móvil, puede ser resuelta de forma
exitosa empleando visión artificial, en particular usando la metodología 3D – 3D propuesta
por (Scaramuzza & Fraundorfer, 2011), pero suprimiendo el uso de la técnica de
correspondencia estadística de RANSAC para la estimación de la matriz de transformación
de cuerpo rígido, y reemplazándola por técnicas estadísticas más livianas como la correlación
2D o mínimos cuadrados, con la finalidad de construir un prototipo de un sistema de
odometría visual con bajos tiempos de operación computacional útil para ser empleando en
aplicaciones de navegación en tiempo real en entornos interiores o exteriores.
17
1. Objetivos
1.1. Objetivo General
Medir el error en la estimación de las coordenadas de una trayectoria de 2 dimensiones
seguida por un agente robótico móvil que emplea un sistema de odometría visual
estereoscópico, contra las coordenadas reales de la trayectoria medidas con un sistema de
referencia cartesiano absoluto, usando la metodología de estimación de matriz de pose con
puntos 3D y el uso de normas vectoriales como la Euclidiana o de Frobenius para la medida
del error, con la finalidad de evaluar el desempeño del sistema de odometría diseñado.
1.2. Objetivos Específicos
Diseñar e implementar un algoritmo de estimación de coordenadas de una trayectoria de 2
dimensiones seguida por un agente robótico móvil, empleando visión artificial
estereoscópica (Barros Gallego & Agudelo, 2015) y la estimación de correspondencias entre
puntos 3D de dos imágenes estereoscópicas calculadas en tiempos K y K-1 usando la
correlación 2D entre imágenes, con la finalidad de obtener la matriz de pose que contiene la
información de traslación y rotación del agente robótico respecto al sistema de referencia
cartesiano absoluto en el instante de tiempo K.
Medir los errores de estimación de las coordenadas de las trayectorias con la técnica de
odometría visual propuesta, para caminos en 2 dimensiones con formas de línea recta y
caminos de 2 dimensiones cerrados con varios radios de curvatura, con la finalidad de evaluar
el comportamiento del sistema diseñado ante las condiciones cinemáticas impuestas por las
trayectorias bajo estudio.
Diseñar e implementar una interface gráfica en Matlab, para soportar la técnica de odometría
visual estudiada.
18
2. Estimación de las coordenadas en dos (2) dimensiones de un agente robótico móvil
aplicando la metodología de matrices de cuerpo rígido
En el presente capítulo se ilustrarán los elementos más relevantes del marco teórico de la
técnica de odometría visual orientada a la estimación de las coordenadas 2D de un agente
robótico móvil, medidas en un sistema de referencia absoluto inmóvil puesto sobre una
superficie de navegación plana, fundamentada en la estimación iterativa de la matriz de
transformación de cuerpo rígido (matriz que contiene la información de los ángulos de
rotación y del vector de traslación del móvil) que se calcula utilizando la metodología 3D -
3D propuesta por Scaramuzza (Scaramuzza, 2012); esta técnica emplea las coordenadas 3D
de puntos característicos pertenecientes a objetos inmóviles en la escena, como por ejemplo
esquinas o vértices (puntos característicos), medidas respecto a un sistema cartesiano de
referencia anclado a un sistema de visión artificial estereoscópico bicameral ubicado sobre
el robot.
(Moravec, 1980) Estableció las bases conceptuales de la odometría visual o estimación de
las coordenadas 2D o 3D de ubicación de un móvil respecto a un sistema de referencia
cartesiano, que alcanzó su máximo apogeo con los aportes de (Nister, Naroditsky, & Bergen,
2004). Un sistema de odometría visual está compuesto básicamente por un sistema de visión
artificial de una (1) o más cámaras digitales conectadas a un sistema computacional, que a
partir de la correspondencia existente entre los puntos característicos 3D presentes en 2
imágenes estereoscópicas capturadas en los instantes de tiempo K-1 y K, es capaz de estimar
la matriz de transformación de cuerpo rígido del robot en el instante de tiempo k, 𝑇𝑘,𝑘−1, que
contiene los valores de los cambios del vector de traslación y de la matriz de rotación, útiles
para estimar las coordenadas absolutas de la trayectoria a través de la estimación de la matriz
de pose de las cámaras. (Agudelo España, Silva Lopez, & Gil Lopez, 2013, Scaramuzza &
Fraundorfer, 2011).
En la figura 1 se presenta los pasos sugeridos para la estimación de las coordenadas absolutas
3D de un agente robótico según Scaramuzza (Scaramuzza & Fraundorfer, 2011).
19
Figura 1. Pasos metodológicos sugeridos para la estimación de las coordenadas absolutas 3D de un
agente robótico móvil propuesta por (Scaramuzza & Fraundorfer, 2011).
En las siguientes secciones del capítulo se procederá a ampliar cada uno de los pasos de la
metodología empleada para la estimación de la trayectoria del móvil, a partir de la
información visual capturada por el sistema de visión artificial.
20
2.1 Definición de matriz de transformación de cuerpo rígido relativa
En la figura 2 se presenta un caso típico de estudio de estimación de coordenadas 3D de un
móvil: Se posee un sistema de referencia absoluto y estático < 𝑥𝑤, 𝑦𝑤, 𝑧𝑤 > que sirve para
medir las coordenadas de un punto inmóvil 𝑃(𝑥𝑤, 𝑦𝑤, 𝑧𝑤) de la escena, y un sistema de
referencia < 𝑥𝑀 , 𝑦𝑀, 𝑧𝑀 > ubicado sobre un móvil:
Figura 2. Un punto P de 3 dimensiones medido respecto a un sistema de referencia estático 〖<x〗
_w,y_w,〖 z〗_w>, y respecto a un sistema de referencia móvil 〖<x〗_M,y_M,〖 z〗_M>.
Las coordenadas del punto P medidas respecto al sistema de referencia cartesiano ubicado en
el agente móvil, se puede relacionar con las coordenadas del mismo punto medido respecto
al sistema de referencia estático usando la expresión (1):
1
10
1
31
1333
W
W
W
M
M
M
z
y
x
TR
z
y
x
(1)
O
1431
1333
14 10
WM X
TRX (1)
21
Donde
XX
XX
YY
YY
ZZ
ZZ
ZYX
sen
sen
sen
sen
sen
sen
R
cos0
cos0
001
cos0
010
0cos
100
0cos
0cos
),,(33
(2)
es la matriz de rotación en función de los ángulos de giro de los ejes del sistema de referencia
móvil respecto al fijo, tal como se ilustra en la figura 3:
Figura 3. Posibles ángulos de giro del sistema de referencia móvil respecto al estático. (Zapata Herrera
& Silva Narvaez, 2015). Reconstrucción de entornos 3D mediante un sistema de visión artificial
estereoscópico, para aplicaciones en navegación robótica).
c
b
a
T 13 es el vector que contiene las coordenadas del origen del sistema de referencia
móvil o’ medido respecto al sistema de referencia estático multiplicadas por -1.
22
Adicionalmente La matriz
10 31
1333 TR recibe el nombre de Matriz de Transformación de
Cuerpo Rígido.
De (1) se puede verificar que:
1313
3313
TXRX WM (Kumar Singh, Shi-Tao Huang, & Blostein, 1987) (3)
Si el móvil cambió de posición respecto al sistema de referencia absoluto en el intervalo de
tiempo discreto SS KTTK ,)1( , donde ST es el periodo de muestreo y K es el número de
muestra, entonces es posible estimar la matriz de transformación de cuerpo rígido a partir de
los valores de las coordenadas 3D o 2D medidas en los instantes 1K y K de por los menos
3 puntos fijos de la escena (Kumar Singh, Shi-Tao Huang, & Blostein, 1987), 1987). Es claro
que si el móvil está cambiando de posición en el tiempo, entonces la matriz de transformación
de cuerpo rígido también lo hará, y por tal motivo dicha matriz tiene el apelativo de “relativa”
y se denota así:
10 31
1,1,
)1,(
KKKK
KK
TRT (4)
Donde los subíndices 1K y K representan los instantes de tiempo empleados para su
estimación.
(Scaramuzza & Fraundorfer, 2011), definen varios métodos para la estimación de la matriz,
𝑇𝑘,𝑘−1,: El Método 3D - 3D, método 2D – 2D y método 3D – 2D.
La metodología 3D – 3D es aplicable en robots que poseen un sistema de visión artificial
estereoscópico y requiere de los siguientes pasos:
Capturar 2 pares de imágenes (una imagen de la cámara izquierda y otra
imagen de la cámara derecha), en los instantes de tiempo K-1 y K.
Extraer puntos característicos de cada par de imágenes, empleando por
ejemplo algoritmos de detección de esquinas como por ejemplo el algoritmo
de (Harris & Sthepens, 1988), o de (Moravec, 1980), entre otros, y establecer
la correspondencia entre dichos puntos, usando geometría epipolar y/o
23
correlación entre píxel - vecindad de ambas imágenes. Esta última
metodología es ampliamente utilizada en equipos de odometría visual, pero
algunos autores como (Rodríguez-Aragón, 2015), comentan que esta técnica
consume mucho tiempo de ejecución en el computador y no es aconsejable
para aplicaciones en tiempo real. Por tal motivo es importante implementar
algoritmos más eficientes bajo el aspecto de tiempo de ejecución.
A partir de los puntos correspondientes 3D se procede a estimar la matriz de
transformación de cuerpo rígido en el instante de tiempo k (𝑇(𝑘,𝑘−1) ), usando
el método de mínimos cuadrados para relacionar 2 conjuntos de puntos
característicos especificados en 3D propuesto por (Kumar Singh, Shi-Tao
Huang, & Blostein, 1987). La matriz (𝑇(𝑘,𝑘−1) ) sirve para calcular las matrices
de traslación y rotación del robot y por ende la matriz de pose del mismo, la
cual establece la posición actual del agente móvil.
La técnica más liviana a nivel computacional es la técnica 3D – 3D (Agudelo España, Silva
Lopez, & Gil Lopez, 2013), que calcula las matrices de rotación y traslación usando
correspondencia entre píxeles característicos de dos imágenes estereoscópicas capturadas en
los instantes de tiempo k-1 y k, aplicando la metodología matemática propuesta por (Kumar
Singh, Shi-Tao Huang, & Blostein, 1987). La correspondencia entre píxeles característicos
de las imágenes estereoscópicas capturadas, se determina usando geometría epipolar y
correlación entre sub-imágenes (Gómez Gómez, 2015) y (Barros Gallego & Agudelo, 2015).
2.2. Captura y extracción de características en un par de imágenes estereoscópicas
La metodología para la estimación de las coordenadas 3D de puntos correspondientes de 2
imágenes provenientes de un par estereoscópico, que está fundamentada en las propiedades
de las matrices de calibración de las cámaras empleadas y de la geometría epipolar
involucrada en el arreglo de las cámaras y las imágenes capturadas de los objetos son
ilustradas por (Zapata Herrera & Silva Narvaez, 2015 y Barros Gallego & Agudelo, 2015).
En particular los puntos seleccionados en las imágenes capturadas para la estimación de sus
24
coordenadas 3D suelen ser puntos característicos: puntos de 2 dimensiones de las imágenes
que poseen propiedades geométricas únicas como por ejemplo, conformar la esquina de
polígonos abiertos o cerrados, o puntos cuyos píxel – vecindades poseen texturas que le
diferencian de otras partes de la escena capturada.
Típicamente los puntos característicos elegidos de una escena estereoscópica son aquellos
que forman los vértices de las figuras (esquinas o “corners”), y se pueden detectar de forma
automática usando algoritmos de visión artificial de detección de esquinas, como los
algoritmos de (Harris & Sthepens, 1988), o la reforma del algoritmo de Harris, el algoritmo
de (Kumar Singh, Shi-Tao Huang, & Blostein, 1987)), el algoritmo de (Moravec, 1980) y
otros.
El algoritmo de detección de esquinas de Harris se fundamenta en el cálculo de la función S
llamada “Suma ponderada de las diferencias al cuadrado de los cambios de intensidad de la
imagen”, que estima el valor del cambio de las intensidades de una vecindad alrededor de
cada punto ),( yx de la imagen:
2
),(),(),( W
iiii yyxxIyxIyxS (5)
Donde ),( yxI es la función que rige la intensidad de la imagen y se evalúa justo en los puntos
),( ii yx definidos por la función de ventana móvil W centrada alrededor del punto ),( yx . La
función de ventana típicamente empleada en el algoritmo de Harris es la “Ventana
Exponencial” cuya matriz es de la forma:
001.002.001.00
01.006.01.006.001.0
02.01.016.01.002.0
01.006.01.006.001.0
001.002.001.00
96.0
1W (6)
x y y son desplazamientos que sirven para evaluar el cambio de la intensidad en todas las
direcciones.
La expresión (4) se puede aproximar por una serie de Taylor:
25
12
2221,),(
y
xMyxyxS (7)
Donde M es una matriz cuadrada de 2×2 que se calcula así:
W
iiY
W W
iiYiiX
W
iiYiiX
W
iiX
yxIyxIyxI
yxIyxIyxI
M),(),(),(
),(),(),(
2
2
(8)
),( iiX yxI es la derivada en la dirección x (horizontal) del punto ),( ii yx que pertenece a la
ventana W centrada alrededor del punto ),( yx .
),( iiY yxI es la derivada en la dirección y (vertical) del punto ),( ii yx que pertenece a la
ventana W centrada alrededor del punto ),( yx .
Los valores propios reales de la matriz M sirven como indicadores para detectar los puntos
),( yx que son esquinas en las imágenes:
- Si 1 y
2 son muy pequeños respecto a un umbral seleccionado por el usuario,
entonces el punto ),( yx no es una esquina ni un borde.
- Si 1 es muy grande y
2 es muy pequeño, entonces el punto ),( yx pertenece a un
borde.
- Si 1 y
2 son más grandes que un umbral seleccionado por el usuario, entonces el
punto ),( yx es una esquina.
Harris inventó una función para cualificar el tipo de punto ),( yx a partir de los valores
propios de M:
2
2121 kR (9)
Donde k se selecciona experimentalmente entre 0.04 y 0.06 (Collins, s.f.)
- Si R es muy grande respecto a un umbral prefijado por el usuario, entonces el punto
),( yx es una esquina.
26
- Si R es negativa y tiene una magnitud superior a un umbral prefijado por el usuario,
entonces el punto ),( yx pertenece a un borde.
- Si la magnitud de R es muy pequeña, entonces el punto ),( yx no es una esquina ni
un borde.
La técnica de Shi – Tomas, es una mejora sobre el algoritmo planteado por (Harris &
Sthepens, 1988), donde se establece una función R más simple y eficiente para detectar
esquinas:
),min( 21 R (10) (Kumar Singh, Shi-Tao Huang, & Blostein, 1987)
.A la fecha la técnica de Shi – Tomas ha mostrado ser experimentalmente más efectiva que
la técnica de Harris a la hora de detectar esquinas presentes en objetos presentes en imágenes
2D, y es la técnica elegida en el presente trabajo para la detección de puntos característicos.
Después de determinar las coordenadas en píxeles de los puntos característicos de las
imágenes capturadas por el sistema de visión estereoscópica, es necesario aplicar un
algoritmo emparejamiento de características entre ambas imágenes (izquierda y derecha),
con la finalidad de emplear estos puntos correspondientes en la estimación de las coordenadas
3D de las esquinas del objeto filmado por el sistema de visión estereoscópica.
2.3. Emparejamiento de puntos característicos 2D presentes en las imágenes izquierda
y derecha
El emparejamiento de características es una operación de la visión artificial que sirve para
discernir cuales puntos característicos 2D identificados en la imagen izquierda, corresponden
a esos mismos puntos sobre la imagen derecha (Barros Gallego & Agudelo, 2015).
La operación de emparejamiento de puntos característicos se fundamenta en el empleo de
propiedades geométricas subyacentes entre las cámaras y la escena (geometría epipolar) y la
información de color o intensidad o textura de las vecindades asociadas a los puntos
característicos de las imágenes izquierda y derecha (Correlación estadística).
Una técnica ampliamente usada para el emparejamiento de puntos característicos, es la
Correlación por área (Barros Gallego & Agudelo, 2015).
27
La correlación entre una sub - imagen 𝑔𝑟(𝑢𝑅𝑖, 𝑣𝑅𝑖) y una sub-imagen patrón 𝑔𝑙(𝑢𝐿𝑖, 𝑣𝐿𝑖), es
una cantidad que indica que tan similar es la sub - imagen 𝑔𝑅 a una sub - imagen patrón 𝑔𝐿,
ambas en escalas de grises y en representación numérica real.
La correlación entre 2 sub - imágenes 𝑔𝑅 y 𝑔𝐿 se define así:
𝜌 =𝜑𝐿𝑅
𝜑𝐿𝜑𝑅 (11)
Dónde:
𝜑𝐿𝑅: Es la covarianza cruzada entre las dos sub - imágenes.
𝜑𝐿 y 𝜑𝑅: Son las desviaciones estándar de cada sub - imagen.
La correlación entre dos sub - imágenes se emplea para determinar cuál punto característico
de 2 dimensiones de la imagen derecha, corresponde fielmente a un punto característico
patrón presente en la imagen izquierda.
Para tal fin se crea una vecindad o sub – imagen cuadrada de N píxeles alrededor de un punto
característico 2D de la imagen izquierda 𝑃(𝑢𝐿𝑖, 𝑣𝐿𝑖), y se calcula la correlación de esta sub -
imagen con cada posible sub - imagen de N píxeles formada alrededor de cada punto 2D de
la imagen derecha.
La pareja de puntos centrales (𝑢𝐿𝑖, 𝑣𝐿𝑖) y (𝑢𝑅𝑖, 𝑣𝑅𝑖) que provoquen que la correlación 𝜌 sea
un máximo global, se seleccionaran como los puntos 2D correspondientes de las imágenes
izquierda y derecha.
Para el cálculo de la correlación se deben seguir los siguientes pasos:
Se calcula el valor promedio de la intensidad de la sub-imagen derecha y la subimagen
izquierda, 𝑔𝑅̅̅̅̅ y 𝑔𝐿̅̅ ̅ respectivamente.
𝑔𝑅̅̅̅̅ = ∑∑𝑔𝑅(𝑢𝑅𝑖, 𝑣𝑅𝑗)
𝑚
𝑗=1
𝑛
𝑖=1
(12)
𝑔𝐿̅̅ ̅ = ∑∑𝑔𝐿(𝑢𝐿𝑖, 𝑣𝐿𝑗)
𝑚
𝑗=1
𝑛
𝑖=1
(13)
28
Dónde:
m: Número de filas de la sub - imagen.
n: Número de columnas de la sub - imagen.
𝑔𝑅(𝑢𝑅𝑖, 𝑣𝑅𝑗) y 𝑔𝐿(𝑢𝐿𝑖, 𝑣𝐿𝑗): son los valores de las intensidades en escala de grises de los
puntos 𝑃𝑅𝑖𝑗(𝑢𝑅𝑖, 𝑣𝑅𝑗) y 𝑃𝐿𝑖𝑗(𝑢𝐿𝑖, 𝑣𝐿𝑗) de las sub - imágenes derecha e izquierda.
Las desviaciones estándar de las sub - imágenes izquierda y derecha, 𝜑𝐿 y 𝜑𝑅 se calculan
así:
𝜑𝐿 = √∑ ∑ (𝑔𝐿(𝑢𝐿𝑖, 𝑣𝐿𝑗) − 𝑔𝐿̅̅ ̅)
2𝑚𝑗=1
𝑛𝑖=1
𝑛𝑚 − 1 (14)
𝜑𝑅 = √∑ ∑ (𝑔𝑅(𝑢𝑅𝑖, 𝑣𝑅𝑗) − 𝑔𝑅̅̅̅̅ )
2𝑚𝑗=1
𝑛𝑖=1
𝑛𝑚 − 1 (15)
Y la covarianza cruzada de las intensidades de las 2 sub – imágenes se calcula con la
expresión (16):
𝜑𝐿𝑅 = √∑ ∑ [(𝑔𝐿(𝑢𝐿𝑖, 𝑣𝐿𝑗) − 𝑔𝐿̅̅ ̅)(𝑔𝑅(𝑢𝑅𝑖, 𝑣𝑅𝑗) − 𝑔𝑅̅̅̅̅ )]𝑚
𝑗=1𝑛𝑖=1
𝑛 × 𝑚 − 1 (16)
Por último, se evalúa la correlación 𝜌 sobre las sub – imágenes formadas alrededor de cada
punto característico de la imagen izquierda de coordenadas (𝑢𝐿𝑖 , 𝑣𝐿𝑖) y de cada punto de la
imagen derecha de coordenadas (𝑢𝑅𝑖, 𝑣𝑅𝑖), y el par de puntos 2D 𝑃𝐿𝑖(𝑢𝐿𝑖, 𝑣𝐿𝑖) y 𝑃𝑅𝑖(𝑢𝑅𝑖, 𝑣𝑅𝑖)
que posean la máxima correlación, son los puntos característicos correspondientes detectados
entre la imagen derecha y la imagen izquierda.
29
2.4 Correspondencia entre píxeles usando correlación y geometría epipolar
Para hallar o estimar las coordenadas (𝑋𝑤, 𝑌𝑤, 𝑍𝑤) de un punto 𝑃 de 3 dimensiones
empleando un sistema de visión artificial estereoscópico, es necesario disponer de los valores
de las coordenadas 2D de dicho punto registrado por ambas cámaras, (𝑈𝐿 , 𝑉𝐿) y (𝑈𝑅 , 𝑉𝑅),
lo cual implica que es necesario implementar un algoritmo de correspondencia de píxeles
fundamentado por ejemplo en el cálculo de correlación entre sub – imágenes, explicado en
la sección anterior.
La técnica de correspondencia o emparejamiento entre puntos 2D característicos presentes
en 2 imágenes, usualmente presenta fallas (detección de falsos puntos correspondientes)
provocadas por la existencia de sub – imágenes en la imagen derecha que presentan un gran
parecido con la sub – imagen patrón de la imagen izquierda (Barros Gallego & Agudelo,
2015).
Para incrementar el grado de éxito del algoritmo de correspondencia de puntos característicos
2D usando correlación, se sugiere crear las sub – imágenes de la imagen derecha alrededor
de cada uno de los puntos que pertenecen a las rectas epipolares asociadas a su vez a cada
punto característico 𝑃(𝑢𝐿𝑖, 𝑣𝐿𝑖).
Un concepto importante de geometría epipolar es el siguiente: “Todo punto de 2 dimensiones
𝑃𝐿𝑖(𝑈𝐿 , 𝑉𝐿) presente en la imagen izquierda, equivale a una línea recta ubicada sobre la
imagen capturada por la cámara derecha”. En la figura 4 se ilustra este concepto.
30
Figura 4. Geometría epipolar existente entre 2 cámaras.
La ecuación de la línea recta epipolar asociada al punto 𝑃𝐿𝑖(𝑈𝐿 , 𝑉𝐿) se puede calcular a partir
de la matriz fundamental F, que se calcula con los puntos correspondientes existentes en las
imágenes izquierda y derecha (Zisserman, 2010).
Matriz fundamental F
La matriz fundamental F, es una matriz cuadrada de orden 3×3, que representa
algebraicamente la geometría epipolar presente entre los puntos característicos
correspondientes entre 2 imágenes provenientes de un par estereoscópico (Zisserman, 2010),
(Barros Gallego & Agudelo, 2015). La matriz fundamental F sirve para calcular los
coeficientes que rigen a la ecuación de la recta epipolar 𝑙 formada en la cámara derecha,
asociada al punto característico de 2 dimensiones existente en la imagen izquierda,
𝑃𝐿𝑖(𝑈𝐿𝑖, 𝑉𝐿𝑖):
𝐹3×3 𝑈 𝐿𝑖3×1
= 𝐹 [𝑈𝐿𝑖
𝑉𝐿𝑖
1] = [
𝐴𝐵𝐶] (17)
31
Dónde
𝐴𝑈𝑅 + 𝐵𝑉𝑅 + 𝐶 = 0 (18)
Es la ecuación de la recta epipolar, donde 𝑃𝑅(𝑈𝑅 , 𝑉𝑅) representa los puntos de dos
dimensiones que hacen parte del lugar geométrico mencionado.
La ecuación (18) se puede reescribir en notación vectorial así:
[𝑈𝑅
𝑉𝑅
1]
𝑇
. [𝐴𝐵𝐶] = 0 (19)
Reemplazando la expresión (17) en la expresión (19) se llega al siguiente resultado:
[𝑈𝑅
𝑉𝑅
1]
𝑇
𝐹 [𝑢𝐿𝑖
𝑣𝐿𝑖
1] = 0 (20)
O de forma equivalente:
𝑈 𝑅3×1
𝑇 𝐹 𝑈𝐿𝑖 = 0 (21)
La expresión (21) recibe el nombre de restricción epipolar y es clave para establecer el
método para la estimación de la matriz fundamental F usando la técnica de los 8 puntos con
y sin normalizar (Zisserman, 2010).
En la figura 5 se ilustra el concepto de transformación de un punto 𝑃𝐿𝑖(𝑈𝐿 , 𝑉𝐿) de la imagen
izquierda en una línea recta epipolar en la imagen capturada por la cámara derecha.
32
Figura 5. Ejemplo de graficación de 5 rectas epipolares en la imagen derecha a partir de los 5 puntos
seleccionados en la imagen izquierda (Barros Gallego & Agudelo, 2015).
Una propiedad interesante de una recta epipolar es que esta contiene al punto 𝑃𝐿𝑖(𝑈𝐿 , 𝑉𝐿)
visto desde la perspectiva de la cámara derecha, 𝑃𝑅𝑖(𝑈𝑅𝑖, 𝑉𝑅𝑖) , y es imposible que dicho
punto correspondiente esté por fuera de este lugar geométrico (Zisserman, 2010).
Ahora bien, limitando la región de ejecución del algoritmo de correlación alrededor de las
sub – imágenes construidas alrededor de los puntos pertenecientes a las rectas epipolares
asociadas a los puntos característicos de la imagen izquierda, aumentamos en gran medida la
velocidad de ejecución del mismo, debido a la reducción de las regiones de búsqueda de los
puntos característicos de la imagen derecha.
2.5 Estimación de las coordenadas 3D de un punto característico
(Zapata Herrera & Silva Narvaez, 2015), ilustran la expresión que rige las coordenadas del
punto de 2 dimensiones P(𝑈 , 𝑉 ), proveniente de un punto de 3 dimensiones
),,( WWW ZYXP :
[𝑈 𝑡𝐻𝑉 𝑡𝐻𝑡𝐻
] = 𝐶 [
𝑋𝑤
𝑌𝑤
𝑍𝑤
1
] (22)
Dónde:
U : Coordenadas en píxeles en la dirección horizontal.
V : Coordenadas en píxeles en la dirección vertical.
33
tH: Coordenada homogénea [por unidad].
C: Matriz de calibración de la cámara.
En particular las coordenadas 2D que se forman sobre la imagen izquierda y derecha
producido por un punto de la escena ),,( WWW ZYXP están dadas por las expresiones (23) y
(24):
[𝑈𝐿𝑡𝐻𝐿
𝑉𝐿𝑡𝐻𝐿
𝑡𝐻𝐿
] = 𝐶𝐿 [
𝑋𝑤
𝑌𝑤
𝑍𝑤
1
] (23)
&
[𝑈𝑅𝑡𝐻𝑅
𝑉𝑅𝑡𝐻𝑅
𝑡𝐻𝑅
] = 𝐶𝑅 [
𝑋𝑤
𝑌𝑤
𝑍𝑤
1
] (24)
Donde:
𝐶 𝐿3×4
y 𝐶 𝑅3×4
son las matrices de calibración de las cámaras izquierda y derecha,
respectivamente, 𝑡𝐻𝐿 y 𝑡𝐻𝑅 son los factores de escala asociados a los puntos formados en los
CCD’s de dichas cámaras.
Las matrices 𝐶𝐿 y 𝐶𝑅 se pueden estimar usando diversas técnicas de calibración como las
propuestas por (Hall), (Zhang, 2000) entre otras. (Zapata Herrera & Silva Narvaez, 2015),
definen claramente las metodologías para estimar estas matrices, y sugieren a partir de
pruebas experimentales el uso de la técnica de calibración homogénea, ya que las matrices
de calibración estimadas con dicha técnica producen los errores más pequeños en la
estimación de las coordenadas de puntos 3D.
A partir de los valores estimados de las matrices 𝐶𝐿 y 𝐶𝑅, es posible estimar las coordenadas
de un punto 𝑃 de 3 dimensiones:
34
Las coordenadas en 2 dimensiones de un punto ),,( WWW ZYXP visto por la cámara
izquierda, (𝑈𝐿 , 𝑉𝐿), y por la cámara derecha, (𝑈𝑅 , 𝑉𝑅), están dadas por las expresiones (25):
[𝑈𝐿𝑡𝐻𝐿
𝑉𝐿𝑡𝐻𝐿
𝑡𝐻𝐿
] = [
𝑐11𝐿 𝑐12
𝐿 𝑐13𝐿 𝑐14
𝐿
𝑐21𝐿 𝑐22
𝐿 𝑐23𝐿 𝑐24
𝐿
𝑐31𝐿 𝑐32
𝐿 𝑐33𝐿 𝑐34
𝐿
] [
𝑋𝑤
𝑌𝑤
𝑍𝑤
1
] (25)
&
[𝑈𝑅𝑡𝐻𝑅
𝑉𝑅𝑡𝐻𝑅
𝑡𝐻𝑅
] = [
𝑐11𝑅 𝑐12
𝑅 𝑐13𝑅 𝑐14
𝑅
𝑐21𝑅 𝑐22
𝑅 𝑐23𝑅 𝑐24
𝑅
𝑐31𝑅 𝑐32
𝑅 𝑐33𝑅 𝑐34
𝑅
] [
𝑋𝑤
𝑌𝑤
𝑍𝑤
1
] (25)
De las expresiones (25) se puede verificar que:
𝑡𝑃𝐻 = 𝑐31𝐿 𝑋𝑤 + 𝑐32
𝐿 𝑌𝑤 + 𝑐33𝐿 𝑍𝑤 + 𝑐34
𝐿 26
Además:
𝑈𝐿𝑡𝐻𝐿 = 𝑐11𝐿 𝑋𝑤 + 𝑐12
𝐿 𝑌𝑤 + 𝑐13𝐿 𝑍𝑤 + 𝑐14
𝐿 (27)
𝑉𝐿𝑡𝐻𝐿 = 𝑐21𝐿 𝑋𝑤 + 𝑐22
𝐿 𝑌𝑤 + 𝑐23𝐿 𝑍𝑤 + 𝑐24
𝐿 (28)
Al reemplazar la expresión (26) en las expresiones (27) y (28) se llega al siguiente resultado:
(𝑈𝐿𝑐31𝐿 − 𝑐11
𝐿 )𝑋𝑤 + (𝑈𝐿𝑐32𝐿 − 𝑐12
𝐿 )𝑌𝑤 + (𝑈𝐿𝑐33𝐿 − 𝑐13
𝐿 )𝑍𝑤 = 𝑐14𝐿 − 𝑢𝑝𝑙𝑐34
𝐿 (29)
&
(𝑉𝐿𝑐31𝐿 − 𝑐21
𝐿 )𝑋𝑤 + (𝑉𝐿𝑐32𝐿 − 𝑐22
𝐿 )𝑌𝑤 + (𝑉𝐿𝑐33𝐿 − 𝑐23
𝐿 )𝑍𝑤 = 𝑐24𝐿 − 𝑉𝐿𝑐34
𝐿 (30)
Repitiendo el procedimiento anterior para la matriz de calibración de la cámara derecha, se
obtienen las siguientes expresiones:
(𝑈𝑅𝑐31𝑅 − 𝑐11
𝑅 )𝑋𝑤 + (𝑈𝑅𝑐32𝑅 − 𝑐12
𝑅 )𝑌𝑤 + (𝑈𝑅𝑐33𝑅 − 𝑐13
𝑅 )𝑍𝑤 = 𝑐14𝑅 − 𝑈𝑅𝑐34
𝑅 (31)
&
35
(𝑉𝑅𝑐31𝑅 − 𝑐21
𝑅 )𝑋𝑤 + (𝑉𝑅𝑐32𝑅 − 𝑐22
𝑅 )𝑌𝑤 + (𝑉𝑅𝑐33𝑅 − 𝑐23
𝑅 )𝑍𝑤 = 𝑐24𝑅 − 𝑉𝑅𝑐34
𝑅 (32)
Las ecuaciones {29, 30, 31,32} se pueden representar matricialmente, así:
[ 𝑈𝐿𝑐31
𝐿 − 𝑐11𝐿 𝑈𝐿𝑐32
𝐿 − 𝑐12𝐿 𝑈𝐿𝑐33
𝐿 − 𝑐13𝐿
𝑉𝐿𝑐31𝐿 − 𝑐21
𝐿 𝑉𝐿𝑐32𝐿 − 𝑐22
𝐿 𝑉𝐿𝑐33𝐿 − 𝑐23
𝐿
𝑈𝑅𝑐31𝑅 − 𝑐11
𝑅 𝑈𝑅𝑐32𝑅 − 𝑐12
𝑅 𝑈𝑅𝑐33𝑅 − 𝑐13
𝑅
𝑉𝑅𝑐31𝑅 − 𝑐21
𝑅 𝑉𝑅𝑐32𝑟 − 𝑐22
𝑟 𝑉𝑅𝑐33𝑅 − 𝑐23
𝑅 ]
[ 𝑥𝑤
𝑦𝑤
𝑧𝑤]
=
[ 𝑐14
𝐿 − 𝑈𝐿𝑐34𝐿
𝑐24𝐿 − 𝑉𝐿𝑐34
𝐿
𝑐14𝑅 − 𝑈𝑅𝑐34
𝑅
𝑐24𝑅 − 𝑉𝑅𝑐34
𝑅 ]
(33)
O de forma equivalente:
𝐴4×3𝑋𝑤3×1= 𝑏4×1 (34)
Dónde:
𝐴 =
[ 𝑈𝐿𝑐31
𝐿 − 𝑐11𝐿 𝑈𝐿𝑐32
𝐿 − 𝑐12𝐿 𝑈𝐿𝑐33
𝐿 − 𝑐13𝐿
𝑉𝐿𝑐31𝐿 − 𝑐21
𝐿 𝑉𝐿𝑐32𝐿 − 𝑐22
𝐿 𝑉𝐿𝑐33𝐿 − 𝑐23
𝐿
𝑈𝑅𝑐31𝑅 − 𝑐11
𝑅 𝑈𝑅𝑐32𝑅 − 𝑐12
𝑅 𝑈𝑅𝑐33𝑅 − 𝑐13
𝑅
𝑉𝑅𝑐31𝑅 − 𝑐21
𝑅 𝑉𝑅𝑐32𝑟 − 𝑐22
𝑟 𝑉𝑅𝑐33𝑅 − 𝑐23
𝑅 ]
𝑋𝑤 =
[ 𝑋𝑤
𝑌𝑤
𝑍𝑤]
𝑏 =
[ 𝑐14
𝐿 − 𝑈𝐿𝑐34𝐿
𝑐24𝐿 − 𝑉𝐿𝑐34
𝐿
𝑐14𝑅 − 𝑈𝑅𝑐34
𝑅
𝑐24𝑅 − 𝑉𝑅𝑐34
𝑅 ]
36
El vector que contiene las coordenadas del punto 3D cuyas imágenes fueron capturadas por
las cámaras izquierda y derecha,𝑋𝑤, se puede estimar aplicando mínimos cuadrados sobre la
ecuación (27):
𝑋�̂� ≅ (𝐴𝑇 𝐴)−1𝐴𝑇𝑏 (35)
2.6 Emparejamiento de puntos característicos 3D existentes en las imágenes
estereoscópicas en los tiempos K y K-1
La metodología propuesta por (Kumar Singh, Shi-Tao Huang, & Blostein, 1987) para estimar
la matriz de transformación de cuerpo rígido relativa, 𝑇𝑘,𝑘−1, requiere de por los menos de
las coordenadas de 3 puntos característicos en 3D estimadas en el instante de tiempo K-1, y
de las coordenadas de los mismos 3 puntos en el instante de tiempo K. El emparejamiento de
puntos característicos 3D entre las imágenes capturadas en los tiempos K-1 y K es la principal
tarea a la hora de estimar la posición y la orientación del móvil, puesto que efectúa la función
de ejecutar un seguimiento o “tracking” a los píxeles característicos, es decir, dados unos
puntos característicos 3D en el par de imágenes estereoscópicas capturadas en el tiempo K-
1, el algoritmo de emparejamiento localiza estos puntos sobre las imágenes capturadas en el
tiempo K.
La técnica utilizada para el emparejamiento de los puntos característicos 3D presentes en las
imágenes K-1 y K, emplea la técnica de emparejamiento de puntos característicos 2D: En el
instante de tiempo K-1 se emparejan 3 puntos característicos 2D entre las imágenes derecha
e izquierda usando correlación + rectas epipolares, y se estiman las coordenadas 3D de dichos
puntos usando la técnica presentada en la sección anterior.
Posteriormente en el instante de tiempo K se obtienen de nuevo los puntos característicos de
la imagen izquierda y derecha, pero adicionalmente se verifica cuales puntos característicos
2D de la imagen izquierda obtenida en el instante de tiempo K-1 corresponden a los puntos
2D característicos de la imagen izquierda en el instante de tiempo K. Estos puntos
emparejados entre las imágenes izquierdas de los tiempos K-1 y K, son usados para establecer
las correspondencias con los puntos característicos de la imagen derecha en el instante de
tiempo K, para así estimar las coordenadas 3D de dichos puntos y posteriormente aplicaros
37
en la técnica propuesta por (Kumar Singh, Shi-Tao Huang, & Blostein, 1987) para la
estimación de la matriz de transformación 𝑇𝑘,𝑘−1.
2.7 Estimación de la matriz de transformación de cuerpo rígido relativa
Si se poseen dos conjuntos de puntos característicos 3D emparejados 𝐹𝐾−1 (conjunto de
puntos característicos obtenidos de las imágenes estereoscópicas en (𝐾 − 1)𝑇𝑠) y 𝐹𝐾
(conjunto de puntos obtenidos de las imágenes estereoscópicas en 𝐾𝑇𝑠), donde cada conjunto
está conformado por lo menos con las coordenadas de 3 puntos (Bay, Tuytelaars, & Gool,
2006), entonces es posible estimar la matriz de transformación de cuerpo rígido relativa, útil
para el proceso de estimación de la posición y orientación de un móvil sobre una superficie.
De la ecuación (3) de la sección 3.1, es posible determinar la relación que tiene un punto 3D
medido en el instante de tiempo K-1 y el mismo punto medido en el instante de tiempo K:
𝑋𝐾 = 𝑅𝑋𝐾−1 + 𝑇 (36)
Para estimar los valores de las matrices de rotación relativa y traslación relativa, y por ende
la matriz de trasformación de cuerpo rígido relativa, se deben seguir los siguientes pasos
recomendados por (Kumar Singh, Shi-Tao Huang, & Blostein, 1987):
1. Calcular los promedios de los vectores de coordenadas de los puntos
característicos 3D medidos en los instantes de tiempo K-1 y K, llamados 𝑝𝑖 y
𝑝′𝑖, así:
𝑃 =1
𝑁∑𝑝𝑖
𝑁
𝑖=1
𝑦 𝑃′ =1
𝑁∑𝑝′𝑖
𝑁
𝑖=1
(37)
Donde:
- P y P’ son los promedios de los conjuntos de puntos 3D característicos en los
tiempos 𝐾 − 1 y 𝐾 respectivamente.
- 𝑝𝑖 y 𝑝′𝑖 son los conjuntos de puntos característicos en los tiempos 𝐾 − 1 y 𝐾
respectivamente.
38
- N es el número total de puntos 3D característicos obtenidos y emparejados en
cada par de imágenes estereoscópicas en los instantes de tiempo 𝐾 − 1 y 𝐾.
2. Calcular la desviación de cada punto respecto a los vectores promedios P y
P’:
𝑞𝑖 = 𝑝𝑖 − 𝑃 (38)
𝑞′𝑖 = 𝑝′𝑖 − 𝑃′ (39)
Donde:
- 𝑞𝑖 y 𝑞′𝑖 son las desviaciones de cada uno de los puntos 3D característicos en
los tiempos 𝑘 − 1 y 𝑘 respectivamente.
3. Determinar la matriz H:
𝐻 = ∑𝑞𝑖 𝑞′𝑖𝑇
𝑁
𝑖=1
(40)
4. Factorizar la matriz H empleando la descomposición en valores singulares
(SVD):
𝐻 = 𝑈𝜆𝑉𝑇 (41)
Donde:
- U es una matriz cuyas columnas son mutuamente ortogonales.
- 𝜆 es una matriz diagonal que contiene los valores singulares o raíces de los
valores propios de la matriz 𝐻𝑇𝐻.
- 𝑉 es una matriz ortonormal que contiene los vectores propios de la matriz
𝐻𝑇𝐻.
39
5. Calcular la matriz Z:
𝑍 = 𝑉 𝑈𝑇 (42)
6. Finalmente se determina el valor del determinante de la matriz Z, dónde se
obtienen dos posibles resultados:
- Si det(𝑍) = +1, entonces la matriz de rotación relativa �̂� estimada es igual
a la matriz Z.
- Si det(𝑍) = −1, entonces al algoritmo ha fallado. En este caso existen dos
opciones:
I. Unos de los valores singulares de la matriz diagonal 𝜆 (por
ejemplo 𝜆𝑖) es cero, entonces la matriz de rotación deseada es:
𝑍′ = 𝑉′𝑈𝑇 (43)
Donde la matriz 𝑉′ es obtenida a partir de la matriz 𝑉,
cambiando el vector 𝑉𝑖 singular de la i - ésima columna por
−𝑉𝑖.
II. Ningún valor singular de 𝐻 es cero, entonces la solución por
mínimos cuadrados no es la apropiada, se recomienda entonces
usar la técnica RANSAC.
7. Estimar el vector de traslación:
�̂� = 𝑃′ − �̂�𝑃 (44)
Donde:
- �̂� es la matriz de traslación relativa estimada.
- �̂� es la matriz de rotación relativa estimada.
40
Partiendo de las estimaciones iterativas de la matriz 𝑇𝑘,𝑘−1 se puede establecer la matriz de
transformación de cuerpo rígido absoluta del móvil o matriz de pose, 𝐶𝐾, donde K es el
instante de tiempo donde se estima la matriz, la cual es dependiente de la matriz de
trasformación de cuerpo rígido relativa.
La matriz 𝐶𝐾 tiene la siguiente forma matemática:
10 31
1333KK
K
TRC (45)
Donde la matriz 33
KR entrega toda la información asociada a la orientación del móvil respecto
a un sistema de referencia arbitrario fijo sobre la superficie donde se mueve el robot en el
instante de tiempo K, y el vector T contiene las coordenadas 3D donde se encuentra ubicado
el centro del sistema de referencia de la cámara izquierda en el instante de tiempo K.
Para estimar la posición del vehículo en el instante de tiempo K es necesario resolver la
siguiente ecuación en diferencias:
𝐶𝐾 = 𝑇𝐾,𝐾−1𝐶𝐾−1 , 𝐶0 = 𝐶0 (46)
Donde 𝐶0 es la matriz de pose absoluta medida justo cuando el móvil comenzará a moverse,
y contiene la información de posición y orientación del robot en el instante de tiempo K=0.
41
3. Metodología experimental
La metodología propuesta para alcanzar los objetivos general y específicos trazados en el
trabajo: “Diseñar e implementar un algoritmo de estimación de coordenadas de una
trayectoria de 2 dimensiones seguida por un agente robótico móvil, empleando visión
artificial estereoscópica (Barros Gallego & Agudelo, 2015) y la estimación de
correspondencias entre puntos 3D de dos imágenes estereoscópicas calculadas en tiempos K
y K-1 usando la correlación 2D entre imágenes, con la finalidad de obtener la matriz de pose
que contiene la información de traslación y rotación del agente robótico respecto al sistema
de referencia cartesiano absoluto en el instante de tiempo K”, “Medir los errores de
estimación de las coordenadas de las trayectorias con la técnica de odometría visual
propuesta, para caminos en 2 dimensiones con formas de línea recta y caminos de 2
dimensiones cerrados con varios radios de curvatura, con la finalidad de evaluar el
comportamiento del sistema diseñado ante las condiciones cinemáticas impuestas por las
trayectorias bajo estudio” y “Diseñar e implementar una interface gráfica en Matlab, para
soportar la técnica de odometría visual estudiada” se ilustra en la figura 6:
42
Figura 6. Diagrama de bloques donde se evidencian los pasos metodológicos para implementar un
sistema de visión artificial.
A continuación, se procede a revisar detenidamente cada uno de los bloques presentados
con sus respectivas técnicas matemáticas de procesamiento digital de señales.
3.1 Implementación del sistema de visión artificial estereoscópica bicameral
El sistema de captura de imágenes estereoscópicas está conformado por 2 cámaras de
referencia LIFECAM STUDIO de Microsoft, que captura imágenes de 1920X1080 píxeles
43
HD (high definition) con formato de video YUY2, que cuenta además con un excelente
sistema óptico que atenúa considerablemente las distorsiones introducidas por la lente. Las
cámaras poseen un sistema mecánico que le permite al usuario ajustar sus ángulos de
elevación y azimutal entre 0 y 360º.
En el sistema estereoscópico bicameral empleado en el trabajo de investigación, el sistema
de referencia absoluto WWW ZYX ,, está ubicado sobre el centro de la lente de la cámara
izquierda, y el sistema de referencia cartesiano de la cámara derecha CRCRCR ZYX ,, tiene
su origen trasladado en el punto )0,0,(A respecto al origen del sistema de referencia absoluto,
y su eje CRX es colineal con el eje WX tal como se ilustra en la figura 7:
Figura 7. Sistema estereoscópico propuesto. Nótese que el sistema de referencia cartesiano de la cámara
derecha esta trasladado respecto al sistema de referencia absoluto ubicado en la cámara izquierda.
Imagen elaborada por los autores.
La disposición de cámaras enseñadas en la figura 3 reposa sobre la superficie de una
plataforma robótica, y ha sido elegida para garantizar que las rectas epipolares presentes en
la imagen derecha tengan una pendiente igual a 0 y unos interceptos con el eje vertical igual
a las coordenada Liv de cada punto i – ésimo formado en el CCD de la cámara izquierda, con
la finalidad de optimizar la tarea de emparejamiento de puntos característicos a la hora de
estimar las coordenadas 3D de puntos característicos. Más adelante en la sección 4.4 se
demostrarán estas hipótesis.
44
(Zapata Herrera & Silva Narvaez, 2015), ilustran que las cámaras LifeCam Studio poseen un
ángulo de apertura horizontal de 60°, y por lo tanto la distancia más corta a la que se puede
ubicar un objeto tridimensional para garantizar que su imagen sea capturada por ambas
cámaras está dada por la expresión (47):
2
60tan2
min
Ad (47)
Donde mind es la distancia más corta a la que se puede ubicar el objeto 3D de las cámaras y
A es la distancia de separación entre los centros de las lentes, ambas dimensiones están en
[cm].
Arbitrariamente se ha elegido una distancia de separación A igual a 9.5 [cm], lo que provoca
que la distancia mínima de detección de un objeto por ambas cámaras sea igual 8.22 [cm], lo
cual implica que durante las pruebas de calibración de ambas cámaras se deben situar los
objetos a distancias superiores a la indicada dentro del ángulo de apertura.
En la figura 8 se ilustra la disposición final del sistema de cámaras sobre el agente robótico
móvil.
45
Figura 8. Disposición del sistema de cámaras sobre el agente robótico móvil enseñando uno de los
elementos empleados de la plantilla de calibración de las cámaras.
3.2 Experimento de calibración de las cámaras y obtención del modelo matemático
para la estimación de las coordenadas 3D
En la figura 8 se ilustra la disposición del experimento de calibración de las cámaras
empleando plantillas 3D:
Al aplicar la metodología sugerida por (Zapata Herrera & Silva Narvaez, 2015), se procede
a capturar las imágenes izquierda y derecha de la plantilla 3D, y luego se tabulan en Excel
las coordenadas 3D de cada punto de la plantilla contra sus respectivas coordenadas en
píxeles capturadas por ambas cámaras.
Para la captura de los puntos 3D de la plantilla con las cámaras se elaboró un pequeño
programa en Matlab, que captura y almacena las imágenes en escala de grises con
dimensiones de 640 X 480 Píxeles, inferiores a las máximas que puede generar las cámaras,
46
con la finalidad de disminuir la carga computacional en el procesamiento de las mismas. Los
puntos 2D capturados corresponden a las esquinas de unas cuadriculas de ajedrez de 2X2 cm,
que fueron identificados usando el algoritmo de detección de esquinas propuesto por Harris,
e implementado como una función en el programa Matlab.
Para estimar las matrices de calibración de las cámaras se aplicó la técnica Homogénea
documentada por (Zapata Herrera & Silva Narvaez, 2015):
Se implementó una plantilla tridimensional donde los puntos característicos a capturar son
las esquinas de las cuadriculas de ajedrez de la plantilla, y sus coordenadas deben ser medidas
con precisión de subpíxel. Para la detección de las esquinas de las cuadriculas de ajedrez y
sus respectivas coordenadas en píxeles, se empleó el algoritmo de detección de esquinas de
Shi - Tomasi. Los puntos característicos a emplear en el proceso de calibración no pueden
estar alineados vertical u horizontalmente ya que pueden provocar inestabilidad numérica en
la estimación de las matrices de calibración de ambas cámaras. Las coordenadas en
centímetros y en píxeles de los puntos elegidos para las pruebas de calibración se tabularon
en tablas de Excel.
A partir de los datos capturados se construye el siguiente problema de vectores y valores
propios para la cámara izquierda:
[𝐿𝑇 𝐿]𝐶12𝑋1̂ = λ 𝐶12𝑋1̂ (48)
Donde L es la siguiente matriz:
122
2
2
22
22
22
22
22
22
222
222
1
1
11
1
11
11
12
11
111
111
1
0000
0
1
000
1
0000
0
1
000
1
0000
0
1
000
NLN
LN
LNW i
LNW N
LNW i
LNW N
LNW N
LNW N
W NW NW N
W NW NW N
L
L
LW
LW
LW
LW
LW
LW
WWW
WWW
L
L
LW
LW i
LW
LW
LW
LW
WWW
WWW
V
U
Vz
Uz
Vy
Uy
Vx
Ux
zyx
zyx
V
U
Vz
Uz
Vy
Uy
Vx
Ux
zyx
zyx
V
U
Vz
Uz
Vy
Uy
Vx
Ux
zyx
zyx
L
(49)
),,(,),,,(),,,(),,,( 333222111 W NW NW NWWWWWWWWW zyxzyxzyxzyx son N puntos
tridimensionales medidos desde el sistema de referencia cartesiano cuyo origen está presente
47
en la cámara izquierda, ),(,),,(),,(),,( 332211 LNLNLLLLLL VUVUVUVU son las
coordenadas de los puntos característicos medidos por la cámara izquierda asociados a cada
punto 3D.
La solución
112C es el vector propio unitario que minimiza la norma al cuadrado del error,
y corresponde al valor propio más pequeño de la matriz M.
Finalmente los elementos de este vector conforman la matriz de calibración.
El procedimiento explicado se aplica posteriormente en la cámara derecha usando los mismos
puntos característicos 3D y las coordenadas de los puntos característicos 2D medidos con la
cámara derecha.
3.3 Aplicación del algoritmo de Shi – Tomasi para la detección de esquinas presentes
en una imagen
Uno de los elementos claves en el éxito del algoritmo de estimación de la trayectorias de un
móvil empleando la técnica propuesta por (Scaramuzza & Fraundorfer, 2011), es la detección
de las esquinas de los objetos presentes en la escena capturada por ambas cámaras. Para tal
fin se aplicó un pequeño programa en Matlab que usa la función ‘corner” configurada con la
técnica de Shi – Tomasi, que presenta mayor grado de éxito en la detección de esquinas
presentes en imágenes. En la figura 9 se presenta el código de Matlab empleado para detectar
y almacenar las esquinas:
48
Figura 9. Código de Matlab empleado para detectar y almacenar las esquinas
3.4 Implementación de algoritmo de emparejamiento de puntos característicos 2D
Para el emparejamiento de características presentes en las imágenes izquierda y derecha, se
aplica el procedimiento descrito en la sección 3.2, hallando 𝜌 de cada sub – imagen. Se
sugiere que la búsqueda de los puntos característicos RiP en la imagen derecha asociados a
los puntos característicos LiP elegidos en la imagen izquierda, se ejecute sobre los puntos
que subyacen en cada una de las recta epipolares de la imagen derecha asociadas a los puntos
LiP ; pero la ejecución de esta tarea afecta negativamente el tiempo de ejecución del algoritmo
de odometría implementado, debido a que en cada iteración se debe efectuar el cálculo de
estimación de la matriz fundamental F usando los puntos característicos de ambas imágenes,
para luego obtener las ecuaciones de las rectas epipolares asociadas a cada punto LiP , por
ejemplo, si en la imagen izquierda se encontraron 10 esquinas, entonces en la imagen derecha
se deberán trazar el mismo número de rectas epipolares y recorrer cada punto de estos lugares
geométricos para buscar los puntos correspondientes RiP midiendo la máxima correlación.
Para incrementar la rapidez de ejecución del algoritmo de emparejamiento, se partió del
hecho que si el sistema bicameral presenta la configuración enseñada en la figura 3 entonces
las rectas epipolares trazadas en la imagen derecha son horizontales, y en particular si un
punto característico en la imagen izquierda tiene coordenadas 𝑃(𝑢𝐿𝑖, 𝑣𝐿𝑖), entonces la recta
epipolar asociada a dicho punto tendrá una ecuación igual a, 𝑣𝑅 = 𝑣𝐿𝑖; propiedad que reduce
drásticamente el espacio de búsqueda del punto correspondiente en la imagen derecha: A
partir del valor de la coordenada 𝑣𝐿𝑖 del punto característico LiP de la imagen izquierda, el
49
espacio de búsqueda se reduce a los puntos que pertenecen a la fila de la imagen derecha con
coordenadas en píxel 𝑣𝐿𝑖.
Debido a posibles errores de desalineamientos entre los sistemas de referencia de la cámara
izquierda y derecha, el espacio de búsqueda sugerido en la imagen derecha son las píxel -
vecindades asociadas a los puntos característicos de la imagen derecha RiP , que están dentro
de una distancia máxima de Píxel5 o Píxel10 de la fila 𝑣𝐿𝑖.
A continuación se procede a demostrar la hipótesis planteada:
Sean ),,( 1111 WWW ZYXP y ),,( 2222 WWW ZYXP dos puntos de 3 dimensiones de la escena que
son colineales con el centro del sistema de referencia de la cámara izquierda, que es el sistema
de referencia absoluto de medición del sistema de visión estereoscópica, enseñados en la
siguiente figura.
Figura 10. Sistema de referencia de la cámara izquierda ilustrando 3 puntos colineales con el centro de
la lente, que es el centro del sistema de referencia.
El vector director de la línea recta es el vector distancia entre los puntos C y 1P :
1
1
1
1
1
1
1
0
0
0
W
W
W
W
W
W
CP
Z
Y
X
Z
Y
X
r (50)
50
Un vector distancia conformada por cualquier punto de la línea recta ),,( WWW ZYXP y el
origen debe ser paralelo al vector CPr 1
:
Rrr CPPC ,1 (51)
Al reemplazar (50) en (51) se obtiene que la ecuación de cualquier punto que esté sobre la
línea recta es igual a:
1
1
1
1
1
1
0
0
0
W
W
W
W
W
W
W
W
W
W
W
W
Z
Y
X
Z
Y
X
Z
Y
X
Z
Y
X
(52)
Por lo tanto las coordenadas 3D del punto ),,( 2222 WWW ZYXP deben ser iguales a:
R
Z
Y
X
Z
Y
X
W
W
W
W
W
W
,
1
1
1
2
2
2
(53)
Las coordenadas 2D de los puntos ),,( 1111 WWW ZYXP y ),,( 2222 WWW ZYXP formados en los
CCD’s de las cámaras izquierda y derecha:
),(),,(),,(),,( 222111222111 RRRRLLLL VUPVUPVUPVUP se pueden calcular usando las
matrices de calibración de ambas cámaras (Zapata, 2015):
1
1
1
1
43
1
11
11
W
W
W
L
HL
HLL
HLL
Z
Y
X
C
t
tV
tU
,
1
2
2
2
43
2
22
22
W
W
W
L
HL
HLL
HLL
Z
Y
X
C
t
tV
tU
,
(54)
1
1
1
1
43
1
11
11
W
W
W
R
HR
HRR
HRR
Z
Y
X
C
t
tV
tU
,
1
2
2
2
43
2
22
22
W
W
W
R
HR
HRR
HRR
Z
Y
X
C
t
tV
tU
(55)
51
Donde 1HLt ,
1HRt ,2HLt y
2HRt son las coordenadas homogéneas asociadas a los puntos
formados en os CCD’s de las cámaras.
La matriz de calibración de la cámara izquierda es igual a:
1333
333343LLLL TIRKC (Zhang, 2004) (56)
Donde LK es la matriz de parámetros intrínsecos de la cámara (parámetros constructivos),
LR es la matriz de rotación que contiene los ángulos de giro del sistema de referencia absoluto
con respecto al sistema de referencia cartesiano de la cámara, y LT es el vector de traslación
que contiene las coordenadas de ubicación del centro de la cámara izquierda medidas
respecto al sistema de referencia arbitrario WWW ZYX ,, multiplicadas por -1. La matriz
I es la identidad.
Si el sistema de referencia arbitrario WWW ZYX ,, es el mismo sistema cartesiano de
referencia de la cámara izquierda, entonces:
100
010
001
IRL ,
0
0
0
LT (57)
Al reemplazar (57) en (56) y teniendo presente que la matriz LK es igual a:
010
0
0
010
0
0
2322
1211
kk
kk
Fv
uF
K C
C
L
Donde , son las densidades lineales de píxeles en las direcciones horizontal y vertical
del CCD de la cámara en
m
Píxely F es la distancia focal de la lente en m , entonces se
obtiene la expresión que rige a la matriz de calibración de la cámara izquierda:
52
0010
00
00
2322
1211
43
kk
kk
CL (58)
La matriz de calibración de la cámara derecha se calcula así:
1333
333343RRRR TIRKC (59)
Si las dos cámaras empleadas son idénticas, como en el caso del presente trabajo
investigativo, entonces se cumple que:
RL Kkk
kk
K
010
0
0
2322
1211
(60)
Además si la cámara derecha no presenta rotaciones con respecto al sistema de referencia
absoluto WWW ZYX ,, presente en la cámara 1, entonces:
100
010
001
IRR (61)
Por último, si el centro de la lente de la cámara derecha está ubicado en las coordenadas
RAA ),0,0,( , tal como se ha planteado en el presente trabajo, entonces el vector de
traslación RT será igual a:
0
0
A
TR (62)
Al reemplazar las expresiones (60), (61) y (62) en (59) se obtiene que:
0010
00
0
2322
111211
43
kk
Akkk
CR (63)
53
Por lo tanto de las expresiones (54) y (55) se tiene que las coordenadas 2D de los puntos
),,( 1111 WWW ZYXP y ),,( 2222 WWW ZYXP formados en las imágenes izquierda y derecha son
iguales a:
132
112111
1
W
WWL
Yk
YkXku
&
132
123122
1
W
WWL
Yk
ZkYkv
(64)
132
112111
2
W
WWL
Yk
YkXku
&
132
123122
2
W
WWL
Yk
ZkYkv
(65)
Nótese que las coordenadas en 2D de puntos 3D colineales con el centro de la lente de la
cámara izquierda, son idénticas, es decir dos puntos 3D colineales con el centro de la lente
de la cámara izquierda se convierten en un único punto 2D en la imagen capturada.
Además:
132
11211111
1
W
WWR
Yk
YkXkAku
&
132
123122
1
W
WWR
Yk
ZkYkv
(66)
132
11112111
2
)(
W
WWR
Yk
AkYkXku
&
132
123122
2
W
WWL
Yk
ZkYkv
(67)
Nótese de las expresiones (66) y (67) que los puntos colineales ),,( 1111 WWW ZYXP y
),,( 2222 WWW ZYXP se mapean en dos puntos de 2 dimensiones en la imagen de la cámara
derecha, que definen la ecuación de la línea recta epipolar de dichos puntos, pero lo más
interesante es que las alturas de los 2 puntos formados en la imagen 2 son idénticas (lo cual
implica que la pendiente de la recta epipolar es igual a 0) y tienen la misma altura del punto
formado en la imagen capturada por la cámara izquierda:
2121 LLRR vvvv (68)
Por lo tanto la ecuación de la recta epipolar será igual a:
10 LRR vuv (69)
Concluye así la demostración planteada.
54
Para efectuar el proceso de emparejamiento de puntos 2D característicos se propone la
siguiente metodología presentada en la siguiente figura:
Figura 11. Metodología para estimar la correspondencia entre puntos característicos 2D presentes en
las imágenes derecha e izquierda
Para depurar los resultados de correspondencia o emparejamiento de puntos característicos
presentes en la imagen izquierda y derecha, se procedió a estimar las coordenadas 3D de
dichos puntos y se utilizaron las siguientes reglas para descartar las parejas de puntos
característicos 2D erróneamente emparejados por el algoritmo:
55
- Puntos 2D emparejados que representen un punto 3D que tenga una altura estimada
mayor a 200 cm e inferior a -25cm, que es la altura de la superficie plana y rígida
por donde navega el agente robótico móvil.
- Puntos 2D emparejados que representen un punto 3D que tengan un valor en su
coordenada “Y” inferior a 30 cm que equivale a la distancia mínima detectable de
un punto 3D por ambas cámaras, o superior a 400 cm debido al error presente en la
estimación de las coordenadas 3D para estas distancias.
3.5 Implementación de algoritmo de emparejamiento de puntos característicos 3D de
imágenes en los instantes de tiempo K-1 y K
El desarrollo del algoritmo para el emparejamiento de características presentes en las
imágenes capturadas en los tiempos k-1 y k, fundamentalmente es el mismo que el utilizado
para el emparejamiento de puntos característicos entre cámaras izquierda y derecha. La
diferencia reside en que las condiciones propuestas para algoritmo no se cumplen para la
condición de movimiento, pues no es posible utilizar la técnica de rectas epipolares que limita
el espacio de comparación del algoritmo. Por esta razón se propone un algoritmo que realice
una comparación entre los puntos característicos capturados en el tiempo k-1 y los puntos
característicos capturados en el tiempo k, limitando el cálculo de correlación al número de
puntos característicos hallados.
Para llevar a cabo este procedimiento se propone la siguiente metodología ilustrada en la
siguiente figura:
56
Figura 12. Metodología propuesta por el equipo de trabajo para estimar la correspondencia entre
puntos característicos presentes en las imágenes capturadas en los tiempos K-1 y K.
3.6 Estimación de las coordenadas 3D de los puntos característicos emparejados en las
imágenes izquierda y derecha
A partir de las matrices de calibración es posible estimar las coordenadas 3D de un (1) punto
de la escena capturado simultáneamente por ambas cámaras, usando los puntos
característicos 2D emparejados de las imágenes izquierda - derecha y resolviendo el sistema
de ecuaciones (34) presentado en el capítulo 3:
𝐴4×3𝑋𝑤3×1= 𝑏4×1 (34)
57
Dónde:
𝐴 =
[ 𝑈𝐿𝑐31
𝐿 − 𝑐11𝐿 𝑈𝐿𝑐32
𝐿 − 𝑐12𝐿 𝑈𝐿𝑐33
𝐿 − 𝑐13𝐿
𝑉𝐿𝑐31𝐿 − 𝑐21
𝐿 𝑉𝐿𝑐32𝐿 − 𝑐22
𝐿 𝑉𝐿𝑐33𝐿 − 𝑐23
𝐿
𝑈𝑅𝑐31𝑅 − 𝑐11
𝑅 𝑈𝑅𝑐32𝑅 − 𝑐12
𝑅 𝑈𝑅𝑐33𝑅 − 𝑐13
𝑅
𝑉𝑅𝑐31𝑅 − 𝑐21
𝑅 𝑉𝑅𝑐32𝑟 − 𝑐22
𝑟 𝑉𝑅𝑐33𝑅 − 𝑐23
𝑅 ]
𝑋𝑤 =
[ 𝑋𝑤
𝑌𝑤
𝑍𝑤]
𝑏 =
[ 𝑐14
𝐿 − 𝑈𝐿𝑐34𝐿
𝑐24𝐿 − 𝑉𝐿𝑐34
𝐿
𝑐14𝑅 − 𝑈𝑅𝑐34
𝑅
𝑐24𝑅 − 𝑉𝑅𝑐34
𝑅 ]
El vector que contiene las coordenadas del punto 3D cuyas imágenes fueron capturadas por
las cámaras izquierda y derecha, 𝑋𝑤, se puede estimar aplicando mínimos cuadrados sobre
la ecuación (34):
𝑋�̂� ≅ (𝐴𝑇 𝐴)−1𝐴𝑇𝑏 (35)
3.7 Implementación de algoritmo de Arun para la estimación de la matriz de
transformación de cuerpo rígido relativa
Para estimar la matriz de trasformación de cuerpo rígido relativa es necesario contar por lo
menos con las coordenadas de 3 puntos 3D correspondientes medidas en los instantes de
tiempo K y K-1 (Bay, Tuytelaars, & Gool, 2006). A continuación en la figura 13 se muestran
los pasos necesarios para la estimación de la matriz de rotación de cuerpo rígido relativa.
58
Figura 13. Pasos metodológicos necesarios para la estimación de la matriz de rotación de cuerpo rígido.
59
Se propone para este algoritmo el uso de filtros que ayuden a eliminar datos erróneos,
mostrando al final solo los datos que sean considerados correctos. Las reglas propuestas para
aceptar / rechazar los datos son:
- Las coordenadas del eje X presente en la matriz de transformación de cuerpo rígido
relativa no deben superar los ±10 [cm], debido a que es imposible que el agente
robótico móvil tenga un desplazamiento de este tipo.
- Las coordenadas del eje Y presente en la matriz de transformación de cuerpo rígido
relativa no deben superar los ±10 [cm], ya que se considera de antemano que el agente
robótico móvil sólo puede realizar movimientos cortos, para una mejor estimación
del movimiento.
- Las coordenadas del eje Z presentes en la matriz de transformación de cuerpo rígido
relativa no deben superar los ±5 [cm], debido a que el agente robótico móvil nunca
presentará un desplazamiento sobre este eje, debido a la superficie donde se mueve
que es totalmente plano (suelo zona de laboratorios).
3.8 Diseño e implementación de un algoritmo para la estimación de la matriz de pose
de las cámaras
Para la estimación de la matriz de pose, que contiene la información de posición y orientación
del agente robótico móvil, es necesario conocer la ubicación inicial del agente robótico y la
matriz de transformación de cuerpo rígido relativa 𝑇𝑘,𝑘−1. Partiendo de las estimaciones
iterativas de la matriz 𝑇𝑘,𝑘−1 podemos establecer una posición del vehículo 𝐶𝑛 (donde n es el
instante de tiempo donde se estima la posición), la cual es dependiente de la matriz de
trasformación de cuerpo rígido estimada constantemente, así para estimar la posición del
vehículo en el instante de tiempo n tenemos que:
𝐶𝐾 = 𝑇𝐾,𝐾−1𝐶𝐾−1
Donde:
- 𝐶𝐾 es la posición actual estimada del vehículo
- 𝐶𝐾−1 es la posición inmediatamente anterior de vehículo
60
- 𝑇𝐾,𝐾−1 es la matriz de rotación de cuerpo rígido estimada a partir del algoritmo
propuesto por Arun.
Para determinar las matrices de transformación de cuerpo rígido relativas se aplicará la metodología
explicada en la sección 3.5 del capítulo 3, usando los puntos característicos 3D emparejados en los
instantes de tiempo K-1 y K.
3.9 Diseño de experimento para la estimación de la trayectoria del agente robótico
móvil, y de la medida del error cometido por la técnica de odometría
Para el experimento se guio el vehiculo por un segmento de linea recta como se ilustra en la
figura 14, y se realizaron las capturas de imagen con desplazamientos de 10 cm para evaluar
el funcionamiento del algoritmo con movimientos basicos.
Figura 14. Sistema de referencia absoluto empleado durante las pruebas. En particular el vehículo se
movió en la trayectoria del eje Y_W.
Para el segundo experimento se guío el vehículo a través de una trayectoria curva como se
muestra en la figura 15, se realizaron las capturas de imagen con desplazamientos de cada 10
cm para evaluar el funcionamiento del algoritmo con un movimiento un poco más complejo.
61
Figura 15. . Toma de Experimento 2: Trayectoria curvilínea seleccionada para el móvil.
62
4. Resultados y análisis
A partir de la aplicación de la metodología expuesta en el capítulo 4, se procede a ilustrar los
resultados experimentales obtenidos.
4.1 Calibración de las cámaras y obtención del modelo matemático para la estimación
de las coordenadas 3D
Para la estimación de las matrices de calibración se realiza el montaje propuesto en la figura
8; par al estimación de las matrices de calibración se tabularon los datos en Excel de las
coordenadas 3D de cada uno de los centros de las cuadriculas dispuestas en las balizas,
posteriormente se ejecutó el algoritmo captura de detección de esquinas “Harris” en Matlab,
y se tabularon las coordenadas en píxeles de las esquinas de las cuadriculas; los datos
tabulados se presentan en la siguiente tabla:
Tabla 1. Datos de las esquinas elegidas para la calibración de las cámaras
Punto Coordenadas del punto
Coordenadas en píxeles
cámara izquierda
Coordenadas en píxeles
cámara derecha
X(cm) Y(cm) Z(cm) Upl Vpl Upr Vpr
p1 -91,5 243,2 27 60 150 28 149
p2 -30,5 152 2 177 213 132 213
p3 61 213,5 27 497 142 467 141
p4 30,5 122 2 475 213 423 212
p5 30,5 274,5 52,5 381 100 356 98
p7 0 183 2 309 217 273 216
p10 -122 335,5 74 66 80 41 78
p11 -61 335,5 52,5 190 123 168 121
p12 0 335,5 74 307 83 288 81
Posteriormente se ingresaron los datos al software Derive para realizar los cálculos
pertinentes de dicha matriz; la matriz ingresada al Derive fue:
63
Figura 16. Matriz para la calibración de la cámara izquierda
Ahora se realizan los cálculos especificados por (Zapata y Narváez, 2015) para la estimación
de la matriz de calibración, obteniendo como resultado la siguiente matriz:
Tabla 2. Matriz de calibración estimada para cámara izquierda
Matriz de calibración de la cámara izquierda
0.0756 0.0404 -0.0132 0.8537
0.0008 0.0284 -0.0797 -0.5064
0.0000 0.0001 -0.0000 -0.0022
Posteriormente, se realiza el mismo procedimiento para la estimación de la matriz de
calibración izquierda
64
Figura 17. Matriz para la calibración de la cámara izquierda
Obteniendo la siguiente matriz de calibración:
Tabla 3. Matriz de calibración estimada para cámara derecha
Matriz de calibración de la cámara derecha
0.0547 0.0276 -0.0074 -0.9627
0.008 0.0195 -0.0555 -0.2569
0.0000 0.0001 0.0000 -0.0013
El algoritmo de estimación de coordenadas 3D parte de las matrices de calibración izquierda
y derecha para la estimación de coordenadas 3D. Para la verificación de estimación de
coordenadas, ahora bien, a continuación, se muestra una tabla la cual contiene los puntos
estimados a partir de las matrices de calibración, el cálculo del error relativo de las
coordenadas estimadas con respecto a las coordenadas reales de las esquinas de las balizas:
65
Tabla 4. Calculo del error relativo de las coordenadas 3D estimadas con el algoritmo de matrices de
calibración, con respecto a las coordenadas reales del sistema de balizas
Coordenadas del punto
reales
Coordenadas del punto
estimadas Errores relativos estimados
X(cm) Y(cm) Z(cm) X Y Z Error eje
X
Error
eje Y
error
eje Z
-91,5 243,2 27 -93,9058 248,2227 27,7639 2,629% 2,065% 2,829%
-30,5 152 2 -28,8351 148,5395 1,9562 5,459% 2,277% 2,190%
61 213,5 27 61,6135 215,1163 27,4339 1,006% 0,757% 1,607%
30,5 122 2 30,9168 125,1306 1,9664 1,367% 2,566% 1,680%
30,5 274,5 52,5 28,8026 257,8121 49,4402 5,565% 6,079% 5,828%
0 183 2 -0,1013 180,5448 1,5389 N/A 1,342% 23,055%
-122 335,5 74 -118,609 327,2598 71,9881 2,779% 2,456% 2,719%
-61 335,5 52,5 -62,5251 341,7747 53,851 2,500% 1,870% 2,573%
0 335,5 74 -0,155 352,6124 77,4565 N/A 5,101% 4,671%
4.2 Aplicación del algoritmo de Shi – Tomasi para la detección de esquinas presentes
en una imagen
El algoritmo propuesto en la figura 9 para la detección de esquinas, fue ejecutado en Matlab,
sobre una captura realizada por una de las cámaras del sistema estereoscópica, la figura
mostrada a continuación señala con una ‘x’ lo que el algoritmo considera como esquina, el
cual se puede notar a simple vista su buen funcionamiento. Cabe señalar que el todo el
algoritmo depende de la buena estimación de esquinas, es decir, que las esquinas encontradas
verdaderamente lo sean, pues un error en esta etapa (que se repite de manera iterativa durante
todo el proceso de estimación de movimiento) puede causar fatales resultados a la hora de
emparejar puntos que no sean esquinas, debido a que las características de intensidad que
poseen las esquinas no necesarias para el buen funcionamiento de los algoritmos de
correlación.
A continuación, se muestra una imagen capturada por la cámara izquierda y las esquinas
estimadas por el algoritmo de detección de esquinas de Matlab:
66
Figura 18. Aplicación de algoritmo de estimación de esquinas sobre una imagen
4.3 Algoritmo de emparejamiento de puntos característicos 2D
El algoritmo para estimar la correlación entre sub- imágenes de las imágenes izquierda y
derecha, fundamentalmente realiza la comparación entre cada sub – imagen para estimar su
similitud, para evitar realizar este proceso de comparación de una sub – imagen patrón de la
cámara izquierda con todas las posibles sub – imágenes de la cámara derecha, se realiza esta
comparación solo con los puntos característicos hallados sobre la cámara derecha, este
proceso de comparación se ilustra de una manera clara en la siguiente figura.
67
Figura 19. Metodología propuesta para el cálculo de correlación entre sub – imágenes de la cámara
izquierda y derecha
En la figura 18 se puede ver el recorrido que realiza el algoritmo para la estimación de
correlación, así: se compara una sub – imagen patrón con centro en las coordenadas
(𝑈𝐿1, 𝑉𝐿1), con cada sub – imagen centrada en las coordenadas de las esquinas presentes en
la cámara derecha, además de la aplicación del filtro sobre la coordenada vertical, esto es,
solo se efectúa el cálculo de la correlación si las coordenadas verticales de los centros de las
sublimarnos tienen un valor cercano, solo se realiza correlación si 𝑉𝐿𝑖 = 𝑉𝑅𝑗 ± 10.
A continuación, se muestran las imágenes capturadas por las cámaras derechas e izquierda,
y sus esquinas emparejadas.
68
Figura 20. Imágenes capturadas por la cámara derecha e izquierda respectivamente, en donde se
muestra el emparejamiento estimado por el algoritmo.
Tabla 5. Coordenadas de los puntos característicos presentes en las imágenes capturadas por las
cámaras izquierda y derecha emparejados.
𝑈𝐿𝐾−1 𝑉𝐿𝐾−1 𝑈𝑅𝐾−1 𝑉𝑅𝐾−1
200 215 161 216
443 205 371 205
455 206 384 206
198 300 157 301
196 300 157 301
202 257 164 258
207 214 160 223
454 219 383 220
432 190 360 190
202 224 164 225
207 224 157 233
322 251 294 252
196 257 158 258
216 239 192 240
451 368 379 370
430 368 351 366
Se puede ver claramente de la figura 19 que los puntos señalados son los estimados por el
algoritmo de emparejamiento, mostrados en la tabla; Esta tabla muestra todas las coordenadas
de los puntos característicos emparejados presentes en las imágenes tomadas por las cámaras
derechas e izquierda. Si se realiza una revisión exhaustiva del emparejamiento de cada una
de las esquinas podemos encontrar puntos falsos, es decir, que el algoritmo de
69
emparejamiento haya fallado. Este problema es fácil de detectar sobre la imagen, pero
matemáticamente es complicado saber el emparejamiento de dos esquinas fue correcto, para
ello la razón del filtro aplicado, el cual dependiendo de las coordenadas 3D estimadas a partir
de las esquinas emparejadas, se procede a evaluar los valores de los puntos. La aplicación de
este filtro redujo en gran medida la aparición de falsos positivos durante la aplicación de
algoritmo de emparejamiento, pues la existencia de zonas similares generaba una gran
aparición de emparejamiento erróneas; por esta razón en los posteriores experimentos se
procuraba garantizar que la zona donde se realizó no contuviera elementos similares
presentes en la escena, (por ejemplo, sillas iguales, mesas similares, entre otros).
4.4 algoritmo de emparejamiento de puntos característicos de imágenes en los
instantes de tiempo K-1 y K
Para realizar la implementación del algoritmo de correspondencia entre las imágenes tomadas
con la cámara izquierda en los tiempos K-1 y K, se planteó un movimiento hacia adelante
(sin ser un movimiento totalmente recto), lo que se puede notar claramente en la figura 20.
Es importante aclarar que es de vital importancia garantizar que los movimientos sean cortos,
es decir, movimientos que no superen los 10 cm si son movimientos hacia adelante con una
rotación mínima del agente robótico móvil, pero si el movimiento realizado es curvo, se debe
garantizar que la rotación sea de máximo de 10 a 15 grados. Las restricciones de movimiento
fundamentalmente se hacen para evitar cambios fuertes en la imagen capturada que pueden
generar cambios de iluminación y perspectiva, o simplemente que las esquinas capturadas
desaparezcan.
70
Figura 21. Imágenes capturadas por la cámara izquierda en los tiempos K-1 y K, en donde se muestra
el emparejamiento estimado por el algoritmo
Tabla 6. Coordenadas de los puntos característicos presentes en las Imágenes capturadas por la cámara
izquierda en los tiempos K-1 y K.
𝑈𝐿𝐾−1 𝑉𝐿𝐾−1 𝑈𝑅𝐾−1 𝑉𝑅𝐾−1 𝑈𝐿𝐾 𝑉𝐿𝐾 𝑈𝑅𝐾 𝑉𝑅𝐾
200 215 161 216 242 217 201 216
443 205 371 205 526 204 443 203
455 206 384 206 542 205 458 205
198 300 157 301 240 308 199 308
196 300 157 301 237 307 196 308
202 257 164 258 244 261 204 261
207 214 160 223 238 261 197 261
454 219 383 220 396 147 366 146
432 190 360 190 243 306 203 307
202 224 164 225 512 396 427 396
207 224 157 233 249 226 197 235
322 251 294 252 242 217 201 216
196 257 158 258 238 261 197 261
216 239 192 240 526 204 443 203
451 368 379 370 537 397 434 402
430 368 351 366 512 396 427 396
71
4.5 Implementación de algoritmo de ARUN para la estimación de la matriz de
transformación de cuerpo rígido relativa y matriz de pose.
La estimación de la matriz de transformación de cuerpo rígido relativa es de vital importancia
en el algoritmo total de estimación de movimiento, pues se encarga de estimar la matriz que
contiene la información de los movimientos de rotación y traslación relativa que haya tenido
el agente robótico móvil. Para la aplicación del algoritmo de estimación de la matriz de
trasformación de cuerpo rígido se parte del hecho de que ya se poseen conjuntos de puntos
característicos presentes en la imagen izquierda capturada en el tiempo K-1, emparejados con
puntos característicos presentes en la imagen izquierda capturada en el tiempo K, de los
cuales se tiene conocimiento de sus coordenadas 3D. Partiendo de que mínimamente se
necesitan un conjunto de 3 puntos característicos emparejados entre dos imágenes izquierdas
consecutivas, se procede formar conjuntos de 3 puntos emparejados entre imágenes
consecutivas y estimar la matriz de transformación de cuerpo rígido. Dada la aplicación del
filtro mencionada anteriormente se descartan puntos considerados erróneos, entonces los
datos obtenidos se asumen que son correctos.
A continuación, se muestran la tabla que contiene el tiempo de la captura de imágenes, la
descripción del punto capturado, y las coordenadas sobre los ejes X y Y, de los puntos
mencionados en las imágenes izquierdas capturadas de manera consecutiva
Tabla 7. Coordenadas de los puntos característicos emparejados en las imágenes izquierdas capturadas
en tiempos consecutivos en el experimento de movimiento recto de 30 cm.
Movimiento Recto 30cm
Tiempo de la
captura Descripción del punto X(k-1) Y(k-1) X(k) Y(k)
k-1
p3 superior derecha -21,43 177,1 -20,14 162,63
p2 central 4,99 149,24 5,17 139,41
p7 inferior derecha 38,55 125,83 36,85 114,39
K
p3 superior derecha -20,14 162,63 -17,83 153,78
p2 central 5,17 139,41 7,4453 128,19
p7 inferior derecha 36,85 114,39 41,039 106,057
k+1
p3 superior derecha -17,83 153,78 -18,63 143,85
p2 central 7,4453 128,19 8,9795 117,18
p2 superior derecha 9,6158 128,4686 10,7155 117,37
72
En la tabla 7, se relacionan las coordenadas de los puntos en 2 dimensiones emparejados
entre los tiempos K-1, K y K+1 de ambas cámaras, en el experimento de movimiento recto
de 30 cm. Se puede observar que en el tiempo de la toma k-1 para las coordenadas Y(k-1) y
Y(k) el desplazamiento fue de 15 cm y consecutivamente para los siguientes puntos el
desplazamiento fue de 10 cm aproximadamente, esto se acerca a lo esperado, ya que los
movimientos reales fueron de 10 cm, aunque también se puede observar una variación en las
coordenadas estimadas sobre el eje X, lo cual no es de esperarse pues el agente robótico
móvil no realizó ningún movimiento sobre este eje.
Tabla 8. Coordenadas de los puntos característicos emparejados en las imágenes izquierdas capturadas
en tiempos consecutivos en el experimento de movimiento curvo
Movimiento Curvo
Numero de
movimiento Descripción del punto X(k-1) Y(k-1) X(k) Y(k)
k-1
p4 esquina inferior central 12,1787 88,7868 -6,68 74,64
p2 esquina central 47,0343 147,2314 14,749 136,077
esquina #2 derecha 51,9 142,59 20,9709 141,3323
K
p2 esquina central 14,749 136,077 1,7188 125,93
p7 esquina inferior izq 48,56 118,25 37,32 111,53
punto 2 numero 13,4758 134,76 1,2 128,14
k+1
p2 esquina central 1,7188 125,93 -24,3025 119,67
p7 esquina inferior izq 37,32 111,53 9,7339 101,7275
p2 numero 1,2 128,14 -29,9265 124,66
En la tabla 8, se relacionan las coordenadas de los puntos en 2 dimensiones correlacionados
entre los tiempos K-1, K y K+1 de ambas cámaras, en el movimiento curvo. Se puede
observar que en el tiempo de la toma k-1 para las coordenadas Y(k-1) y Y(k) los
desplazamientos están entre 5cm -10 cm los cuales se acercan a lo esperado, en comparación
con los movimientos reales.
73
Tabla 9. Coordenadas X y Y, extraídas de la matriz de translación, estimadas por el algoritmo de
cálculo de la matriz de calibración de cuerpo rígido
Movimiento recto 30cm
Matriz de traslación relativo estimada Matriz de traslacion
relativo real
Tiempo X Y X Y
k-1 0 0 0 0
K 5,3 9,29 0 10
k+1 -13,55 9,04 0 10
k+2 -13,53 9,72 0 10
En la tabla 9, se ilustra las coordenadas sobre el eje X y el eje Y de las matrices de traslación
calculadas con el algoritmo de estimación de matriz de trasformación de cuerpo rígido, para
el movimiento recto de 30 cm, no se muestran las coordenadas sobre el eje z ya que el
movimiento fue realizado sobre un plano de dos dimensiones.
Tabla 10. Coordenadas extraídas de la matriz de pose, estimada a partir del algoritmo de arun
Movimiento recto 30cm
Matriz de traslación estimada Matriz de traslación real
Tiempo X Y X Y
k-1 0,00001 0,00001 0 0
K 5,3 9,29 0 10
k+1 -8,25 18,33 0 20
k+2 -21,78 28,05 0 30
En la tabla 10, se ilustra las coordenadas extraídas de la matriz de pose calculadas de manera
iterativa a partir de las matrices de trasformación de cuerpo rígido relativas estimadas con el
algoritmo de Arun, para el movimiento recto de 30 cm en el eje X y el eje Y.
74
Figura 22. Grafica de la trayectoria estimada en el experimento 1
En la figura 22 se grafica el movimiento estimado por la matriz de pose del experimento de
movimiento recto de 30 cm.
Con los resultados obtenidos podemos establecer, que las coordenadas de los puntos
característicos estimadas reflejan el movimiento que en realidad realizo el agente robótico
móvil, pero a la hora de realizar el cálculo de la matriz de transformación e cuerpo rígido, las
medidas obtenidas reflejan un movimiento diferente. Además, cabe resaltar que se mejoró
notablemente la velocidad de ejecución del algoritmo, el cual es el mayor problema a la hora
de realizar un sistema de odometría visual.
0
5
10
15
20
25
30
35
-25 -20 -15 -10 -5 0 5 10
Dis
tan
cia
eje
Y (
cm)
Distancia eje X (cm)
Movimiento Recto 30cm
Estimado
Real
75
5. Conclusiones
1. Es de vital importancia el buen funcionamiento del algoritmo de emparejamiento de
características entre imágenes derechas e izquierda, pues de este depende el cálculo de
coordenadas 3D y por ende la estimación de la matriz de translación de cuerpo rígido, esto
se logra con una buena práctica y elaboración del experimento de calibracion de las cámaras.
2. Garantizar que el físico de odometría visual no cambie, es decir, la disposición física de
los elementos que lo componen, es de vital importancia para garantizar la correcta estimación
de coordenadas 3D
3. la incidencia de luz artificial afecta seriamente la perspectiva y los niveles de luz sobre los
objetos, haciendo que para movimientos cortos los niveles de intensidad sobre los objetos
cambien notablemente, dificultando la ejecución del algoritmo de emparejamiento, por ello
se deben garantizar movimientos muy cortos.
4. El algoritmo propuesto por Arun, para la estimación de matriz de trasformación de cuerpo
rígido presenta inconvenientes cuando los datos ingresos no son perfectos, esta presente alta
sensibilidad a cambios pequeños en las coordenadas, generando grandes errores en las
matrices estimadas
76
Referencias
Agudelo España, D. A., Silva Lopez, J. S., & Gil Lopez, J. D. (2013). Estimacion De La
Localizacion De Un Vehiculo Usando Un Sistema De Vision Por Computador.
Pereira: Universidad Tecnologica De Pereira.
Aperador Chaparro, W. A., Plaza Torres, M., & Cifuentes Bernal, A. (2014). Silla de
ruedas eléctrica con un dispositivo de imágenes térmicas para el control de
desplazamiento en personas invidentes y con limitaciones de movilidad. Bogota:
Universidad Militar Nueva Granada.
Barros Gallego, S., & Agudelo, L. (2015). Implementacion De Un Algoritmo De
Correspondencia Entre Pixeles. Medellin: Universidad de San Buenaventura.
Bay, H., Tuytelaars, T., & Gool, L. V. (2006). SURF: Speeded Up Robust Features.
Leuven: Katholieke Universiteit Leuven.
Colciencias. (2013). Electrónica, Telecomunicaciones e Informática. Obtenido de
https://goo.gl/8ISjSS
Cruz, C. L. (2013). Seguimiento Automatico de Objetos en Sistemas con Multiples
Camaras. Madrid: Universidad Autonoma de Madrid.
Durrant-Whyte, H., & Bailey, T. (2012). Simultaneous localization and mapping: part I.
IEEE Robotics & Automation Magazine, Volume:13 , Issue: 2.
Gómez Gómez, D. H. (2015). Desarrollo de una técnica SLAM para ambientes dinámicos
tridimensionales. Bogota: Universidad Nacional de Colombia.
Hall, E. L. (s.f.). Measuring Curved Surfaces For Robot Vision. Comput J No 15, pp. 42-
54.
Harris, C., & Sthepens, M. (1988). A Combined Corner And Edge Detector. United
Kingdom: Plessey Research Roke Manor.
Kehagias, A., Djugash, J., & Singh, S. (2006). Range-only SLAM with Interpolated Range
Data. Pittsburgh: Carnegie Mellon University.
Kumar Singh, A., Shi-Tao Huang, T., & Blostein, S. (1987). Least-Squares Fitting of two
3-D Points Sets. IEEE Transactions on Pattern Analysis and Machine Intelligence,
Volume PAM1-9 No 5.
77
Moravec, H. (1980). Obstacle Avoidance and Navigation in the Real World by a Seeing
Robot Rover. Stanford: Computer Science Department Stanford University.
Nister, D., Naroditsky, O., & Bergen, J. (2004). Visual Odometry for Ground Vehicle
Applications. Princeton: Sarnoff Corporation.
Paniagua, L. J. (2014). Localizacion de Robots Moviles de Recursos Limitados Basados en
Fusion Sensorial por Eventos. Valencia: Universidad Pontificia de Valencia.
Rodríguez-Aragón, F. D. (2015). Un Sistema de Odometría Visual Monocular bajo
Restricciones de Tiempo Real. Salamanca: Universidad de Salamanca.
Scaramuzza, D., & Fraundorfer, F. (2011). Visual Odometry: Part I - The First 30 Years
and Fundamentals. IEEE Robotics and Automation Magazine, Volume 18.
Thrun, S. (October de 2010). https://googleblog.blogspot.com.co/2010/10/what-were-
driving-at.html.
Zapata Herrera, A. M., & Silva Narvaez, J. E. (2015). Reconstrucción de Entornos 3D
mediante un Sistema de Vision Artifcial Estereoscopico, para aplicaciones en
Navegacion Robotica. Medellin: Universidad de San Buenaventura.
Zhang, Z. (2000). A Flexible New Technique For Camera Calibration. IEEE, 1330-1334.
Zisserman, H. (2010). Epipolar Geometry. CVPR’99 tutorial.
