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 y un ejemplo de aplicacion de ellas se encuentran en la libreria "ciiirpp" (disponible proximamente).
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
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.
