1 Navegación en espacios exteriores no estructurados mejorada mediante Odometría Visual Master Universitario en Sistemas Electrónicos Avanzados. Sistemas Inteligentes. Trabajo Fin de Master. Dirigido por: D. Rafael Barea Realizado por: D. Ramón Rodríguez

2 Navegación en espacios exteriores no estructurados mejorada mediante Odometría Visual Estado del Arte Objetivos Recursos utilizados Algoritmo Resultados Conclusiones y trabajos futuros Agradecimientos

3 Estado del Arte (I) Real-Time Stereo Visual Odometry for Autonomous Ground Vehicles (A. Howard) Bumblebee -> Disparidad Detección de inliers: Detección Harris. Mapa de correlación (SAD) entre características. Compatibilidad Bilateral. Distancia constante. Error de 0.25% en 400m.

4 Estado del Arte (II) 3D Visual Odometry for Road Vehicles (R.García) Unibrain Fire-i. Automóvil en ciudad. Correlación ZMNCC (ventana). Detector Harris. Eliminación outliers. RANSAC. Error de 2.1% en 135m.

5 Estado del Arte (III) Real-time Localization in Outdoor Env. using Stereo V. and Inexp. GPS (M.Agrawal) Bumblebee. Harris. NCC. Doble matching R-L. Fusión con GPS. Error de 4.8% en 141m. (0.3% con GPS)

6 Estado del Arte (IV) Large Scale Visual Odometry for Rough Terrain (K.Konolige, M.Agrawal y J.Solà) Pareja de cámaras. Detector CenSurE. SBA, no dos frames. ZMNCC. Modelo de movimiento. EKF fusión IMU (correción angular)

7 Estado del Arte (V) Large Scale Visual Odometry for Rough Terrain (K.Konolige, M.Agrawal y J.Solà) HARRIS CenSurE Puntos detectados Sobreviven al RANSAC

8 Estado del Arte (VI) Two years of Visual Odometry on the Mars Exploration Rovers. (M.Maimone, Y.Cheng y L.Mathies) Pareja de cámaras. Detector Harris con rejilla. VO sist. sec. (CPU 20MHz). Odometría proyección Caract. RANSAC IMU corrección angular.

9 Estado del Arte (VII) Leaving Flatland: Toward real-time 3D navigation. (B. Morisset) Pareja de cámaras. Detector CenSurE RANSAC. SBA. Fusión IMU

10 Estado del Arte (VIII) Comparativa de técnicas Shi y TomasiZMNCCRANSACNO NO. Subsamp. Este Trabajo

11 Objetivos 1. Construir un sistema de odometría visual a bordo de un robot Seekur Jr. 2. Puesta en marcha y uso del conjunto en la navegación en espacios exteriores. 3. Evaluar la utilidad de la odometría visual en la localización de un robot móvil en espacios exteriores.

12 Recursos utilizados Arquitectura Hardware (I)

13 Recursos utilizados Arquitectura Hardware (II) ROBOT GPS BLUETOOTH ORDENADOR CONVERSOR USB-SERIE CAMARA IZQ. CAMARA DER. USBRS-232 FIREWIRE BLUETOOTH

14 Recursos utilizados Software(I) Toolbox de calibración (MATLAB) Librerías: A medida: Matching y clase para gestión de caract. OpenCV, libdc1394. Software específico para: Calibración de cámaras ( C ) Captura de datos ( C ) Rectificación de videos ( C ) Seguimiento de características ( C ) RANSAC y Estimación de movimiento (MATLAB)

15 Recursos utilizados Software(II) Software servidor: Player/Stage Trabaja de interfaz entre la aplicación y la información de odometría del robot y la localización de GPS. Drivers: p2os (modificado) garminnmea Interfaces: position2d gps

16 Algoritmo Rectificación de imágenes Cámaras Unibrain Cámara Bumblebee Seguimiento características Extracción Coordenadas 3D RANSAC Estimación movimiento Filtrado RANSAC OK? Repetir estimación frame anterior Estimación de movimiento SI NO

17 Shi y Tomasi. Detección de características Búsqueda de puntos característicos en las imágenes → esquinas. Fáciles para hacer seguimiento. Matriz de estructura local λ 1 y λ 2 son los autovalores de C Si λ 1 q· λ max

18 Rectificación de imágenes Corrección distorsiones radial y tangencial. Simplifica el matching estéreo. y l = y r Simplifica la extracción de la información 3D de las características.

19 Características en seguimiento que finalizaron en t = K-1 PASAN MATCHING TEMPORAL PASAN 2o MATCHING ESTÉREO NUEVAS CARACTERÍSTICAS PASAN 2o MATCHING ESTÉREO PASAN 1er MATCHING ESTÉREO Seguimiento características (I)

20 Seguimiento características (II) Frame izquierdo Frame derecho Extracción Shi y Tomasi Matching estéreo Características nuevas Z -1 Matching temporal Z -1 Matching estéreo + Características actualizadas Características a extraer 3D Fi -1 Fi Fd Fi Fd

21 Submuestreo Realizar la toma de muestras a la máxima velocidad de captura. Estudiar la frecuencia de captura óptima. Facilita el seguimiento de características. Frame Características 1 2 3 4 5 Subsampling x2 12345670 2460 FrameNº Caract.Nº Caract. (x2) 12- 242 35- 455 53- 622 71-

22 ZMNCC Correlación normalizada de media 0 Usada tanto en matching temporal como estéreo. P(x,y) → Inten. puntos de la ventana de puntos de la característica en la imagen origen. I(x,y) → Inten. puntos en la imagen objetivo. P' e I' → Las anteriores con valor medio cero. correlation = AB / cvSqrt(A2 * B2)

