Visual Odometry Using Multiple Landmarks
Motivation
The experimental validation of different control strategies as well of computer vision algorithms aimed to solve specific problems of mobile robot navigation needs, in general, to have a reliable measure of the robot pose (location and orientation) respect to some predefined reference frame. Here, robustness must be required in order to deal with the usual problems of variable illumination conditions and/or significant view-point changes.
General (brief) description
To solve the above mentioned problems we propose to integrate the multiple measures of the robot pose relative to a set of artificial landmarks distributed over the environment on which the robot moves. Such landmarks, also called "fiducial markers" in the literature, corresponds to simple black and white planar patterns that are easily detectable in real-time by standard image processing methods. Once their are detected, the knowledge of the camera parameters (obtained off-line) and the pattern physical dimensions will allow to recover the pose of the robot relative to each one of such landmarks. This individual measures, together with their uncertainty estimates, will be used to compute the instantaneous pose of the robot in a robust way, e.g., using a non-linear Kalman filter.
Determinacion de Pose
En el paper "Robust Pose Estimation from a Planar Target" se discute sobre la determinacion de pose, y en la foma de tratar una ambiguedad en la misma que se da cuando el angulo que forman las normales de los planos de la camara y del landmark es mayor a 34,8º. En estos casos, los algoritmos de deteccion de pose arrojan dos resultados probables. La diferencia entre estos dos resultados aumenta conforme aumenta el nivel de ruido y la distancia del landmark a la camara. Para resolver esta ambiguedad, se trabaja con una funcion de error en la cual el minimo de esta funcion indica la pose correcta. Cuando los algoritmos de pose arrojan dos soluciones posibles, esta funcion posee dos minimos locales en vez de uno, y se determina el resultado correcto buscando el menor de ellos.
Los algoritmos de determinacion de pose propuestos en el paper "Robust Pose Estimation from a Planar Target", estan codificados en C++ en la libreria "librpp" de ARToolkitPlus. Se migraron estos codigos para utilizar los tipos de datos y funciones presentes en las VXL. Estas funciones se encuentran encapsuladas en la clase pose_estimation_schweighofer, donde se calcula la pose utilizando el algoritmo iterativo de C. Lu mencionado en el paper, y se resuelve la ambiguedad.
Analisis de desempeño con imagenes generadas en forma virtual:
Analisis de desempeño con imagenes reales:
http://ciii.frc.utn.edu.ar/Vision/VisualOdometryWithLandmarks/AnalisisErrorPoseSchweighofer
Resultados en Octave
Se comenzó con los scripts en Octave de Dominik para estimar la pose de dos landmarks, contemplando la salida y entrada de cualquiera de los dos en la toma. Estos scripts obtienen un único resultado de pose de la cámara respecto a un punto considerado fijo, aplicando Kalman sobre los resultados individuales.
Los scripts se modificaron para aceptar N landmarks, y tomar como referencia la pose inicial de la cámara (pose calculada en la primera toma). El resultado único se obtiene por un promedio de los resultados individuales obtenidos, como un cálculo provisorio.
El script principal es "many_landmarks.m", que debe ser una función pero en este caso se editó para poder hacer debugging del código. Se cargan los 33 puntos del landmark y se acondicionan para utilizar la funcion rpp() que devuelve la pose que minimiza el error, y eventualmente puede devolver la segunda posibilidad.
El problema surge cuando se en la toma se encuentra un landmark perpendicular al eje de la cámara. Al momento de estimar la pose, el número de iteraciones pasa de 20 - 40 a 700 - 800, lo que hace muy lento el algoritmo (en Octave). Revisando el código, se encontró una parte donde se resuelve una ecuación de orden 4 (getRotationY_wrtT.m). En el caso de landmarks perpendiculares al eje de la cámara, la solución a esa ecuación tiene 1 y hasta 2 pares de soluciones complejas conjugadas. Estas soluciones no son debidamente contempladas (aparentemente) en los cálculos posteriores. Se modifico la función para que considere solamene las soluciones reales a la ecuación. Ahora no devuelve un error, pero los resultados obtenidos son valores muy erráticos para esa pose determinada. En el caso que los landmarks no sean perpendiculares al eje de la cámara, la solución es repetitiva.
El problema de estimación del algoritmo parece ser la falta de precisión en el cálculo de Beta_t. Para valores medios, el problema no es tan evidente, pero para los que están próximos a cero, el error es grande. Eso se evidencia cuando el plano del landmark es perpendicular al eje de la cámara (Beta próximo a cero).
Mudando a C++
La clase pose (camera) tiene almacenados los datos referentes a la pose única de la cámara, haciendo fusión de la pose calculada para cada landmark encontrado. Los métodos se encargan de estimar la pose (con rpp()), relacionar los landmarks encontrados (landmarks_found) con los que están registrados (landmarks), actualizar los datos y fusionar los resultados para encontrar la pose única. El método que se encarga de hacer todo esto esto es calculate_pose de esta forma:
- Crea el vector de landmarks basado en la cantidad de elemento de un vector iptrs que se pasa como parámetro
- Ordena los datos para pasarlos a la función rpp que estima la pose, para cada elemento de iptrs
- Se cargan esos valores en el vector de landmarks (landmarks_found)
- Se encuentra la correspondencia con los landmarks registrados (para frame > 0). Se compara por diferencia entre los módulos de los vectores de traslación (más adelante se puede hacer analizando un código implícito en el modelo del landmark)
- Si el landmark que se encontró ya estaba en el registro, se actualizan los valores (found = true) y se elimina el elemento del vector de landmarks encontrados. Los landmarks que quedan en el vector landmarks_found son los nuevos, que no estaban registrados.
- Se calcula la pose de la camara en relacion a la pose inicial y la pose anterior, para cada landmark registrado y vuelto a encontrar. Estos son los que se utilizarán para hacer la fusión.
- Se realiza la fusión de los datos. Por ahora es solamente el promedio.
- Con esa pose única calculada se calcula la pose actual para los elementos perdidos y la pose 0 para los elementos nuevos.
El resto de los métodos son para cargar o leer valores desde la clase
Análisis de las Fuentes de Error
Un error introducido en cualquiera de los parámetros puestos en la determinación de la pose lleva a un error en el cálculo de la odometría visual. Para conocer el grado de incidencia de cada una de estas fuentes de error, se deben analizar por separado para determinar sus efectos y conocer cuál es la más crítica de éstas. Se analizan errores discriminados en la determinación de la pose (R;t) y algunas de sus posibles causas debidas a errores en la calibración de la cámara.
http://ciii.frc.utn.edu.ar/Vision/VisualOdometryWithLandmarks/AnalisisFuenteError
Programa para la creación de patrones virtuales
Para evitar las fuentes de error causadas por el desconocimiento de la pose exacta de la cámara al momento de realizar pruebas de los algoritmos de estimación de pose, se desarrollo un programa que genera los patrones virtualmente con la pose, constantes de distorsión y matriz característica de la cámara que el usuario desea. Además permite el barrido de cualquiera de estos parámetros, generando el set de imágenes correspondiente.
http://ciii.frc.utn.edu.ar/Vision/VisualOdometryWithLandmarks/LandmarksVirtuales
Generador de archivo de datos para trayectorias curvas
Se desarrolo un script en lenguaje Octave que genera el archivo de datos para pasar como parámetro del programa que crea las imagenes virtuales del patrón. Funciona para trayectorias curvas, con un centro y radio determinados, y con un angulo de inclinación de la cámara.
http://ciii.frc.utn.edu.ar/Vision/VisualOdometryWithLandmarks/ArchivoDatos
Pendiente
- Generar un vector de control points, rotarlo, trasladarlo y detectarlo directamente, en vez de generar imagenes. Esto elimina los errores en la deteccion de los contol points y errores introducidos por redondeo de pixeles al generar la imagen.
- Analisis de errores en los parametros de calibracion, dejando fijos los parametros R y t del fiducial.
- Generacion virtual de trayectorias mas complejas.
Separar el algoritmo de desambiguacion de pose de la clase schweighofer, y modificar su llamado para poder pasarle un puntero a funcion, de manera de poder aplicar el metodo de desambiguacion con otro algoritmo de deteccion de pose(actualmente se utliza solo pose_estimation_lu).
Referencias
[1].- Gerald Schweighofer and Axel Pinz, "Robust Pose Estimation from a Planar Target", IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 28, no. 12, Dec. 2006.
[2].- ARToolkitPlus.