Repositorio Institucional de Documentos

Localizaci´ on por visi´ on omnidireccional para asistencia personal Daniel Guti´ errez G´ omez Directores: Alejandro Rituerto Sin y Jos´ e Jes´ us Guerrero Campo Ingenier´ ıa Industrial Automatizaci´ on Industrial y Rob´ otica Departamento de Inform´ atica e Ingenier´ ıa de Sistemas Centro Polit´ ecnico Superior Universidad de Zaragoza Agosto de 2011 2 Localizaci´ on por visi´ on omnidireccional para asistencia personal RESUMEN El problema de Localizaci´ on y Reconstrucci´ on 3D (SLAM1) plantea la posiblidad de que un m´ ovil sea capaz de crear un mapa de un entorno desconocido y localizarse en este mapa tan solo con mediciones de sus sensores. La mayor´ ıa de implementaciones de SLAM se basan en el filtro Kalman extendido (EKF), que permite realizar predicciones del estado de un sistema a partir ´ unicamente de mediciones del entorno. Se pueden utilizar varios tipos de sensores para extraer la informaci´ on del entorno (radar, laser, sonar, vision...). Actualmente los sensores m´ as utilizados en SLAM son los de visi´ on (Visual SLAM), dentro de los cuales, las c´ amaras omnidireccionales tienen un ´ angulo de visi´ on mucho m´ as amplio que las c´ amaras convencionales. A pesar de la gran ventaja que ofrece un mayor ´ angulo de visi´ on en problemas como el de SLAM , existen pocas aplicaciones de Visual SLAM que utilicen c´ amaras omnidireccionales. El objetivo de este proyecto es el desarrollo de una aplicaci´ on SLAM para una c´ amara omnidireccional a partir de una aplicaci´ on EKF-SLAM Monocular en tiempo real, programada en C++ y dise˜ nada para su uso con c´ amaras convencionales. Para ello, se realizar´ an modificaciones sobre dos aspectos b´ asicos: el modelo de proyecci´ on y el descriptor de puntos caracter´ ısticos. El modelo de proyecci´ on debe ser adaptado a uno apropiado para sistemas omnidireccionales. Se toma el Modelo de la Esfera, un modelo no lineal que permite calcular la proyecci´ on de puntos del espacio en puntos de la imagen omnidireccional, y de puntos en la imagen en rayos tridimensionales en los que se encuentra el punto. El descriptor de puntos caracter´ ısticos permite identificar los puntos en una imagen. Las im´ agenes omnidireccionales conllevan un modelo de proyecci´ on m´ as complejo, as´ ı como una importante deformaci´ on y una escala variable en la imagen debido al cambio de resoluci´ on a lo largo de la direcci´ on radial. Por lo tanto, se implementa un nuevo descriptor de puntos para c´ amaras omnidireccionales que los haga invariantes a cambios de escala y rotaci´ on en la imagen. Finalmente se han llevado a cabo varios experimentos divididos en dos fases. En la primera fase se han utilizado datos facilitados por Rawseeds para hacer una comparaci´ on entre el nuevo descriptor implementado y el descriptor normal. Los resultados de estos experimentos muestran que el nuevo descriptor tiene un mejor rendimiento en el emparejamiento, que se traduce en una mejora de rendimiento en el SLAM. En la segunda fase se ha experimentado con secuencias tomadas en el Campus R´ ıo Ebro con un casco-c´ amara del Grupo de Rob´ otica, Visi´ on y Tiempo Real, comparando los resultados obtenidos a partir de la aplicaci´ on SLAM utilizando el nuevo descriptor con los obtenidos con un dispositivo GPS tambi´ en proporcionado por dicho grupo. Los resultados muestran una gran precisi´ on en la estimaci´ on de la trayectoria en un recorrido corto, mientras que en un recorrido largo, la precisi´ on disminuye considerablemente. Adem´ as de la deriva del error propia del algoritmo EKFSLAM por tratarse de un algoritmo incremental, se han establecido dos posibles causas de esta disminuci´ on de la precisi´ on, ambas consecuencia de la inobservabilidad de la escala de un SLAM monocular: por un lado, la deriva de escala a lo largo del recorrido debido a la gran longitud de este, y por otro lado, la existencia de zonas del entorno con diferentes escalas alrededor de una misma localizaci´ on y que afectar´ ıa en especial a c´ amaras omnidireccionales por su gran ´ angulo de visi´ on. 1del ingl´ es Simultaneous Localization And Mapping 3 4 ´ Indice general 1. Introducci´ on 7 2. El Modelo de la Esfera 11 3. Localizaci´ on y reconstrucci´ on 3D 15 3.1. Adquisici´ on de datos y gesti´ on de mapa en un SLAM Monocular . . . 18 4. Adaptaci´ on de SLAM para c´ amaras omnidireccionales 21 4.1. Modelo de la Esfera para EKF-SLAM . . . . . . . . . . . . . . . . . 21 4.2. Filtrado de predicciones del modelo de proyecci´ on .......... 22 4.3. Desarrollo de un nuevo parche invariante a escala y rotaci´ on para im´ agenes omnidireccionales . . . . . . . . . . . . . . . . . . . . . . 24 4.3.1. Invarianza a rotaci´ on...................... 24 4.3.2. Invarianza a escala . . . . . . . . . . . . . . . . . . . . . . . 25 4.3.3. Obtenci´ on del nuevo parche para emparejamiento . . . . . . . 28 5. Realizaci´ on de experimentos 31 5.1. Experimento 1: Evaluaci´ on del proceso de emparejamiento utilizando elnuevoparche ............................. 31 5.1.1. Test 1: Transformaci´ on de rotaci´ on con rotaci´ on 180ode la c´ amara ............................. 32 5.1.2. Test 2: Transformaci´ on de rotaci´ on con traslaci´ on de la c´ amara 34 5.1.3. Test 3: Factor de escala . . . . . . . . . . . . . . . . . . . . . 34 5.2. Experimento 2: Comprobaci´ on de la ejecuci´ on de un SLAM con el nuevoparche .............................. 38 5.3. Experimento 3: SLAM con casco-c´ amara ............... 39 5.3.1. Secuencia corta . . . . . . . . . . . . . . . . . . . . . . . . . 41 5.3.2. Secuencia larga . . . . . . . . . . . . . . . . . . . . . . . . . 41 6. Conclusiones 49 A. Parametrizaci´ on de profundidad inversa 53 A.1. Inicializaci´ on de puntos caracter´ ısticos................. 54 A.2. Cambio de IDP a parametrizaci´ onXYZ ................ 55 B. La base de datos de Rawseeds 57 C. Calibraci´ on de las c´ amaras 59 5 ´ INDICE GENERAL D. Resultados completos de los tests del experimento 1 61 E. Adapting a Real-Time Monocular Visual SLAM from Conventional to Omnidirectional Cameras 73 E.1. Introduction............................... 73 E.2. The Spherical Camera Model . . . . . . . . . . . . . . . . . . . . . . 75 E.3. Simultaneous Localisation And Mapping . . . . . . . . . . . . . . . 76 E.3.1. The Spherical Camera Model for the EKF . . . . . . . . . . . 76 E.3.2. Data Association and Map Management . . . . . . . . . . . . 77 E.4. New patch formulation . . . . . . . . . . . . . . . . . . . . . . . . . 77 E.4.1. Rotation invariance . . . . . . . . . . . . . . . . . . . . . . . 77 E.4.2. Scale invariance . . . . . . . . . . . . . . . . . . . . . . . . 78 E.4.3. Computation of patch transformation . . . . . . . . . . . . . 79 E.5. Experiments............................... 80 E.5.1. Experiment1.......................... 81 E.5.2. Experiment2.......................... 84 E.6. Conclusion ............................... 85 6 Cap´ ıtulo 1 Introducci´ on El problema de localizaci´ on y mapeado 3D simult´ aneos recibe el nombre de SLAM (Simultaneous Localization And Mapping) y ha sido ampliamente tratado en los ´ ultimos a˜ nos. La resoluci´ on del SLAM permite construir un mapa de un entorno desconocido a partir de mediciones con un sensor sobre ese entorno y al mismo tiempo localizar dicho sensor en el mapa. El SLAM se formula normalmente de manera probabil´ ıstica, es decir, la estimaci´ on de la posici´ on del sensor y el mapa son calculadas como una distribuci´ on de probabilidad. Por ello, la mayor´ ıa de implementaciones se basan en una variante del filtro Kalman, conocido como filtro Kalman extendido (EKF) que linealiza las ecuaciones no lineales presentes en el modelo de proyecci´ on mediante aproximaci´ on de Taylor. Se pueden utilizar varios tipos de sensores para extraer la informaci´ on del entorno necesaria (radar, laser, sonar, vision...). En el pasado uno de los sensores m´ as utilizados en aplicaciones SLAM fue el tel´ emetro l´ aser [21], pero debido a su alto coste y sus limitaciones, las investigaciones recientes se han movido hacia el uso de sensores de visi´ on, m´ as baratos y capaces de obtener mayor informac´ on del entorno, lo que se conoce como Visual SLAM. En las aplicaciones de Visual SLAM, las proyecciones en la imagen de puntos caracter´ ısticos, tambi´ en llamados caracter´ ısticas locales, se toman como medidas. Para extraer y guardar estos puntos se usan un extractor y un descriptor de caracter´ ısticas respectivamente. El extractor de caracter´ ısticas procesa la imagen y detecta los puntos claves en ella, que son aquellos en los que tiene lugar un cambio brusco de iluminaci´ on en las direcciones horizontal y vertical (esquinas). El procesamiento de la imagen es un paso que consume bastante tiempo de computaci´ on, lo cual es cr´ ıtico en una aplicaci´ on en tiempo real. Centr´ andose en este inconveniente Rosten et. al. [24] desarrollaron el algoritmo de extracci´ on de features FAST1y compararon su extractor FAST con otros extractores ampliamente utilizados, mostrando que el extractor FAST ofrece un mejor rendimiento tanto en coste computacional como en repetibilidad viendo la misma escena desde diferentes posiciones. El descriptor proporciona un identificador a cada punto extra´ ıdo de forma que pueda ser reconocido en futuras medidas. El descriptor m´ as b´ asico es un parche de cierto tama˜ no centrado en el punto extra´ ıdo, aunque existen descriptores m´ as sofisticados como SIFT [18], SURF [5], LBP [15], etc. El presente proyecto est´ a enfocado hacia Visual SLAM con sistemas de visi´ on 1del ingl´ es, Features Accelerated Segment Test 7 omnidireccionales. Dichos sistemas han experimentado un gran desarrollo en los ´ ultimos a˜ nos, impulsado por la gran influencia que tiene el campo de visi´ on en aplicaciones como localizaci´ on, navegaci´ on o vigilancia. En un SLAM, gracias al campo de visi´ on de 360ode las c´ amaras omnidireccionales, los puntos caracter´ ısticos detectados permanecen m´ as tiempo en la imagen que en el caso de las c´ amaras convencionales. Este efecto se puede apreciar de forma clara bajo grandes rotaciones de la c´ amara donde una c´ amara convencional pierde gran parte de sus puntos debido a su estrecho campo de visi´ on, mientras que la c´ amara omnidireccional es capaz de seguir detectando dichos puntos. El aumento de la vida de los puntos caracter´ ısticos en la imagen se traduce en una mejor estimaci´ on de la posici´ on de los puntos en el mapa 3D, una menor necesidad de inicializar nuevos puntos y un incremento de la robustez del SLAM. Dentro de las c´ amaras omnidireccionales existen diversos tipos seg´ un sus caracter´ ısticas constructivas: C´ amara rotatoria. Se trata de una c´ amara convencional con un sistema mec´ anico, el cual le permite moverse en una trayectoria circular y capturar im´ agenes de todo el entorno. Conjuntos de c´ amaras convencionales. Cada una de las c´ amaras enfoca en una direcci´ on en una configuraci´ on circular o esf´ erica. Las im´ agenes recogidas se pueden unir para formar una sola imagen panor´ amica. Sistemas di´ optricos. Estos sistemas est´ an compuestos por c´ amaras convencionales y lentes de gran angular como las lentes de ojo de pez. Sistemas catadi´ optricos. Son la combinaci´ on de c´ amaras convencionales y espejos. En aplicaciones de rob´ otica los sistemas omnidireccionales m´ as usados son los catadi´ optricos. Los sistemas catadioptricos han sido estudiados por Baker y Nayar [3] que probaron que los que incorporan espejos el´ ıpticos, hiperb´ olicos y parab´ olicos son los ´ unicos que dan lugar a sistemas con un ´ unico centro de proyecci´ on. De estos sistemas, los m´ as utilizados son el hiper-catadi´ optrico, que est´ a compuesto por un espejo hiperb´ olico y una c´ amara perspectiva y el para-catadi´ optrico, que utiliza un espejo parab´ olico y una c´ amara ortogr´ afica. El presente proyecto se centrar´ a en el uso de sistemas de visi´ on hipercatadi´ optricos. Dichos sistemas ya han sido utilizados en aplicaciones como vigilancia [28], navegaci´ on [6], localizaci´ on [13], tele-presencia [13] y reconstrucci´ on 3D [13]. Las im´ agenes omnidireccionales obtenidas con c´ amaras catadi´ optricas conllevan un modelo de proyecci´ on m´ as complejo, as´ ı como una importante deformaci´ on y una escala variable en la imagen debido al cambio de resoluci´ on a lo largo de la direcci´ on radial dependiente de la forma del espejo. Por lo tanto, es necesario dise˜ nar un nuevo descriptor para estas c´ amaras. En este sentido Svoboda y Padjla [29] proponen el uso de parches con tama˜ no y forma variable (ventanas activas). Sus experimentos muestran que las ventanas activas proporcionan mejores resultados de emparejamiento que ventanas cuadradas. Ieng et. al. [7] proponen la computaci´ on de parches con diferentes aperturas angulares para hacer frente a los problemas de emparejamiento derivados de la resoluci´ on variable de la c´ amara. Scaramuzza et. al. [26] aprovechan que las l´ ıneas verticales de la escena se proyectan en l´ ıneas rectas orientadas radialmente en la imagen omnidireccional para proponer un m´ etodo para extraer y emparejar l´ ıneas verticales 8 CAP´ ITULO 1. INTRODUCCI ´ ON con descriptores invariantes a rotaci´ on y aplican este m´ etodo a un EKF-SLAM. En [1], Andreasson et. al. proponen un descriptor SIFT modificado que no proporciona invarianza a escala y obtienen invarianza a rotaci´ on rotando cada parche extra´ ıdo a la misma orientaci´ on global. Lu and Zheng [19] combinan el parche invariante a rotaci´ on de Andreasson con un extractor FAST y un descriptor CS-LBP y lo comparan con el algoritmo SIFT para extracci´ on y emparejamiento. Adem´ as, para poder extraer informaci´ on sobre el entorno de la imagen, es necesario un modelo de proyecci´ on m´ as complejo. Para sistemas catadi´ optricos, el m´ as utilizado es el llamado Modelo de Proyecci´ on de la Esfera propuesto por Geyer y Daniilidis [12] y ampliado por Barreto y Araujo [4]. El objetivo de este proyecto es implementar un SLAM monocular en tiempo real con visi´ on omnidireccional y aplicarlo a un casco con una c´ amara omnidireccional acoplada proporcionado por el Departamento de Inform´ atica e Ingenier´ ıa de Sistemas de la Universidad de Zaragoza . Para ello, en primer lugar, se va a modificar el c´ odigo de una aplicaci´ on desarrollada para c´ amaras perspectivas adaptando el modelo de proyecci´ on directo e inverso, los modelos diferenciales y el descriptor de caracter´ ısticas necesario con im´ agenes omnidireccionales que proporcione invarianza a rotaci´ on y escala. En segundo lugar se evaluar´ a la nueva aplicaci´ on mediante la experimentaci´ on con secuencias tomadas por una c´ amara catadi´ optrica instalada sobre un robot obtenidas en el proyecto Rawseeds. En tercer lugar se realizar´ a un experimento con el casco-c´ amara proporcionado por la Universidad de Zaragoza. Por ´ ultimo se pretende presentar los resultados en una conferencia internacional relevante con revisi´ on por pares. En particular se ha enviado una contribuci´ on al 11th OMNIVIS (Anexo E) y en este momento estamos a la espera de la respuesta de los revisores. 9 Figura 3.1: Aplicaci´ on de SLAM. En l´ ınea continua se representa la trayectoria y las posiciones reales del robot, y en l´ ınea discontinua las estimaciones del SLAM. Las mediciones del entorno se representan como estrellas, amarillas con l´ ınea discontinua verde la primera vez que son observadas y blancas con l´ ınea roja discontinua cuando se reconocen desde otra posici´ on. Para poder llevar a cabo la estimaci´ on de un nuevo estado en el instante kes necesario definir previamente un modelo de movimiento y un modelo de observaci´ on. El modelo de movimiento se describe mediante la siguiente funci´ on: xv,k =fv(xv,k−1,uk) + wk(3.2) donde fv(·)es la funci´ on que modela el movimiento del m´ ovil, xv,k−1es el estado anterior del m´ ovil, ukes la entrada de control independiente de las observaciones y del mapa y wkson perturbaciones aditivas con distribuci´ on gausiana, no correlacionadas, de media cero y con covarianza Qk. En este proyecto se va a utilizar el modelo de velocidad constante y aceleraciones lineal y angular desconocidas. Dichas aceleraciones son tratadas como ruido gaussiano y, por lo tanto, englobadas dentro del t´ ermino wkde la ecuaci´ on. Por otro lado, el modelo de medida se describe de la siguiente forma: zk=h(xk) + vk(3.3) donde h(·)describe el modelo de proyecci´ on, xkes el estado total y vkson errores de observaci´ on aditivos, con distribuci´ on gausiana, no correlacionadas, de media cero y con covarianza Rk En cada iteraci´ on del EKF la estimaci´ on se realiza en dos pasos. El primer paso consiste en la predicci´ on del estado y de la covarianza de dicho estado en funci´ on de la informaci´ on del paso anterior. El segundo paso es el de la actualizaci´ on del estado con la informaci´ on obtenida de las medidas. 16 CAP´ ITULO 3. LOCALIZACI ´ ON Y RECONSTRUCCI ´ ON 3D Predicci´ on La predicci´ on del nuevo estado se realiza mediante una funci´ on de transici´ on de estado. La transici´ on de estado se toma como un proceso de Markov en el que el estado xkdepende ´ unicamente del estado anterior xk−1y de la entrada de control uky es independiente de las observaciones y del mapa. De este modo las ecuaciones para la predicci´ on del nuevo estado y la covarianza quedan: Predicci´ on del estado xk|k−1=f(xk−1|k−1,uk)(3.4) Predicci´ on de la covarianza Pk|k−1=Fk−1Pk−1|k−1Fk−1T+Qk(3.5) donde fes la funci´ on de cambio de estado, que actualiza el estado del m´ ovil seg´ un el modelo de movimiento (xv,k|k−1=fv(xv,k−1|k−1,uk)) y no produce ning´ un cambio en el estado de los puntos del mapa (yk|k−1=yk−1|k−1), ya que se asume que son puntos fijos. xk|k−1yPk|k−1son el estado y la covarianza de ese estado estimados en el momento ka partir de las mediciones hechas hasta el momento anterior k−1.Fk−1es el jacobiano de la funci´ on de cambio de estado. Fk−1=∂f(x,u) ∂x|xk−1|k−1 yQkes la matriz de covarianzas del ruido del sistema. Actualizaci´ on Tras la primera predicci´ on del estado del sistema mediante el modelo de movimiento se realiza una actualizaci´ on de esta predicci´ on basada en las medidas de los sensores en el momento actual k. Para ello, en primer lugar se calcula la innovaci´ on de la medida νkque representa la diferencia entre la medida real zk realizada por los sensores y la predicci´ on de la medida h(xk|k−1)dada por el modelo de observaci´ on. Esta innovaci´ on tiene asociada una covarianza Sken la que se tiene en cuenta los posibles errores de medida. Innovaci´ on de la medida νk=zk−h(xk|k−1)(3.6) Covarianza de la innovaci´ on Sk=HkPk|k−1HkT+Rk(3.7) donde Hkes el jacobiano de la funci´ on de medida. Hk=∂h(x) ∂x|xk|k−1 A continuaci´ on se calcula la ganancia de Kalman Wk, que se˜ nala cuanto se puede confiar en las mediciones a la hora de corregir la predicci´ on a priori. En funci´ on de este valor se le da m´ as o menos peso a la innovaci´ on de la medida al calcular el estado y su covarianza finales en el instante k. 17 3.1. ADQUISICI ´ ON DE DATOS Y GESTI ´ ON DE MAPA EN UN SLAM MONOCULAR Ganancia de Kalman Wk=Pk|k−1HkTSk−1(3.8) Actualizaci´ on del estado xk|k=xk|k−1+Wkνk(3.9) Actualizaci´ on de la covarianza Pk|k=Pk|k−1−WkSkWkT(3.10) 3.1. Adquisici´ on de datos y gesti´ on de mapa en un SLAM Monocular Previamente se ha definido de forma gen´ erica el algoritmo de una soluci´ on para SLAM basada en EKF para cualquier tipo de sensor. Sin embargo, el presente proyecto se basa en un SLAM con un solo sensor de visi´ on (SLAM Monocular), por lo que a continuaci´ on se har´ a hincapi´ e en sus particularidades. El uso de sensores de visi´ on para SLAM viene motivado sobre todo por su bajo coste y por su capacidad de proporcionar gran cantidad de informaci´ on del entorno. Sin embargo, presentan el inconveniente de que es necesario un mayor procesamiento de los datos, en este caso im´ agenes. Las medidas en SLAM con sensores de visi´ on se corresponden con las proyecciones en la imagen de puntos relevantes del entorno (caracter´ ısticas locales). Para detectar estos puntos se utilizan extractores de caracter´ ısticas que consisten en algoritmos que procesan la imagen buscando los puntos en los que tiene lugar un cambio brusco de iluminaci´ on en las direcciones horizontal y vertical. El procesamiento de la imagen es un paso que consume bastante tiempo de computaci´ on, lo cual es cr´ ıtico en una aplicaci´ on en tiempo real. Por ello, en la aplicaci´ on en la que se basa el presente proyecto se utiliza el extractor FAST3. Dicho extractor fue desarrollado por Rosten et. al. ( [24])y se ha comparado con otros extractores ampliamente utilizados. Los resultados muestran que el extractor FAST ofrece un mejor rendimiento tanto en coste computacional como en repetibilidad viendo la misma escena desde diferentes posiciones. Por otro lado, para poder hacer un seguimiento de los puntos extraidos, es necesario proporcionarles un identificador o descriptor para que puedan ser reconocidos en las siguientes im´ agenes. En este proyecto usaremos un identificador sencillo consistente en un parche cuadrado centrado en el punto caracter´ ısitico, frente a otros descriptores m´ as sofisticados como el SIFT. La raz´ on es que, pese a que el descriptor SIFT es invariante a cambios de escala y orientaci´ on en imagenes convencionales, viene ligado a la utilizaci´ on de un algoritmo de extracci´ on de puntos (DoG) que es mucho m´ as complejo computacionalmente que el extractor FAST. Adem´ as, como se propone en este proyecto de fin de carrera es posible hacer frente a estos cambios de escala y de orientaci´ on de los puntos caracter´ ısticos en imagenes omnidireccionales, mediante la aplicaci´ on de transformaciones afines. Una vez presentado el algoritmo EKF-SLAM b´ asico y definido el proceso de medici´ on a partir de una imagen, se explicar´ a m´ as detalladamente el proceso de asociaci´ on de datos y gesti´ on de puntos caracter´ ısiticos en el SLAM monocular, que se resume de forma esquem´ atica en el algoritmo 1 al final del cap´ ıtulo. 3Features Accelerated Segment Test 18 CAP´ ITULO 3. LOCALIZACI ´ ON Y RECONSTRUCCI ´ ON 3D Partiendo del estado anterior, definido por el vector de estados y la matriz de covarianzas, se calcula en primer lugar la predicci´ on del nuevo estado mediante las ecuaciones 3.4 y 3.5. A continuaci´ on se lleva a cabo la predicci´ on de las medidas de los puntos del mapa mediante el modelo de medida (Ec. 3.3), que en el caso de sensores de visi´ on es un modelo de proyecci´ on, y se calcula su correspondiente covarianza (Ec. 3.7 ). La covarianza de la innovaci´ on define una elipse de incertidumbre para cada punto caracter´ ıstico dentro de la cual deber´ ıa encontrarse la proyecci´ on real del punto caracter´ ıstico en la nueva imagen. Para obtener una medici´ on de dicho punto se realiza una b´ usqueda activa dentro de la regi´ on de incertidumbre, en la que se selecciona como medici´ on (o emparejamiento) el p´ ıxel con mejor correlaci´ on con el parche del punto caracter´ ıstico. Para detectar y descartar mediciones err´ oneas (es decir, aquellas que no se corresponden con el punto buscado) se realiza un filtrado RANSAC de un punto [11]. Finalmente, teniendo en cuenta s´ olo los emparejamientos correctos, se calcula la innovaci´ on y la ganancia Kalman y se ejecuta el paso de actualizaci´ on para calcular el nuevo estado. Para la inicializaci´ on de nuevos puntos se procesa la imagen con el extractor FAST y se inicializan aquellas que tienen una mayor puntuaci´ on y que est´ en lo suficientemente alejados del resto de puntos del mapa. En el caso de sensores de visi´ on existe un problema en la inicializaci´ on de puntos debido a que la profundidad de un punto en el espacio tridimensional no es observable en la imagen, por lo que tiene que ser inicializada a un valor arbitrario con una incertidumbre muy elevada. Por esta raz´ on, el estado de los puntos reci´ en inicializados se expresa mediante parametrizaci´ on de profundidad inversa (IDP4) que se explica detalladamente en el Anexo A. Dicha representaci´ on consta de 6par´ ametros: las coordenadas de la c´ amara (xi, yi, zi)en el momento de la inicializaci´ on del punto, el ´ angulo de elevaci´ on θi, el ´ angulo de azimuth φiy el inverso de la profundidad ρi. Los puntos son inicializados con una estimaci´ on arbitraria de ρ0ipara la profundidad inversa. En sucesivas observaciones del punto, la estimaci´ on de la profundidad se va afinando gradualmente hasta que, si su incertidumbre decrece por debajo de cierto umbral, el estado del punto pasa a representarse por sus coordenadas cartesianas en la referencia absoluta. N´ otese que la inobservabilidad de la profundidad en la imagen, tiene el problema asociado de la inobservabilidad de la escala en el SLAM Monocular, dependiendo ´ esta del valor arbitrario que se le de a la estimaci´ on inicial de la profundidad. Esto implica que una implementaci´ on de SLAM monocular requiere de un m´ etodo alternativo para determinar la escala del mapa. En lo que respecta a la ejecuci´ on en tiempo real, el coste computacional de la actualizaci´ on del estado crece de forma cuadr´ atica con el n´ umero de puntos en el mapa, superando el l´ ımite impuesto por la velocidad de adquisici´ on de im´ agenes cuando el mapa es demasiado grande. Por lo tanto aquellos puntos que no son emparejados o son emparejados err´ oneamente tras varios intentos se marginalizan fuera del vector de estados, guard´ andolos en otro vector independiente del SLAM en el primer caso y borr´ andolos en el segundo. 4del ingl´ es, Inverse Depth Parametrisation 19 3.1. ADQUISICI ´ ON DE DATOS Y GESTI ´ ON DE MAPA EN UN SLAM MONOCULAR Algorithm 1 Algoritmo EKF-SLAM para sistemas de visi´ on Require: xk−1|k−1,Pk−1|k−1,uk,im actual,parches puntos Ensure: xk|k,Pk|k,parches puntos nuevos (xk|k−1,Pk|k−1) = Prediccion Movimiento (xk−1|k−1,Pk−1|k−1,uk,Qk) if Puntos en el vector de estado 6= 0 then (hk,Sk) = Prediccion Medida (xk|k−1,Pk|k−1,Rk) zk=Busqueda y Emparejamiento (hk,Sk, im actual, parches puntos) (xk|k,Pk|k) = Filtrado y Actualizacion (xk|k−1,Pk|k−1,zk,hk,Sk,Rk) end if (zinit, parches puntos nuevos) = Extraer y Seleccionar Puntos (im actual) (yinit,Py) = Inicializar Puntos (zinit,Rk) xk|k= [xk|kyinit] Pk|k= [Pk|kPy] 20 Cap´ ıtulo 4 Adaptaci´ on de SLAM para c´ amaras omnidireccionales En el cap´ ıtulo anterior se ha presentado el algoritmo para SLAM con EKF y se ha profundizado en las particularidades de una aplicaci´ on de visual SLAM monocular. Dado que partimos de una aplicaci´ on ya desarrollada para c´ amaras convencionales ( [8]), en este cap´ ıtulo nos centraremos en introducir y desarrollar las modificaciones necesarias sobre esta aplicaci´ on para que sea posible su uso con c´ amaras catadi´ optricas. Dichas modificaciones se eval´ uan mediante los experimentos descritos en el siguiente cap´ ıtulo. El uso de sistemas de visi´ on omnidireccionales en lugar de una c´ amara convencional conlleva, adem´ as de la modificaci´ on del modelo de proyecci´ on por el Modelo de la Esfera, otra serie de cambios en los algoritmos de gesti´ on de puntos caracter´ ısticos debido, por un lado, al propio modelo de proyecci´ on, y por otro, a la gran distorsi´ on, deformaci´ on y escala variable que presenta una imagen omnidireccional frente a una imagen normal. 4.1. Modelo de la Esfera para EKF-SLAM En un SLAM para sistemas catadi´ optricos el modelo de medida viene dado por el Modelo de la Esfera, el cual incluye funciones no lineales. Por lo tanto, es necesaria su linealizaci´ on obteniendo una expresi´ on del Jacobiano que permite calcular la ganancia Kalman ( [23]). Usando la misma notaci´ on que en el Cap´ ıtulo 2, el Jacobiano correspondiente al Modelo de la Esfera se obtiene de la siguiente forma: JME =∂u ∂x=∂u ∂x00 |{z} Jfu ∂x00 ∂x0 |{z} HC ∂x0 ∂x |{z} J~ (4.1) con Jfu ="1 z00 0−x00 z002 01 z00 −y00 z002#(4.2) J~=   1 0 0 0 1 0 ξx √x2+y2+z2 ξy √x2+y2+z21 + ξz √x2+y2+z2   (4.3) 21 4.2. FILTRADO DE PREDICCIONES DEL MODELO DE PROYECCI ´ ON Tambi´ en es necesario obtener el Jacobiano de la proyecci´ on inversa, que permite calcular la propagaci´ on de incertidumbres desde la imagen hasta la escena y por lo tanto, hacer una primera estimaci´ on de la covarianza del estado de un punto reci´ en inicializado. Utilizando la notaci´ on del Cap´ ıtulo 2, el Jacobiano de la funci´ on de proyecci´ on inversa se puede calcular como el producto de los jacobianos correspondientes a cada paso de la funci´ on. J−1 ME =∂x ∂x00 =∂x ∂x0 ∂x0 ∂x00 =J~−1HC−1(4.4) siendo J~−1el jacobiano de la inversa de la funci´ on no lineal ~: J~−1=  1 0 0 0 1 0 ∂z ∂x0 ∂z ∂y0 ∂z ∂z0 =   1 0 0 0 1 0 −ξx0 χ−ξy0 χ1−ξ(z0−ξx02+y02+z02 ξz0+χ) χ   (4.5) donde χ=p(1 −ξ2)(x02+y02+z02) 4.2. Filtrado de predicciones del modelo de proyecci´ on Como su nombre indica, un modelo de proyecci´ on no deja de ser una idealizaci´ on de la realidad, por lo que muchas de las medidas predichas carecer´ an de sentido f´ ısico al no ser visibles en la pr´ actica por el sistema de visi´ on. Para descartar las predicciones f´ ısicamente incoherentes se realiza un filtrado en tres fases correspondiendo a las tres posibles causas de incoherencia en la predicci´ on. Puntos no visibles seg´ un el modelo de proyecci´ on te´ orico Son aquellos puntos para los que el rayo x0obtenido mediante la funci´ on ~ tiene componente z0≤0. Geom´ etricamente son los puntos dentro del cono definido por la ecuaci´ on x2+y2=1−ξ2 ξ2z2con z < 0tomando como origen de coordenadas el punto O(Fig. 4.1). Puntos fuera de la imagen Son los puntos que son proyectados en el plano imagen seg´ un el modelo de proyecci´ on, pero cuyas coordenadas quedan fuera de los l´ ımites de la imagen capturada. Puntos proyectados en zonas ciegas de la imagen Por razones constructivas, las c´ amaras catadi´ optricas presentan zonas en el centro y en la periferia de la imagen que permanecen constantemente inm´ oviles, al corresponder a la proyecci´ on de elementos solidarios a la c´ amara. Para filtrar las medidas predichas en estas zonas, se enmascaran los p´ ıxeles de la imagen que se encuentren a una distancia menor de un radio m´ ınimo o mayor de un radio m´ aximo del punto principal. Puede darse el caso de que exista tambi´ en alg´ un elemento de sujeci´ on de la c´ amara como una barra vertical que atraviesa la imagen radialmente (Fig. 4.2) que tambi´ en ser´ ıa necesario enmascarar. 22 CAP´ ITULO 4. ADAPTACI ´ ON DE SLAM PARA C ´ AMARAS OMNIDIRECCIONALES Figura 4.1: Los puntos de la escena que est´ en contenidos dentro del cono de la figura no se pueden proyectar en el plano imagen. Figura 4.2: M´ ascara que se aplica a las c´ amaras omnidireccionales para no inicializar ni emparejar puntos que permancen inm´ oviles en la imagen por ser solidarios a la c´ amara. 23 4.3. DESARROLLO DE UN NUEVO PARCHE INVARIANTE A ESCALA Y ROTACI ´ ON PARA IM ´ AGENES OMNIDIRECCIONALES 4.3. Desarrollo de un nuevo parche invariante a escala y rotaci´ on para im´ agenes omnidireccionales Durante el proceso de medici´ on de puntos caracter´ ısticos, adem´ as de predecir su proyecci´ on, es deseable tambi´ en predecir como cambia la apariencia del parche en la imagen conforme la c´ amara se va desplazando para aumentar las posibilidades de conseguir emparejamientos exitosos. En la aplicaci´ on original esto se consigue guardando un parche de tama˜ no mayor adem´ as del parche usado como descriptor durante la inicializaci´ on. Antes de hacer el emparejamiento de puntos, se determina una transformaci´ on homogr´ afica [14] a partir de la variaci´ on de la posici´ on y orientaci´ on del robot entre el momento actual y el de la inicializaci´ on del punto que se va a emparejar. Para poder construir esta transformaci´ on es necesario asumir que todos los puntos del parche se encuentran en el mismo plano de la escena 3D y que este plano es paralelo al plano imagen, algo que no puede asumirse en im´ agenes catadi´ optricas omnidireccionales. En la aplicaci´ on modificada vamos a definir una nueva transformaci´ on para predecir la variaci´ on en la apariencia de los parches en una c´ amara catadi´ optrica. Con dicha transformaci´ on se pretende proporcionar cierta robustez respecto a los cambios de orientaci´ on y de escala del parche en la imagen. A diferencia de la transformaci´ on implementada para c´ amaras convencionales, la nueva transformaci´ on es m´ as sencilla y no proporciona una predicci´ on del cambio de perspectiva, pero presenta por un lado la ventaja de que no requiere realizar ninguna hip´ otesis sobre el parche en la escena 3D, y por otro lado, que, exceptuando la contribuci´ on de la profundidad en la escala del parche, las transformaciones se calculan a partir de informaci´ on presente solamente en la imagen, por lo que no se requiere informaci´ on sobre las variables de estado del SLAM. 4.3.1. Invarianza a rotaci´ on Para conseguir robustez respecto de la rotaci´ on de la c´ amara en torno al eje de revoluci´ on del espejo, en principio hemos adoptado la idea propuesta por Andreasson et al. [1], consistente en tomar mediante interpolaci´ on bilineal un parche orientado en la direcci´ on radial desde el punto principal como descriptor y rotarlo a una orientaci´ on fija (Fig. 4.3) Sin embargo, a la hora de hacer el emparejamiento, esta soluci´ on tiene el inconveniente de que conlleva un coste computacional elevado, ya que cada parche candidato dentro de la regi´ on de b´ usqueda debe ser rotado tambi´ en por interpolaci´ on bilineal. Por ello, se ha combinado esta idea con la implementaci´ on de la aplicaci´ on original. En vez de extraer un parche en la direcci´ on radial, se extrae un parche de mayor tama˜ no (p.ej. el doble de grande) que el descriptor en las direcciones horizontal y vertical durante la inicializaci´ on de un nuevo punto del mapa. Antes del proceso de emparejamiento, se le aplica a este parche una transformaci´ on de rotaci´ on R∆θa partir de la variaci´ on del ´ angulo polar del punto en la imagen desde su inicializaci´ on (∆θ=θ−θini) R∆θ=  cos(∆θ)−sin(∆θ) 0 sin(∆θ) cos(∆θ) 0 0 0 1 (4.6) y a continuaci´ on se extraer´ ıa el nuevo parche descriptor del centro del parche 24 CAP´ ITULO 4. ADAPTACI ´ ON DE SLAM PARA C ´ AMARAS OMNIDIRECCIONALES Figura 4.3: El parche de un punto caracter´ ıstico reci´ en inicializado, se rota a una orientaci´ on fija antes de ser guardado como descriptor para futuros emparejamientos. De esta forma el parche descriptor es invariante frente a rotaci´ on de la c´ amara. transformado. De esta manera la correlaci´ on de los puntos dentro de la regi´ on de b´ usqueda se determina siempre en las direcciones horizontal y vertical de la imagen y la interpolaci´ on bilineal s´ olo se realiza una vez durante la transformaci´ on del parche grande. 4.3.2. Invarianza a escala Para que el proceso de emparejamiento de un punto sea invariante a la escala, se va a formular el cambio de escala con el que un mismo punto se puede observar en distintas im´ agenes. Para considerar la resoluci´ on variable en la imagen catadi´ optrica en el c´ alculo del factor de escala, se ha obtenido una f´ ormula matem´ atica en funci´ on de los par´ ametros del espejo y la posici´ on en la imagen. Para ello, en primer lugar se ha definido un punto 3D en coordenadas homog´ eneas a una distancia o profundidad Dde la c´ amara con azimut φy elevaci´ on θ. Debido a la simetr´ ıa de revoluci´ on del espejo hiperb´ olico y por simplicidad, se toma un azimut de φ= 0 sin p´ erdida de generalidad. Por lo tanto, las coordenadas del punto 3D quedan X0= (Dcos θ, 0, D sin θ, 1)T. Proyectando este punto en el plano imagen seg´ un el Modelo de la Esfera obtenemos el punto p0= ( γcos θ ξ+sinθ ,0,1)T. Para expresar las coordenadas en la imagen respecto del sistema de referencia en el punto principal hemos tomado u0=v0= 0 en la matriz HC, lo que se mantendr´ a para el resto de la formulaci´ on. La norma del punto proyectado es la distancia desde el punto principal Rim Rim =kp0k=γcos θ ξ+sinθ (4.7) Ahora tomemos una esfera de radio rcentrada en el punto 3D X0que hemos proyectado previamente. Si asumimos que r << D, los puntos en la superficie de la 25 5.1. EXPERIMENTO 1: EVALUACI ´ ON DEL PROCESO DE EMPAREJAMIENTO UTILIZANDO EL NUEVO PARCHE al que no se le aplican transformaciones. En el primer y segundo test se ha evaluado la invarianza a rotaci´ on introducida por la nueva transformaci´ on bajo una rotaci´ on de la c´ amara de unos 180oy una traslaci´ on de la c´ amara con importantes cambios ne las im´ agenes respectivamente. En el tercer test se ha evaluado el rendimiento de un parche al que se le aplica el factor de escala. El dise˜ no de los tres tests es similar. En primer lugar, se han extraido los puntos caracter´ ısticos de la primera imagen de la secuencia seleccionada para el test mediante un extractor FAST. Posteriormente se han seleccionado algunos puntos de los extra´ ıdos y se han guardado sus correspondientes localizaciones y parches. Como el proceso de emparejamiento ha sido desacoplado del SLAM, las localizaciones estimadas de los puntos caracter´ ısticos han sido seleccionadas manualmente en cada imagen y la regi´ on de b´ usqueda se ha fijado en un cuadrado de 50x50 p´ ıxeles. El emparejamiento de cada punto se ha obtenido buscando el parche con mejor correlaci´ on dentro de la regi´ on de b´ usqueda, que es el mismo m´ etodo que se usa en la aplicaci´ on SLAM. Para cada punto caracter´ ıstico e imagen se han definido las siguientes variables a evaluar: Correlaci´ on en la localizaci´ on real del punto Mejor correlaci´ on en la regi´ on de b´ usqueda Distancia entre la localizaci´ on real y la localizaci´ on de la mejor correlaci´ on 5.1.1. Test 1: Transformaci´ on de rotaci´ on con rotaci´ on 180ode la c´ amara Siguiendo el procedimiento explicado anteriormente, para el primer test se ha seleccionado una secuencia en la que el robot rota 180o. De esta secuencia se han extra´ ıdo 6im´ agenes espaciadas entre s´ ı por 20 im´ agenes y se han seleccionado 9puntos caracter´ ısticos (figura 5.1). En la figura 5.2 se muestran los resultados obtenidos para todos los parches. Los resultados de cada parche por separado as´ ı como una explicaci´ on m´ as detallada de casos puntuales se muestran en el Anexo D. Como puede verse en la primera gr´ afica de la figura 5.1, la correlaci´ on en la localizaci´ on real del punto es bastante mejor cuando se aplica la transformaci´ on de rotaci´ on que cuando no se aplica, lo que indica mayor robustez ante rotaciones de la c´ amara en torno al eje de revoluci´ on del espejo. La segunda y la tercera gr´ afica dan una idea del rendimiento de ambas opciones si estuviera integrado el proceso de emparejamiento en el algoritmo SLAM. La segunda gr´ afica muestra que es m´ as probable el emparejamiento de los parches rotados, ya que obtienen mejores valores para la mejor correlaci´ on dentro de la regi´ on de b´ usqueda (todos por encima de 0,9). A partir de la tercera gr´ afica se concluye adem´ as, que la aplicaci´ on de una transformaci´ on de rotaci´ on disminuye la posibilidad de que los emparejamientos obtenidos sean falsos ya que la distancia entre los puntos con mejor correlaci´ on y la localizaci´ on real del punto caracter´ ıstico es muy peque˜ na, mientras que no usando la transofrmaci´ on la distancia es demasiado grande. 32 CAP´ ITULO 5. REALIZACI ´ ON DE EXPERIMENTOS 12 3 4 5 6 7 8 9 1 2 34 5 6 7 8 9 1 2 345 6 7 8 9 1 2 34 5 67 8 9 1 2 3 4 5 6 7 8 9 1 2 3 4 5 6 7 8 9 Figura 5.1: Secuencia de im´ agenes tomada para el test 1(Rotaci´ on de la c´ amara de 180o). Los puntos seleccionados para emparejamiento est´ an marcados en rojo 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 30 frame Distance best correlation point − feature location (pixels) Figura 5.2: Resultados de emparejamiento para el test 1. En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 33 5.1. EXPERIMENTO 1: EVALUACI ´ ON DEL PROCESO DE EMPAREJAMIENTO UTILIZANDO EL NUEVO PARCHE 5.1.2. Test 2: Transformaci´ on de rotaci´ on con traslaci´ on de la c´ amara Para el segundo test se ha seleccionado una secuencia que contiene s´ olo traslaci´ on de la c´ amara. De esta secuencia se han extra´ ıdo 6im´ agenes espaciadas entre s´ ı por 20 im´ agenes y se han seleccionado 6puntos caracter´ ısticos para hacer el emparejamiento (figura 5.1). Al igual que en el anterior test se eval´ ua la aplicaci´ on a los parches de una transformaci´ on de rotaci´ on antes del emparejamiento, pero esta vez ante un movimiento de traslaci´ on de la c´ amara. En la figura 5.4 se muestran los resultados de todos los puntos caracter´ ısticos seleccionados a la vez (consultar Anexo D para ver las gr´ aficas de cada punto por separado y una explicaci´ on m´ as detallada). ´ Estos, al igual que en el test anterior muestran la superioridad de los parches a los que se les aplica la transforamci´ on de rotaci´ on, aunque en este caso la diferencia es menos apreciable que en el test anterior. La raz´ on es que al realizar la transformaci´ on de rotaci´ on se est´ a suponiendo que el punto se mueve en la imagen sobre una circunferencia. Esta hip´ otesis es cierta en el caso de que exista ´ unicamente rotaci´ on de la c´ amara en torno al eje de revoluci´ on del espejo, como en el test anterior. Sin embargo, en el caso de una traslaci´ on de la c´ amara, el movimiento relativo de los puntos del entorno 3D respecto de la c´ amara aparece en la imagen como un movimiento a lo largo de una curva c´ onica, cuya excentricidad aumenta desde 0cuanto m´ as alejado est´ a el punto de la recta que define la direcci´ on de movimiento de la c´ amara en la imagen. Cuando la excentricidad es 1, el punto se mover´ ıa en la imagen sobre la circunferencia que marca al l´ ınea de horizonte, cuyo radio (R∞=γ ξ) se obtiene de proyectar mediante el Modelo de la Esfera un punto cuya componente zen el sistema de referencia de la c´ amara tiende a 0. De la intersecci´ on de la circunferencia con la recta a lo largo de la que se mueve la c´ amara resultan dos puntos de fuga de los que parten/llegan (dependiendo del sentido de traslaci´ on) las trayectorias que describen los puntos en la imagen durante la traslaci´ on. Esto implica que, en un movimiento de traslaci´ on de la c´ amara, aplicar una transformaci´ on de rotaci´ on a los parches mejora el emparejamiento de puntos alejados de la trayectoria de la c´ amara, mientras que empeora el de puntos muy cercanos. Los resultados muestran tambi´ en que, a medida que la c´ amara se traslada, el emparejamiento tiende a hacerse de forma err´ onea tanto con parches rotados como con parches sin transformar. La raz´ on es que en una traslaci´ on tienen lugar un cambio de escala y de punto de vista de los puntos caracter´ ısticos que no pueden ser predichos por la transformaci´ on de rotaci´ on. 5.1.3. Test 3: Factor de escala En este test se lleva a cabo una evaluaci´ on del proceso de emparejamiento aplicando un cambio de escala a los parches mediante el factor de escala kformulado en la Secci´ on 4.3.2. Dado que, como se explic´ o previamente, este experimento est´ a desacoplado del SLAM, no es posible determinar la profundidad en la escena 3D de los parches extra´ ıdos, la cual es necesaria para calcular el factor de escala en un caso general. No obstante, interesa evaluar el cambio de escala introducido por la forma del espejo, ya que el introducido por el cambio de profundidad es simplemente inversamente proporcional a la misma. Por lo tanto en este test, no se considera la contribuci´ on de la profundidad y en lugar de seleccionar una secuencia de la base 34 CAP´ ITULO 5. REALIZACI ´ ON DE EXPERIMENTOS 1 2 3 4 5 6 1 2 3 4 5 6 1 2 3 4 5 6 1 2 3 4 5 6 1 2 3 4 5 6 1 2 3 4 5 6 Figura 5.3: Secuencia de im´ agenes tomada para el test 2(Traslaci´ on). Los puntos seleccionados para emparejamiento est´ an marcados en rojo 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) Figura 5.4: Resultados de emparejamiento para el test 2. En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 35 5.1. EXPERIMENTO 1: EVALUACI ´ ON DEL PROCESO DE EMPAREJAMIENTO UTILIZANDO EL NUEVO PARCHE Figura 5.5: Secuencia de im´ agenes tomada para el 3(Cambio de escala). Los puntos seleccionados para emparejamiento est´ an marcados en rojo de datos de Rawseeds, se ha hecho una adquisici´ on de im´ agenes sin cambios de profundidad siguiendo los siguientes pasos: - Seleccionar una zona en la escena con una potencial riqueza de puntos caracter´ ısiticos y situado lo suficientemente lejos de la c´ amara de forma que se pueda asumir D→ ∞. - Capturar im´ agenes mientras se rota la c´ amara de forma que la zona seleccionada se mueva solamente a lo largo de la direcci´ on radial. Como se ha seleccionado una zona a una distancia infinita, no ser´ an problem´ aticos posibles peque˜ nos desplazamientos de la c´ amara durante la captura. Siguiendo este procedimiento se tom´ o una secuencia de 6im´ agenes para realizar el test, siendo 7el n´ umero de puntos caracter´ ısticos seleccionados en una zona en la que no tiene lugar cambio de orientaci´ on en la imagen a lo largo de la secuencia (figura 5.5). Dentro de este test se han considerado dos posibles casos: disminuci´ on de la escala (k < 1) y aumento de la escala (k > 1). En el caso de la disminuci´ on de la escala, la extracci´ on de puntos caracter´ ısiticos se realiz´ o en la imagen donde la zona de extracci´ on estaba lo m´ as lejos posible del punto principal, mientras que para el caso de aumento de la escala el orden de la secuencia se invirti´ o y los puntos iniciales se extrajeron en la imagen en la que la zona de extracci´ on estaba m´ as cerca del punto principal. En la figura 5.6 se muestran los resultados del caso de una disminuci´ on de la escala. Puede apreciarse que el hecho de aplicar un factor de escala frente a no hacerlo proporciona mejores valores de correlaci´ on y sobre todo una menor proporci´ on de emparejamientos falsos al estimar de forma mucho m´ as precisa la localizaci´ on exacta del punto caracter´ ıstico, como puede verse en la tercera gr´ afica. Por el contrario, en el caso de un aumento de escala (figura 5.7 no existe apenas mejora al aplicar un factor de escala. La explicaci´ on m´ as plausible es que cuando el parche de un punto caracter´ ıstico aumenta su escala en la imagen aparece nueva informaci´ on que no era visible cuando ten´ ıa una escala menor y que, por tanto, no es posible extraer del parche con menor resoluci´ on. 36 CAP´ ITULO 5. REALIZACI ´ ON DE EXPERIMENTOS 1 2 3 4 5 6 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) Figura 5.6: Resultados de emparejamiento para el test 3(disminuci´ on de la escala). En rojo, con parche escalado. En azul, con parche sin escalar. 1 2 3 4 5 0.2 0.4 0.6 0.8 1 frame Correlation in feature location 1 2 3 4 5 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 0 10 20 frame Distance best correlation point − feature location (pixels) Figura 5.7: Resultados de emparejamiento para el test 3(aumento de la escala). En rojo, con parche escalado. En azul, con parche sin escalar. 37 5.2. EXPERIMENTO 2: COMPROBACI ´ ON DE LA EJECUCI ´ ON DE UN SLAM CON EL NUEVO PARCHE Tabla 5.1: Emparejamientos y puntos en el mapa final por punto inicialziado Umbral de Parche omnidireccional Parche normal correlaci´ on PI ReRmPI ReRm 0,8 8648 22,31 0,1 8923 19,75 0,087 0,9 9834 19,38 0,062 10854 16,09 0,046 0,95 13189 13,31 0,027 14970 10,56 0,019 5.2. Experimento 2: Comprobaci´ on de la ejecuci´ on de un SLAM con el nuevo parche En el segundo experimento se ha evaluado la soluci´ on para SLAM con c´ amaras omnidireccionales presentada en este proyecto utilizando la aplicaci´ on en tiempo real. Para su evaluaci´ on se ha tratado de seleccionar una secuencia en exteriores de la base de datos proprocionada por el proyecto Rawseeds. Dicha selecci´ on ha resultado problem´ atica ya que en algunas secuencias tomadas por la c´ amara omnidireccional, la imagen presentaba varias motas de suciedad lo que daba lugar a la detecci´ on de puntos inm´ oviles dentro de la zona no enmascarada provocando el colapso del SLAM. Por otro lado tambi´ en ha sido necesario mejorar la calibraci´ on de la c´ amara proporcionada por Rawseeds (Anexo C). Finalmente se ha seleccionado una trayectoria en la que se cubre una distancia total de 360 metros. Para comparar el nuevo parche con un parche normal al que no se le aplican transformaciones se ha ejecutado el SLAM en la secuencia seleccionada con diferentes umbrales de correlaci´ on para el emparejamiento y se han medido tres variables: N´ umero total de puntos inicializados (PI) Ratio de emparejamientos por punto inicializado: (Re=T otal emparejamientos P I ) Ratio de puntos en el mapa por punto inicialziado: (Rm=T amano mapa final P I ) Los resultados de la tabla 5.1 muestran que el parche dise˜ nado para c´ amaras omnidireccionales da lugar a un mejor rendimiento que el parche sin transformar, ya que se inicializan menos puntos caracter´ ısticos y se obtiene m´ as informaci´ on para el SLAM por punto inicializado. Esta diferencia es mayor a medida que se utiliza un umbral de correlaci´ on m´ as exigente y por tanto, con menos errores de emparejamiento a priori. Por otra parte, la tabla tambi´ en muestra que el umbral de correlaci´ on puede tener una influencia mayor en las variables mostradas que el tipo de parche utilizado, aunque a costa de disminuir la precisi´ on a priori del SLAM. En la figura 5.8 se muestran las proyecciones en los planos XY y YZ de la trayectoria obtenida con el nuevo parche y un umbral de correlaci´ on de 0,8. N´ otese que aunque la aplicaci´ on MonoSLAM es capaz de estimar el movimiento de la c´ amara en el plano 3D, no estando limitada a un plano 2D, la trayectoria obtenida muestra que la c´ amara se esta moviendo en un plano, como realmente ocurre, lo que da idea de su elevada precisi´ on. Finalmente hemos comparado esta trayectoria con el Ground Truth a partir de los datos del GPS, que tambi´ en son proporcionados por Rawseeds. Debido al problema comentado anteriormente de la inobservabilidad de la escala en el SLAM, se ha escalado y alineado la trayectoria obtenida con la proprocionada por los datos del GPS (figura 5.9). Para evaluar num´ ericamente la precisi´ on de la trayectoria obtenida 38 CAP´ ITULO 5. REALIZACI ´ ON DE EXPERIMENTOS −100 0 100 200 300 400 500 600 700 800 −100 −50 0 50 100 150 200 250 300 350 400 −100 0 100 200 300 400 500 600 700 800 −50 0 50 Figura 5.8: Trayectoria SLAM, obtenida con un umbral de correlaci´ on de 0,8y usando el parche para c´ amara omnidireccional, proyectada en el plano XY (arriba) y en el plano XZ (abajo). Los puntos rojos corresponden a los puntos caracter´ ısticos en el mapa final. La figura s´ olo da una idea cualitativa de la trayectoria ya que las dimensiones dependen de una escala estimada de forma arbitraria, al no ser observable. por la aplicaci´ on SLAM se han calculado el error medio de la distancia entre puntos correspondientes al mismo instante de tiempo de ambas trayectorias. El error medio calculado es µerr = 2,95 mcon una desviaci´ on t´ ıpica de σ= 1,35 my un error m´ aximo de maxerr = 5,93 m. Si dividimos el error medio entre la longitud de la trayectoria (360 m) obtenemos un error medio relativo del 0,85 %. Tambi´ en se puede comprobar la exactitud del SLAM comparando la orientaci´ on estimada de la c´ amara con la calculada a partir de los datos del GPS por diferenciaci´ on (figura 5.10) , con la ventaja de que en este caso no es necesario aplicar ning´ un factor de escala ya que es una variable que no depende de unidades de longitud. 5.3. Experimento 3: SLAM con casco-c´ amara El tercer experimento consiste en la ejecuci´ on del SLAM usando el nuevo parche con secuencias tomadas en el Campus R´ ıo Ebro de Zaragoza con una c´ amara omnidireccional acoplada a un casco. Para evaluar los resultados obtenidos se tomar´ a como referencia (Ground Truth) la trayectoria obtenida a partir de los datos proporcionados por un dispositivo GPS. Durante la experimentaci´ on con el casco-c´ amara nos hemos encontrado con una serie de inconvenientes que no exist´ ıan al trabajar con las secuencias de Rawseeds: Baja repetitibilidad del intervalo de tiempo entre capturas: La c´ amara permite 3 velocidades de adquisici´ on (7,5,15, y 30 fps). En principio se intent´ o realizar una adquisic´ on a 15 fps pero el intervalo de tiempo entre capturas se desviaba 39 5.3. EXPERIMENTO 3: SLAM CON CASCO-C ´ AMARA Figura 5.9: Trayectoria GPS (rojo) y trayectoria SLAM (verde) superpuestas sobre la imagen de sat´ elite del Campus de Bovisa (Mil´ an) donde fueron tomadas las secuencias por el equipo de Rawseeds 0 50 100 150 200 250 300 350 400 450 0 1 2 3 4 5 6 7 8Orientación del robot Time(s) orientation(rad) Figura 5.10: Orientaci´ on de la c´ amara obtenida del SLAM (azul) y de los datos de posici´ on del GPS (rojo). El ruido en esta ´ ultima se debe a que ha sido obtenida por diferenciaci´ on. 40 CAP´ ITULO 5. REALIZACI ´ ON DE EXPERIMENTOS constantemente del valor nominal, por lo que se opt´ o finalmente por una velocidad de adquisici´ on de 7,5fps con la que el intervalo de tiempo entre capturas durante la adquisici´ on se ajusta a una distribuci´ on gaussiana centrada en el intervalo nominal y con una desviaci´ on t´ ıpica relativamente baja. Elevada vibraci´ on de la c´ amara: Al ir sobre la cabeza de una persona, existen vibraciones debido al movimiento al andar o a posibles movimientos del cuello, lo que se traduce en un incremento del posible ruido de aceleraci´ on Condiciones de iluminaci´ on: Al hacerse las adquisiciones en el exterior durante d´ ıas de verano se han encontrado dificultades a la hora de conseguir hacer una buena adquisici´ on, debido a la gran variaci´ on de iluminaci´ on entre las zonas de luz y de sombra. Para evaluar el rendimiento del SLAM con el casco c´ amara se han tomado dos secuencias: una corta, haciendo un recorrido en el espacio entre el edificio Ada Byron y la parada de autob´ us, y una larga con el objetivo de llevar el SLAM al l´ ımite, dando una vuelta completa alrededor los edificios Ada Byron, Torres Quevedo y Betancourt . 5.3.1. Secuencia corta Para la secuencia corta se realiz´ o la adquisici´ on en un d´ ıa soleado a lo largo de un recorrido de 236 m. Tras realizar algunas pruebas ajustando el umbral de correlaci´ on y otros par´ ametros de sinton´ ıa como ruidos de aceleraci´ on y ajustes del extractor de puntos, se han obtenido los resultados mostrados en las figuras 5.11 y 5.12. La trayectoria obtenida mediante SLAM (figura 5.11) se ha escalado y alineado con la trayectoria obtenida a partir de los datos del GPS (figura 5.12). Debido a la imprecisi´ on de las medidas del GPS en la zona cercana al estanque, solamente se ha considerado el tramo hasta ese punto para el escalado y posterior an´ alisis cuantitativo de la precisi´ on del SLAM. Asumiendo la referencia del GPS como Ground Truth, se ha calculado un error medio entre ambas trayectorias µerr = 3,03 mcon una desviaci´ on t´ ıpica de σ= 1,31 my un error m´ aximo de maxerr = 5,82 m. Dividiendo el error medio entre la longitud del tramo considerado para el an´ alisis (190 m) se ha obtenido un error medio relativo del 1,52 %. AL igual que en el anterior experimento, en la figura 5.13 se compara tambi´ en la orientaci´ on obtenida por el SLAM con la obtenida a partir de los datos del GPS. 5.3.2. Secuencia larga La adquisici´ on para la secuencia larga se realiz´ o a lo largo de un recorrido de 1400 mdurante un d´ ıa nublado, para tener las mejores condiciones posibles de iluminaci´ on. Durante la ejecuci´ on de un SLAM con esta secuencia nos hemos encontrado con bastantes dificultades para conseguir que se complete todo el recorrido sin que colapse la aplicaci´ on. A los inconvenientes de la adquisici´ on con el cascoc´ amara descritos anteriormente, hay que a˜ nadirle la dificultad para inicializar nuevos puntos en el mapa debido a la superficie con poca textura del suelo y de algunos edificios como el Betancourt, por lo que la c´ amara acaba perdi´ endose en alg´ un punto provocando el colapso de la aplicaci´ on. No obstante se ha conseguido obtener una ejecuci´ on completa a base de reducir dr´ asticamente el umbral de puntuaci´ on de la extracci´ on FAST de puntos de la imagen. En la figura 5.14 se muestra la trayectoria SLAM estimada. 41 5.3. EXPERIMENTO 3: SLAM CON CASCO-C ´ AMARA 48 Cap´ ıtulo 6 Conclusiones El objetivo de este proyecto de fin de carrera ha sido desarrollar un sistema de SLAM monocular en tiempo real con visi´ on omnidireccional. Para ello, se ha partido de una aplicaci´ on SLAM basada en EKF ya existente para c´ amaras convencionales implementada en C++ por Andrew J. Davison 1. Se trata de una aplicaci´ on de software libre y c´ odigo abierto que ha sido continuamente actualizada desde su lanzamiento con la colaboraci´ on de varios autores entre los que se encuentran J.M.M. Montiel y Javier Civera de la Universidad de Zaragoza, que han introducido mejoras importantes como la parametrizaci´ on por profundidad inversa ( [9]) para los puntos reci´ en inicializados o la implementaci´ on de un filtro RANSAC ( [8]) de un punto para realizar el filtrado de mediciones. La principal motivaci´ on para afrontar el proyecto ha sido la mayor ventaja que, gracias a su amplio ´ angulo de visi´ on, ofrecen las c´ amaras omnidireccionales frente a las c´ amaras proyectivas al tratar de resolver un problema SLAM , lo cual se hab´ ıa demostrado previamente en el trabajo desarrollado por Alejandro Rituerto ( [23]) Para alcanzar el objetivo propuesto se han realizado una serie de modificaciones en el c´ odigo original. Las principales modificaciones realizadas han sido de dos tipos. En primer lugar, el modelo de proyecci´ on se ha adaptado a uno apropiado para sistemas omnidireccionales. Se ha implementado el llamado Modelo de la Esfera, un modelo no lineal que permite calcular la proyecci´ on de puntos del espacio en puntos de la imagen omnidireccional, y de puntos en la imagen en rayos tridimensionales en los que se encuentra el punto. Se trata de una modificaci´ on necesaria para el correcto funcionamiento del SLAM con c´ amaras omnidireccionales. Por otro lado, el descriptor de puntos caracter´ ısticos permite identificar los puntos en una imagen, necesario para poder hacer mediciones mediante emparejamientos de los puntos en una imagen nueva con los puntos del mapa. Las im´ agenes omnidireccionales conllevan un modelo de proyecci´ on m´ as complejo, as´ ı como una importante deformaci´ on y una escala variable en la imagen debido al cambio de resoluci´ on a lo largo de la direcci´ on radial. Por lo tanto, la segunda modificaci´ on es la implementaci´ on de un nuevo descriptor espec´ ıfico para c´ amaras omnidireccionales que sea invariante estos cambios de escala e invariante a rotaci´ on de la c´ amara respecto al eje de revoluci´ on del espejo. Esta modificaci´ on es opcional, ya que el algoritmo SLAM permite su ejecuci´ on de manera razonable con un descriptor normal sin invarianza a rotaci´ on ni cambios de escala. Por ello, en la fase de experimentaci´ on se ha llevadoa 1http://www.doc.ic.ac.uk/˜ajd/software.html 49 cabo una comparativa entre ambos tipos de descriptores para evaluar el grado de mejora introducido por el nuevo descriptor implementado. Tambi´ en se han realizado otros peque˜ nos cambios como la modificaci´ on del proceso de filtrado de puntos medidos, adapt´ andolo al nuevo modelo de proyecci´ on y a la morfolog´ ıa de las im´ agenes omnidireccionales, y la marginalizaci´ on fuera del vector de estados de puntos del mapa antiguos que dejan de ser detectados, con el objetivo de reducir el coste computacional cuando el mapa alcanza un tama˜ no considerable. Adem´ as se ha corregido un fallo de la aplicaci´ on en el proceso de borrado de puntos no fiables, ya que no se eliminaban todos los que deb´ ıan eliminarse en cada paso del SLAM. Una vez realizados los cambios en la aplicaci´ on original se ha procedido a la experimentaci´ on. Los experimentos se han realizado en dos fases. La primera fase consta de dos experimentos en los que se han utilizado datos facilitados por el proyecto Rawseeds para comparar el nuevo descriptor espec´ ıfico para c´ amaras omnidireccionales y un descriptor normal. En el experimento 1, se ha realizado una comparaci´ on del proceso de emparejamiento desacoplado del SLAM bajo diferentes condiciones (rotaci´ on, traslaci´ on, cambio de escala), comprob´ andose que el nuevo descriptor mejora en prestaciones al descriptor normal. En el experimento 2, se ha comparado el rendimiento de ambos descriptores durante la ejecuci´ on de un SLAM a lo largo de una trayectoria de 360 metros. Los resultados muestran que el nuevo parche ofrece un mejor rendimiento, al ser necesario inicializar menos puntos caracter´ ısticos y que se consigue un mayor n´ umero de emparejamientos exitosos por punto inicializado. Se ha comparado adem´ as una de las trayectorias obtenidas con el nuevo parche, con la trayectoria dada por el GPS considerada como Ground Truth. Debido a la inobservabilidad de la escala y a las distintas referencias de orientaci´ on de SLAM y GPS se ha realizado un alineamiento y escalado de la trayectoria SLAM para ajustarla a la del GPS. Tambi´ en se han obtenido las estimaciones de orientaci´ on de la c´ amara del SLAM y del GPS. Los resultados finales muestran la elevada precisi´ on del SLAM. En la segunda fase se ha experimentado con dos secuencias, una corta y una larga tomadas por la c´ amara-casco en el Campus R´ ıo Ebro, utilizando la nueva implementaci´ on para el parche. Se han encontrado dificultades para realizar buenas adquisiciones debido a diferentes factores como tiempos de adquisici´ on err´ aticos, vibraciones de la c´ amara o una iluminaci´ on no uniforme de la escena. La secuencia corta se tom´ o en el espacio entre el edificio Ada Byron y la parada de autob´ us a lo largo de un recorrido de 236 metros. Tras ejecutar el SLAM, se han comparado las estimaciones de trayectoria y orientaci´ on con las obtenidas a partir de los datos del GPS, procediendo del mismo modo que en el anterior experimento. Dada la imprecisi´ on del GPS hacia el final del recorrido s´ olo ha sido posible realizar la comparaci´ on en un tramo de 190 metros. Los resultados muestran una gran precisi´ on del SLAM en la estimaci´ on de la trayectoria y de la orientaci´ on. La toma de la secuencia larga se hizo a lo largo de un recorrido de 1400 metros rodeando los edificios Ada Byron, Torres Quevedo y Betancourt. Tras realizar el escalado y alineado, la estimaci´ on de la trayectoria del SLAM muestra una gran desviaci´ on respecto a la del GPS. La posible causa de esta desviaci´ on se estableci´ o, adem´ as de en al deriva del error propia de un algoritmo incremental como el EKFSLAM, en la deriva de escala que puede existir a lo largo del recorrido por su longitud y que hace insuficiente la aplicaci´ on de un escalado para el ajuste de la trayectoria. Sin embargo, evaluando la estimaci´ on de la orientaci´ on, que no depende de la escala y por lo tanto no deber´ ıa verse afectada por la deriva de escala a lo largo del recorrido, se 50 CAP´ ITULO 6. CONCLUSIONES ha observado que existe una peque˜ na variaci´ on de orientaci´ on a lo largo de los tramos rectos, que hace que se acumule un error de orientaci´ on. Este fen´ omeno nos lleva a pensar que, adem´ as de la ya mencionada deriva de escala a lo largo del recorrido existe otra fuente de imprecisi´ on en la presencia de diferentes escalas alrededor de una misma localizaci´ on, que ser´ ıa un problema propio de las c´ amaras omnidireccionales por su gran ´ angulo de visi´ on. 51 52 Anexo A Parametrizaci´ on de profundidad inversa Uno de los mayores problemas del SLAM monocular es la inicializaci´ on de nuevos puntos caracter´ ısticos en el mapa, concretamente en la estimaci´ on inicial de la incertidumbre de la profundidad para puntos distantes. En una parametrizaci´ on cartesiana seg´ un los ejes XYZ, las incertidumbres iniciales en la posici´ on para puntos con poco paralaje no se ajustan a una distribuci´ on Gaussiana impl´ ıcita en el EKF, por lo que antes de su inicializaci´ on es necesario extraer informaci´ on de varias im´ agenes para poder obtener una estimaci´ on precisa de la profundidad. Esta inicializaci´ on especial conlleva el uso de algoritmos que se desv´ ıan del uso de distribuciones de probabilidad Gaussianas. La parametrizaci´ on de profundidad inversa, desarrollada por J.Civera y J.M.Montiel en [9] permite cubrir la incertidumbre de profundidad en un rango de profundidades que comprende desde un punto cercano a la c´ amara hasta el infinito, expresando el inverso de la profundidad como una distribuci´ on Gaussiana en lugar de la propia profundidad. Esta parametrizaci´ on da lugar por tanto a una representaci´ on unificada en la que no es necesario un tratamiento especial a los puntos reci´ en seleccionados para inicializaci´ on, de forma que se pueden inicializar y poner a disposici´ on del algoritmo SLAM en el mismo momento de su selecci´ on. Con la nueva parametrizaci´ on, un punto 3D ise puede representar mediante un vector de 6 estados: yi= (xi, yi, zi, θi, φi, ρi)(A.1) donde (xi, yi, zi)son las las coordenadas de la c´ amara en el momento de la inicializaci´ on del punto, θiyφison los ´ angulos de elevaci´ on y azimuth respectivamente yρies el inverso de la profundidad. El paso de parametrizaci´ on IDP a parametrizaci´ on cartesiana se realiza mediante la siguiente ecuaci´ on: yXY Z =  Xi Yi Zi =  xi yi zi +1 ρi m(θi, φi)(A.2) donde m= (cos φisin θi,−sin φi,cos φicos θi) 53 A.1. INICIALIZACI ´ ON DE PUNTOS CARACTER´ ISTICOS Como el modelo de proyecci´ on presentado en el Cap´ ıtulo 2 se realiza la proyecci´ on a partir de un punto en coordenadas homog´ eneas tambi´ en puede ser conveniente obtener el cambio de IDP a coordenadas homog´ eneas: yh=    Xh,i Yh,i Zh,i Wh,i    =    ρi  xi yi zi +m(θi, φi) ρi    (A.3) A.1. Inicializaci´ on de puntos caracter´ ısticos El estado inicial para un punto reci´ en observado viene dado por: yi(rW C ,qW C ,p, ρ0, σρ0) = (xi, yi, zi, θi, φi, ρi)(A.4) donde rW C yqW C indican la posici´ on y orientaci´ on de la c´ amara, pla observaci´ on del punto en la imagen y ρ0yσρ0las estimaciones iniciales de la media y la desviaci´ on t´ ıpica para la distribuci´ on gaussiana de la profundidad inversa. La posici´ on de la c´ amara en el momento de la inicializaci´ on se obtiene de forma trivial: (xi, yi, zi) = rW C (A.5) Los ´ angulos de azimuth y elevaci´ on se obtienen del rayo proyectivo xen el que es mapeado el punto pseg´ un el modelo inverso de proyeccci´ on, expresado en la referencia absoluta: m(θi, φi) = RCW x(p) kx(p)k(A.6) θi φi=arctan(mx, mz) arctan(−my,√mx2+mz2)(A.7) A la profundidad inversa ρise le asigna un valor inicial ρ0determinado emp´ ıricamente junto con su desviaci´ on t´ ıpica σρ0de forma que el intervalo de confianza del 95 % englobe un rango de profundidades desde la cercan´ ıa de la c´ amara hasta el infinito. Por ello la desviaci´ on t´ ıpica debe ser elegida de tal forma que uno de los l´ ımites del intervalo de confianza sea negativo. La covarianza del resto de variables de estado del punto se obtienen de la covarianza Ridel error de medida y de la covarianza del estado total Pk|k, siendo la covarianza del estado despu´ es de la inicializaci´ on: Pnew k|k=J  Pk|k0 0 0Ri0 0 0 σρ0 JT(A.8) J=  I0 ∂y ∂rW C ,∂y ∂qW C ,0, ..., 0,∂y ∂p,∂y ∂ρ  (A.9) 54 ANEXO A. PARAMETRIZACI ´ ON DE PROFUNDIDAD INVERSA A.2. Cambio de IDP a parametrizaci´ on XYZ El inconveniente de la parametrizaci´ on de profundidad inversa es el aumento de coste computacional que supone expresar el estado de un punto con 6 pa´ rametros en vez de los 3 de al parametrizaci´ on cartesiana. Para hacer frente a este problema, se trata de convertir los puntos, siempre que sea posible, de IDP a parametrizaci´ on XYZ. La posibilidad de realizar esta transformaci´ on vendr´ a dada por el valor de un ´ ındice de linealidad Ld, que tiende a 0cuanto m´ as se aproxima la profundidad del punto a una distribuci´ on Gaussiana. Dicho ´ ındice se determina de la siguiente manera: Ld=4σd di|cos α|(A.10) con σd=σρ ρi,di=khW XY Z k,α=mThW XY Z khW XY Z k−1 σρes la covarianza extra´ ıda de la matriz de covarianzas del estado del punto que se quiere convertir y hW XY Z =yXY Z −rW C donde yXY Z son las coordenadas del punto en parametrizaci´ on cartesiana obtenidas mediante al ecuaci´ on A.2. Si Ldse encuentra por debajo del umbral de conversi´ on determinado experimentalmente, la transformaci´ on se realiza mediante la ecuaci´ on A.2 para el estado del punto y mediante el correspondiente jacobiano para la covarianza: Pnew =JPJT(A.11) J= diag(I,∂yXY Z ∂yIDP ,I)(A.12) 55 A.2. CAMBIO DE IDP A PARAMETRIZACI ´ ON XYZ 56 Anexo B La base de datos de Rawseeds Las im´ agenes utilizadas para la realizaci´ on de algunos de los experimentos del presente proyecto han sido tomadas de la base de datos puesta a disposici´ on por el proyecto Rawseeds, cuyo objetivo es construir un conjunto exhaustivo de herramientas de referencia para sistemas rob´ oticos compuesto por: Conjunto de datos de alta calidad tomados con diferentes tipos de sensores y con un Ground Truth asociado Problemas de referencia basados en los conjuntos de datos disponibles Soluciones de referencia para los problemas Este conjunto de herramientas est´ a dirigido sobre todo a la resoluci´ on de problemas de localizaci´ on, mapeado y SLAM en rob´ otica, aunque su uso no est´ a estrictamente limitado a ello, y est´ an disponibles para descargar libremente en la p´ agina web de Rawseeds. El conjunto de datos del proyecto Rawseeds corresponde a los datos tomados de forma sincronizada por el conjunto de sensores equipados en un robot mientras ´ este explora un entorno. Cada conjunto de datos est´ a compuesto por un conjunto de sesiones, cada una de las cuales incluye los datos tomados en la misma zona en momentos diferentes. Todos los conjuntos de datos de Rawseeds contienen informaci´ on de varios tipos de sensores, incluyendo datos precisos sobre la trayectoria de referencia (Ground Truth). El Ground Truth es necesario para evaluar el rendimiento de cualquier soluci´ on de referencia aplicada a los datos de uno o varios sensores. El robot utilizado para la adquisici´ on de los datos ha sido construido por el propio equipo de Rawseeds, quienes lo han bautizado como Robocom (figura B.1). Este robot se mueve mediante tracci´ on diferencial, y dispone de los siguientes sensores: Sensores ultras´ onicos de profundidad alrededor del robot. Sensores l´ aser de profundidad. Un sensor inercial que facilita la orientaci´ on del robot en 3ejes. Sistemas de visi´ on de diferentes tipos. Dispone de sistemas binocular y trinocular, de una c´ amara perspectiva convencional y un sistema hiper-catadi´ optrico de visi´ on omnidireccional. 57 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 5 10 15 20 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.8 0.85 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) (a) (b) 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 30 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.85 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 30 frame Distance best correlation point − feature location (pixels) (c) (d) 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 30 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.8 0.85 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) (e) (f) 1 2 3 4 5 6 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.8 0.85 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.85 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 5 10 15 frame Distance best correlation point − feature location (pixels) (g) (h) 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 30 frame Distance best correlation point − feature location (pixels) (i) Figura D.1: Resultados de emparejamiento para los puntos seleccionados en el test 1. En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 64 ANEXO D. RESULTADOS COMPLETOS DE LOS TESTS DEL EXPERIMENTO 1 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 30 frame Distance best correlation point − feature location (pixels) Figura D.2: Resultados de emparejamiento para todos los puntos del test 1. En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 65 1 2 3 4 5 6 0.9 0.95 1 frame Correlation in feature location 1 2 3 4 5 6 0.94 0.96 0.98 1 frame Best correlation in search region 1 2 3 4 5 6 0 0.5 1 1.5 2 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 0.6 0.8 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 5 10 15 frame Distance best correlation point − feature location (pixels) (a) (b) 1 2 3 4 5 6 −0.5 0 0.5 frame Correlation in feature location 1 2 3 4 5 6 0.9 0.95 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 0.4 0.6 0.8 1 frame Correlation in feature location 1 2 3 4 5 6 0.8 0.85 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 5 10 15 20 frame Distance best correlation point − feature location (pixels) (c) (d) 1 2 3 4 5 6 0.6 0.8 1 frame Correlation in feature location 1 2 3 4 5 6 0.85 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 0.4 0.6 0.8 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 5 10 frame Distance best correlation point − feature location (pixels) (e) (f) Figura D.3: Resultados de emparejamiento para los puntos seleccionados en el test 2. En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 66 ANEXO D. RESULTADOS COMPLETOS DE LOS TESTS DEL EXPERIMENTO 1 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) Figura D.4: Resultados de emparejamiento de todos los puntos del test 2. En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 67 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Correlation in feature location 1 2 3 4 5 6 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 0.5 0.6 0.7 0.8 0.9 frame Correlation in feature location 1 2 3 4 5 6 0.85 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 5 10 frame Distance best correlation point − feature location (pixels) (a) (b) 1 2 3 4 5 6 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 5 10 15 20 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 0.6 0.7 0.8 0.9 1 frame Correlation in feature location 1 2 3 4 5 6 0.95 0.96 0.97 0.98 0.99 frame Best correlation in search region 1 2 3 4 5 6 0 2 4 6 frame Distance best correlation point − feature location (pixels) (c) (d) 1 2 3 4 5 6 0.8 0.85 0.9 0.95 1 frame Correlation in feature location 1 2 3 4 5 6 0.92 0.94 0.96 0.98 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 6 0.8 0.85 0.9 0.95 1 frame Correlation in feature location 1 2 3 4 5 6 0.85 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 5 10 frame Distance best correlation point − feature location (pixels) (e) (f) 1 2 3 4 5 6 0.85 0.9 0.95 1 frame Correlation in feature location 1 2 3 4 5 6 0.9 0.95 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) (g) Figura D.5: Resultados de emparejamiento para los puntos seleccionados en el primer caso del test 3(disminuci´ on de la escala). En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 68 ANEXO D. RESULTADOS COMPLETOS DE LOS TESTS DEL EXPERIMENTO 1 1 2 3 4 5 6 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) Figura D.6: Resultados de emparejamiento de todos los puntos para el primer caso del test 3(disminuci´ on de la escala). En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 69 1 2 3 4 5 0.7 0.8 0.9 1 frame Correlation in feature location 1 2 3 4 5 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 0 5 10 15 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 0.6 0.7 0.8 0.9 frame Correlation in feature location 1 2 3 4 5 0.9 0.95 frame Best correlation in search region 1 2 3 4 5 0 5 10 frame Distance best correlation point − feature location (pixels) (a) (b) 1 2 3 4 5 0.8 0.9 1 frame Correlation in feature location 1 2 3 4 5 0.95 0.96 0.97 0.98 0.99 frame Best correlation in search region 1 2 3 4 5 0 5 10 15 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 0.9 0.95 1 frame Correlation in feature location 1 2 3 4 5 0.94 0.96 0.98 1 frame Best correlation in search region 1 2 3 4 5 0 0.5 1 1.5 2 frame Distance best correlation point − feature location (pixels) (c) (d) 1 2 3 4 5 0.5 0.6 0.7 0.8 0.9 frame Correlation in feature location 1 2 3 4 5 0.94 0.96 0.98 frame Best correlation in search region 1 2 3 4 5 0 10 20 frame Distance best correlation point − feature location (pixels) 1 2 3 4 5 0.4 0.6 0.8 1 frame Correlation in feature location 1 2 3 4 5 0.92 0.94 0.96 0.98 1 frame Best correlation in search region 1 2 3 4 5 0 5 10 15 frame Distance best correlation point − feature location (pixels) (e) (f) 1 2 3 4 5 0.2 0.4 0.6 0.8 frame Correlation in feature location 1 2 3 4 5 0.85 0.9 0.95 frame Best correlation in search region 1 2 3 4 5 0 5 10 15 frame Distance best correlation point − feature location (pixels) (g) Figura D.7: Resultados de emparejamiento para los puntos seleccionados en el caso del test 3(aumento de escala). En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 70 ANEXO D. RESULTADOS COMPLETOS DE LOS TESTS DEL EXPERIMENTO 1 1 2 3 4 5 0.2 0.4 0.6 0.8 1 frame Correlation in feature location 1 2 3 4 5 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 0 10 20 frame Distance best correlation point − feature location (pixels) Figura D.8: Resultados de emparejamiento de todos los puntos para el segundo caso del test 3(disminuci´ on de la escala). En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. 71 72 Anexo E Adapting a Real-Time Monocular Visual SLAM from Conventional to Omnidirectional Cameras Abstract The SLAM (Simultaneous Localization and Mapping) problem is one of the essentials challenges for the current robotics. Our main objective in this work is to develop a real-time visual SLAM system using monocular omnidirectional vision. Our approach is based on the Extended Kalman Filter (EKF). We use the Spherical Camera Model to obtain geometric information from the images wich is valid for both types of cameras. To integrate this model in the EKF-based SLAM we linearize the direct and the inverse projections. We also introduce a new patch descriptor for catadioptric omnidirectional cameras which aims to be rotation and scale invariant. We perform experiments with omnidirectional images using our new patch and the conventional one. The experimentation confirms that our patch works better with omnidirectional cameras since features last longer and constructed maps are bigger. E.1. Introduction An autonomous robot should be able to navigate through a previously unknown environment. The SLAM [31] problem tries to build a map of the surrounding and localize an autonomous robot relative to this map using only partial measurements of the environment. SLAM is usually formulated in a probabilistic way, i.e. the estimate of the robot position and map are computed as a probability distribution. Two main approaches are used for the computation of the probability distribution: the extended Kalman filter (EKF) [31] and the particle filter [2]. In Visual SLAM applications, image projections of relevant points known as local features are used as measurements. To extract and store the features on the image a feature extractor and a feature descriptor are used. The features (or corners) extractor process the image and detects the key-points on it. The image processing is a high 73 E.5. EXPERIMENTS 0 50 100 150 200 250 300 350 400 6 8 10 12 14 16 18 Distance to image centre (Rim) Size in the image (rim) 0 50 100 150 200 250 300 350 400 6 8 10 12 14 16 18 Distance to image centre (Rim) Size in the image (rim) (a) (b) Figura E.1: Comparison of the theoretical formulas to calculate the ellipse semiaxis in which the sphere is projected (red) with the results of a simulation using a camera model with radial distortion (blue) The prediction for the minor semiaxis (a) is more accurate than the one for the major semiaxis(b). sizes of the big patch and the descriptor patch respectively, the computation of the warping is done in the following steps: 1) Check the condition k > √2hP hBP cos(π 4−∆θ) + 0,1to avoid extracting a patch with no information from the big patch. If kdoes not fill this condition it is set to the threshold value. 2) Calculation of the transformation matrix: H=HtrHSHtr−1(E.22) HS=  kcos(∆θ)−ksin(∆θ) 0 ksin(∆θ)kcos(∆θ) 0 0 0 1 (E.23) with HSthe isotropic transformation matrix which combines the rotation transformation R∆θwith the scale factor kand Htr is a translation matrix to translate the patch coordinate frame to the center of the patch. 3)Computation of the warped patch by doing the inverse mapping to the original big patch and performing a bilinear interpolation. XBP =HS−1XWP (E.24) 4) Extraction of the new descriptor from the center of the warped patch. E.5. Experiments In this section the experiments carried out to evaluate the omnidirectional visual SLAM and the new patch are presented. The first experiment consists of three tests to evaluate separately the rotation and the scale invariance of the new patch by comparation with the normal patch. In the second experiment we evaluate the performance of the new patch during SLAM. The images used to lead the experiments were taken from one of the image databases provided by The Rawseeds Project 1. This database consists of a sequence 1HTTP://www.rawseeds.org 80 ANEXO E. ADAPTING A REAL-TIME MONOCULAR VISUAL SLAM FROM CONVENTIONAL TO OMNIDIRECTIONAL CAMERAS 12 3 4 5 6 7 8 9 1 2 34 5 6 7 8 9 1 2 345 6 7 8 9 1 2 34 5 67 8 9 1 2 3 4 5 6 7 8 9 1 2 3 4 5 6 7 8 9 Figura E.2: Image sequence taken for test 1(180orotation). Selected corners for matching are shown in red with more than 32000 frames acquired by a robot equiped with a hyper-catadioptric camera. E.5.1. Experiment 1 In the first experiment we have carried out three tests where we have decoupled the matching process from the SLAM algorithm to make a preliminary evaluation of our new patch attributes. In the first and the second test we evaluate an only rotation invariant patch under a rotation of 180oand a translation respectively. In the third test we evaluate the performance of an only scale invariant patch. The set up of the three test was quite similar. First, we extracted corners on the first frame with the FAST extractor. Among the extracted corners, we selected some features and stored their locations and their patches. Like the matching proccess has been decoupled from the SLAM algorithm the estimated locations of the features were manually selected on each frame (Fig. E.2) and the search region was fixed to a 50x50 pixels square. The matching in the selected frames is done by obtaining for each feature the best correlation inside the search region, which is the method used in the SLAM application where the tested patches are going to be applied. For each feature and frame, we have defined the following variables to be measured: Correlation in estimated feature location Best correlation in search region Distance between estimated feature location and best correlation location Test 1: Rotated patch and 180orotation For this test a sequence in which the robot rotates 180owas selected. From this sequence we have extracted 6frames which are spaced by 20 frames between them. We have selected 9features to carry out the test(Fig. E.2). The results show that rotated patches provide a better correlation value on the true feature location, which confirms that they are more rotation invariant than the non rotated patches (Fig. E.3). The rotated patch also provides by far better values for the best correlation inside the search region (all of them above 0,9) as well as a very low distance between true feature location and matched location. Test 2: Rotated patch and translation For the second test a sequence containing only camera translation was taken. 6 frames were extracted with intervals of 20 frames between them and the number of selected features was 6(Fig. E.4). 81 E.5. EXPERIMENTS 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 30 frame Distance best correlation point − feature location (pixels) Figura E.3: Matching results of test 1. In red, with oriented patch. In blue, with notoriented patch 1 2 3 4 5 6 1 2 3 4 5 6 1 2 3 4 5 6 1 2 3 4 5 6 1 2 3 4 5 6 1 2 3 4 5 6 Figura E.4: Image sequence taken for test 2(translation). Selected corners for matching are shown in red 82 ANEXO E. ADAPTING A REAL-TIME MONOCULAR VISUAL SLAM FROM CONVENTIONAL TO OMNIDIRECTIONAL CAMERAS 1 2 3 4 5 6 −0.5 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) Figura E.5: Matching results of test 2. In red, with oriented patch. In blue, with notoriented patch The results (Fig. E.5) reveal that under camera translation, slightly better correlation values are obtained using a rotated patch. It can be seen too, that as the distance the robot has translated increases, the matchings tend to be made in the wrong location for both patches. One posible reason is that as the robot translates the features change their scale and their point of view and can become hidden by other scene objects. Test 3: Scaled patch In this test we have evaluated the performance of the matching process with respect to the scale changes. As this experiment is decoupled from SLAM, it is not possible to determine the depth of the patches extracted. Therefore the contribution of the depth to the scale factor is not considered. So, an image adquisition without depth changes of features has been made following the next steps: - Select a zone in the scene with potential patch richness and situated far enough from the camera so that D→ ∞. - Capture images while camera rotates so that the selected zone moves only along the radial direction. Like infinite distance has been assumed, little camera displacements during capture are not problematic. A sequence of 6images was taken for the test. The number of selected features was 7. To evaluate the performance of the patches under scale change, the features were selected in a zone with no orientation change in the image (Fig. E.6). Two cases have been carried out to prove the performance under scale decrease k < 1and scale increase k > 1. In the scale decrease case, the extraction of the features was made in the image where the zone of extraction was the furthest from the image centre. For the scale increase case the order of the images has been inverted. The results of the tests show that in scale decrease (Fig. E.7) the patch with scaling offers a better performance than a normal patch while in the case of scale 83 E.5. EXPERIMENTS Figura E.6: Image sequence taken for test 3(scale change). Selected corners for matching are shown in red 1 2 3 4 5 6 0 0.5 1 frame Correlation in feature location 1 2 3 4 5 6 0.7 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 6 0 10 20 frame Distance best correlation point − feature location (pixels) Figura E.7: Matching results of test 3(scale decrease). In red, with scaled patch. In blue, with not-scaled patch increase(Fig.E.8) both patches perform in a similar way. E.5.2. Experiment 2 In this experiment we have evaluated our visual SLAM approach for omidirectional cameras with the Real-Time application developed by Davison et al. . For the evaluation we selected a long outdoor sequence from the database provided by the Rawseeds Project. To compare our new patch with the normal patch we have run the sequence using both patches with different correlation thresholds for matching and we have measured three variables ratios: Total number of features initialised (FI) . Matchings per feature ratio: (Rm=T otal matchings F I ) . Features in map per feature initialised ratio: (Rf=F inal map size F I ) The results in Table E.1 show that the patch for omnidirectional cameras (warped patch) performs better than the patch for conventional cameras with no warping (normal patch), as it initialises less features and is capable of obtaining more information for SLAM per initialised feature. However the table also shows that the 84 ANEXO E. ADAPTING A REAL-TIME MONOCULAR VISUAL SLAM FROM CONVENTIONAL TO OMNIDIRECTIONAL CAMERAS 1 2 3 4 5 0.2 0.4 0.6 0.8 1 frame Correlation in feature location 1 2 3 4 5 0.8 0.9 1 frame Best correlation in search region 1 2 3 4 5 0 10 20 frame Distance best correlation point − feature location (pixels) Figura E.8: Matching results of test 3(scale increase). In red, with scaled patch. In blue, with unescaled patch. Tabla E.1: Matchings and features in map per initialised feature Correlation Warped patch Normal patch threshold FI RmRfFI RmRf 0,8 8648 22,31 0,1 8923 19,75 0,087 0,9 9834 19,38 0,062 10854 16,09 0,046 0,95 13189 13,31 0,027 14970 10,56 0,019 correlation threshold has more influence on the measured variables than the kind of patch used. In Fig. E.9 the projections on the XY plane and the YZ plane of the trajectory obtained with the new patch and a correlation threshold of 0,8are shown. Note that although the MonoSLAM application estimates 3D camera motion, being not bounded to a 2D plane, the obtained trajectory shows that the camera is moving on the ground plane. Finally we compared this trajectory with respect to the ground truth provided by the GPS data. As scale is not observable by one single camera, for the comparison we scaled the trajectory and aligned it with the trajectory obtained with the GPS (Fig. E.10). To evaluate the accuracy of the SLAM trajectory numerically we have calculated the mean error of the distance between the corresponding points of both trajectories. The calculated mean error is µerr = 3,44 mwith a standard deviation of σ= 1,93 m and a maximum error of maxerr = 6,73 m. If we divide the mean error and the trajectory lenght, we obtain a relative mean error of 1 %. E.6. Conclusion In this work we have developed a Visual SLAM algorithm for omnidirectional cameras building on state of the art EKF monocular SLAM [8] for conventional 85 E.6. CONCLUSION −100 0 100 200 300 400 500 600 700 800 −100 −50 0 50 100 150 200 250 300 350 400 −100 0 100 200 300 400 500 600 700 800 −50 0 50 Figura E.9: SLAM trajectory with correlation threshold 0,8using the warped patch projected on the XY plane (up) and on the YZ plane (down) The red dots are the map features Figura E.10: GPS trajectory (red) and SLAM trajectory (green) superposed on the satellite image of the Campus of Bovisa (Milan) where the sequences were adquired 86 ANEXO E. ADAPTING A REAL-TIME MONOCULAR VISUAL SLAM FROM CONVENTIONAL TO OMNIDIRECTIONAL CAMERAS cameras. Two main modifications have been made: the implementation of the Spherical Camera Model for projection and the formulation of a new patch for omnidirectional cameras which is rotation and scale invariant. Then we have lead experiments to compare the new patch with the conventional patch. First we have tested the matching process decoupled from the SLAM. Once the supperiority of our new patch has been proven we have run the SLAM algorithm in a 340 meters long trajectory. Results have shown that the obtained trajectory estimation is quite accurate, which encourages us to make experiments with longer trajectories in the future. 87 E.6. CONCLUSION 88 ´ Indice de figuras 2.1. Proyecci´ on de un punto 3DXwen el plano imagen utilizando el ModelodelaEsfera........................... 12 2.2. Pasos de la proyecci´ on del Modelo de la Esfera . . . . . . . . . . . . 13 3.1. Aplicaci´ ondeSLAM........................... 16 4.1. Los puntos de la escena que est´ en contenidos dentro del cono de la figura no se pueden proyectar en el plano imagen. . . . . . . . . . . . 23 4.2. M´ ascara que se aplica a las c´ amaras omnidireccionales para no inicializar ni emparejar puntos que permancen inm´ oviles en la imagen por ser solidarios a la c´ amara....................... 23 4.3. El parche de un punto caracter´ ıstico reci´ en inicializado, se rota a una orientaci´ on fija antes de ser guardado como descriptor para futuros emparejamientos. De esta forma el parche descriptor es invariante frente a rotaci´ on de la c´ amara. ..................... 25 4.4. Comparaci´ on de las f´ ormulas te´ oricas para calcular los semiejes de la elipse en la que la esfera es proyectada (rojo) con las gr´ aficas obtenidas de una simulaci´ on con el modelo de una c´ amara con distorsi´ on radial (azul). La gr´ afica (a) correspodne al semieje menor y la gr´ afica (b) al semiejemayor. ............................. 27 4.5. Error absoluto (arriba) y relativo (abajo) en funci´ on de la distancia al punto principal entre las longitudes de los semiejes obtenidas por simulaci´ on y las obtenidas por la formulaci´ on te´ orica. En azul, para el semieje mayor y en rojo para el semieje menor. . . . . . . . . . . . . 28 4.6. Evoluci´ on del tama˜ no de un parche en la imagen (rim) en funci´ on de su distancia al punto principal (Rim) y de la distancia en metros (z) del plano horizontal en el que se encuentra el parche en la escena respecto del plano en el que se mueve la c´ amara. Si el parche se encuentra por debajo de la c´ amara (z < 0), rim decrece con Rim, mientras que si se encuentra por encima rim aumenta con Rim. ............. 29 5.1. Secuencia de im´ agenes tomada para el test 1(Rotaci´ on de la c´ amara de 180o). Los puntos seleccionados para emparejamiento est´ an marcados enrojo.................................. 33 5.2. Resultados de emparejamiento para el test 1. En rojo, con transformaci´ on de rotaci´ on. En azul, sin transformaci´ on de rotaci´ on. . . . . . . . 33 5.3. Secuencia de im´ agenes tomada para el test 2(Traslaci´ on). Los puntos seleccionados para emparejamiento est´ an marcados en rojo . . . . . . 35 89 BIBLIOGRAF´ IA [12] Christopher Geyer and Konstantinos Daniilidis. A unifying theory for central panoramic systems and practical applications. In ECCV (2), pages 445–461, 2000. [13] J. J. Guerrero, A. C. Murillo, and C. Sagues. Localization and matching using the planar trifocal tensor with bearing-only data. IEEE Transactions on Robotics, 24(2):494–501, 2008. [14] Richard Hartley and Andrew Zisserman. Multiple View geometry in Computer vision. Cambridge university press, 2000. [15] Marko Heikkila, Matti Pietikainen, and Cordelia Schmid. Description of interest regions with local binary patterns. Pattern Recognition, 42(3):425–436, mar 2009. [16] J. Neira J. A. Castellanos, J. M. M. Montiel and J. D. Tard´ os. IEEE Transactions on Robotics and Automation, 15(5):pp. 948 – 952, October 1999. [17] Sing Bing Kang. Catadioptric self-calibration. In Computer Vision and Pattern Recognition, pages 1201–1207, 2000. [18] David G. Lowe. Distinctive image features from scale-invariant keypoints. Int. J. Comput. Vision, 60:91–110, November 2004. [19] Huimin Lu and Zhiqiang Zheng. Two novel real-time local visual features for omnidirectional vision. Pattern Recogn., 43:3938–3949, December 2010. [20] C. Mei and P. Rives. Single view point omnidirectional camera calibration from planar grids. In IEEE International Conference on Robotics and Automation, April 2007. [21] L.M. Paz, P. Jensfelt, J.D. Tard´ os, and J. Neira. EKF SLAM updates in O(n) with Divide and Conquer SLAM. In Proc. of the IEEE International Conference on Robotics and Automation (ICRA’07), Rome,Italy, April 2007. [22] A. Rituerto, L. Puig, and J. J. Guerrero. Comparison of omnidirectional and conventional monocular systems for visual slam (best paper award). In OMNIVIS - 10th Workshop on Omnidirectional Vision, Camera Networks and Non-classical Cameras, Zaragoza, Spain, 2010. [23] Alejandro Rituerto, Luis Puig, and J. J. Guerrero. Visual slam with an omnidirectional camera. In Proceedings of the 2010 20th International Conference on Pattern Recognition, ICPR ’10, pages 348–351, Washington, DC, USA, 2010. IEEE Computer Society. [24] Edward Rosten, Reid Porter, and Tom Drummond. Faster and better: A machine learning approach to corner detection. IEEE Trans. Pattern Analysis and Machine Intelligence, 32:105–119, 2010. [25] D. Scaramuzza, F. Fraundorfer, and R. Siegwart. Real-time monocular visual odometry for onroad vehicles with 1-point ransac. In International Conference on Robotics and Automation (ICRA), pages 4293–4299, 2009. 96 BIBLIOGRAF´ IA [26] Davide Scaramuzza, Nicolas Criblez, Agostino Martinelli, and Roland Siegwart. Robust Feature Extraction and Matching for Omnidirectional Images. In 6th International Conference on Field and Service Robotics - FSR 2007, volume 42 of Springer Tracts in Advanced Robotics, Chamonix, France, 2007. Springer. [27] Davide Scaramuzza and Agostino Martinelli. A toolbox for easily calibrating omnidirectional cameras. In In Proc. of the IEEE International Conference on Intelligent Systems, IROS06, pages 5695–5701, 2006. [28] G. Scotti, L. Marcenaro, C. Coelho, F. Selvaggi, and C. S. Regazzoni. Dual camera intelligent sensor for high definition 360 degrees surveillance. Vision, Image and Signal Processing, 152(2):250–257, 2005. [29] Tom´ as Svoboda and Tom´ as Pajdla. Matching in catadioptric images with appropriate windows, and outliers removal. In Proceedings of the 9th International Conference on Computer Analysis of Images and Patterns, CAIP ’01, pages 733– 740, London, UK, 2001. Springer-Verlag. [30] Tomas Svoboda and Tomas Pajdla. Epipolar geometry for central catadioptric cameras, 2002. [31] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics. MIT Press, 2005. [32] Sebastian Thrun, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics (Intelligent Robotics and Autonomous Agents). The MIT Press, 2005. [33] Christian Toepfer and Tobias Ehlgen. A unifying omnidirectional camera model and its applications. In ICCV, pages 1–5, 2007. 97