23 ZMNCC (II) P(x,y)I(x,y) 2K+1 2dw+1 2K+1 2dh+1

24 Matching Estéreo Búsqueda de una característica de la imagen izquierda en la imagen derecha. Parámetros: Tamaño del bloque de emparejamiento (9x9, K=4) Tamaño de ventana de búsqueda (45x1, dw=45, dh=0) Umbral de admisión de emparejamiento, Q = 0.4

25 Matching Temporal Búsqueda de una característica de la imagen izquierda del frame anterior en el actual. Parámetros: Tamaño del bloque de emparejamiento (9x9, TK=4) Tamaño de ventana de búsqueda (30x30, Tdw=15,Tdh=15) Umbral de admisión de emparejamiento, TQ = 0.4 t=k-1 t=k

26 ALGORITMO DE RANSAC (I) : Selección de 'inliers' Se toman 3 puntos al azar, s i Diferencia de Distancias < UMBRAL1 Calcular distancias entre ellos en ambos instantes Número de iteraciones > UMBRAL2 NO SI FALLA RANSAC12 Inicio

27 RANSAC (II) 1 Calcular movimiento W i =f( s i ) (3 puntos) Reproyectar todos los puntos con W i Eliminar puntos con error de reproyección superior a UMBRAL3 Total puntos >UMBRAL4 NO SI Número iteraciones >UMBRAL5 SI 2 NO Tomar conjunto con mayor número de inliers Calcular la estimación De movimiento

28 Estimación de movimiento (I)

29 Estimación de movimiento (II) j = punto conocidas sus coordenadas 3D en dos instantes 0 y 1 Vector de movimiento: Con N puntos Con: 3·N ecuaciones

30 Estimación de movimiento (III) Objetivo: Minimizar → Sistema no lineal → Mínimos cuadrados no lineales → Descenso por el gradiente → Búsqueda de un mínimo local a partir de una aproximación razonable → Linealización alrededor del punto de estudio

31 Estimación de movimiento (IV)

32 Estimación de movimiento (V)

33 Estimación de movimiento (VI)

34 Estimación de movimiento (VII) El sistema obtenido es lineal: Resolviendo por mínimos cuadrados: Si la pseudoinversa es: Entonces: Se puede ir mejorando de forma recursiva w: El Error de estimación como:

35 Estimación de movimiento (VIII) Se parte de una estimación razonable w 0 Se mejora la estimación de forma iterativa. Hasta obtener un error máximo de estimación Hasta un Nº max iteraciones

36 Resultados (I) Tipos de superficies: Asfalto Cesped Tipo de cámara: Bumblebee Dos cámaras Unibrain Fire-i Videos de seguimiento de características. Video de representación de trayectoria. Video de proyección 3D de características.

37 Resultados (II)

38 Bumblebee 72 – video 1 Cámara Bumblebee. Submuestreo x20. Bucle cerrado de unos 30 metros sobre asfalto. Horas centrales del día en zona de sombra. Robot: 29.5m, VO: 29.13m, : 36.5 cm, 1.24%

39 Bumblebee 72 – video 1. (1m 26seg)

40 Bumblebee 75 – video 2 Cámara Bumblebee. Submestreo x5. Bucle cerrado de unos 43 metros sobre cesped. Al atardecer en zona de sol. Robot: 43.31m, VO: 38.96m, : 4.35 m, 10%

41 Bumblebee 75 – video 2. (1m 50seg)

42 out11 – video 3 Cámaras Unibrain. Submestreo x9. Bucle cerrado de unos 130 metros sobre asfalto. Primeras horas de la mañana. Robot: 131.05m, VO: 138.4m, : 7.35 m, 5.6%

43 out11 – video 3. (3m 59seg)

44 out12 – video 4 Cámaras Unibrain. Submestreo x8. Bucle cerrado de unos 60 metros sobre cesped. Primeras horas de la mañana. Robot: 61.58 m, VO: 57.6 m, : 3.98 m, 6.47%

45 out12 – video 4. (3m 7seg)

46 Conclusiones (I) Estimación de la longitud de la trayectoria Llegando al 1% Mayor error en los giros. Periodo de muestreo Gran influencia. Estudiado a diferentes muestreos. Orientación de la cámara Influye en la variedad de puntos en seguimiento Óptica de las cámaras Como el caso anterior Disminución del número de características

47 Conclusiones (II) Se puede considerar una solución alternativa a la odometría clásica. Se deben resolver problemas graves derivados de la iluminación en exteriores. Se debe estudiar la fusión con otros recursos como unidades IMU y GPS.

48 Trabajos futuros Periodo de muestreo adaptativo Periodo adaptable a la velocidad del robot Incertidumbre de las características Incorporar la incertidumbre asociada a los puntos Implementación a bajo nivel Posibilidad de implementar los algoritmos en FPGA wide dynamic range sensor Solucionar los graves problemas de adaptación a los grandes contrastes de luz en exteriores Plataforma de desarrollo de odometría visual Sensores CMOS acoplados a FPGA.

49 Agradecimientos A los compañeros de los laboratorios que siempre me han dado su apoyo, y en especial a Pablo F. Alcantarilla e Ignacio Parra. A los profesores que me han guiado hasta aquí, especialmente a mi Director Rafael Barea. A la Consejería de Educación de la Comunidad de Madrid, y en especial al Servicio de Inspección Educativa de la DAT SUR.

50 NO OLVIDAR Justificante de examen para el trabajo. Solicitar Certificado de notas.