Medici´on Visual de Desplazamiento y Posici´on

El Kinect es le´ıdo utilizando el paquete openni launch, que entrega los drivers necesarios para obtener la imagen rectificada en formato RGB y con profundidad. El paquete fovis ros es utilizado para traducir estos datos en una odometr´ıa visual. La calibraci´on utilizada son los valores por defecto. Los datos de la covarianza de las medi- ciones no son conocidos por lo que se les asigna un valor de 0.05 seg´un sugiere ROS para experimentos con este tipo de sensores.

Existen situaciones en las que no se puede estimar la posici´on, como por ejemplo cuando no se tienen suficientesinliers. En estos casos, el resultado de la odometr´ıa entre- gado por el algoritmo son ceros. Esto genera saltos de una posici´on cualquiera al origen. Es evidente que esto tendr´ıa consecuencias negativas al ingresarlo comoinputde posici´on al algoritmo de vuelo.

Para corregir este problema y agregar la matriz de covarianza se crea el nodokinect to posewithcovstamp. Este nodo esta suscrito a la publicaci´on de odometr´ıa, agrega la covarianza y publica el mensaje tipo PoseWithCovarianceStamped necesario para el algoritmo. Adicionalmente guarda el ´ultimo valor de odometr´ıa correcto registrado. En

caso de que exista error (todas las mediciones son 0), entrega en vez de esta medici´on una repetici´on del ´ultimo valor correcto. El c´odigo completo se encuentra en el anexo D.3.

La interacci´on entre los nodos se muestra en la figura 3.7.

FIGURA 3.7. Esquema de la interacci´on de los nodos en la odometr´ıa Kinect.

X, Y, Z representan las coordenadas de la odometr´ıa directa, mientras que

X0, Y0, Z0 representan las coordenadas luego de la correcci´on y Σla matriz de covarianza de las mediciones.

La odometr´ıa utilizada se obtiene del paquetefovis rosque implementa el c´odigo presentado por Huang et al. (2011). Un esquema general del algoritmo se muestra en la figura 3.8. El algoritmo consta de seis partes principales se describen brevemente a continuaci´on.

a) Pre-procesamiento de la imagen

A partir de la imagen en escala de grises, una pir´amide gaussiana es generada para encontrarfeaturesen niveles m´as altos. Estos corresponden a objetos m´as grandes, por lo que tienen mayor probabilidad de repetirse y son m´as robustos frente a im´agenes borrosas generadas por el movimiento.

b) Extracci´on defeatures

Se extraen features de cada nivel de la pir´amide utilizando el algoritmo FAST para detecci´on defeaturesde Rosten y Drummond (2006). Cadafeaturese extrae junto con su profundidad, en caso de no tener este dato, se descarta. Elthresholddel algoritmo FAST se adapta con un control proporcional simple para alcanzar suficientes features en cada cuadro. Los niveles de la pir´amide son divididos enbucketsdentro de los cuales se elije un

FIGURA3.8. Algoritmo de odometr´ıa visual implementado.

subconjunto de features que tengan mayorFAST corner score. El objetivo es lograr una distribuci´on uniforme de losfeaturesen la imagen.

c) Estimaci´on de rotaci´on inicial

La rotaci´on 3D es lo que causa la mayor parte del aparente desplazamiento de fea- turesen cuadros sucesivos. En este paso es utilizada la t´ecnica propuesta por Mei, Sibley, Cummins, Newman, y Reid (2009), que propone estimar la rotaci´on inicial minimizando la suma de los errores de pixeles al cuadrado de cuadros submuestreados. Esto es preferi- do frente a otras alternativas, como por ejemplo, el uso de la IMU, debido a que hace el algoritmo m´as independiente de la plataforma y sus sensores, mientras se mantiene una precisi´on razonable.

d) Matchingdefeatures

Un descriptor es asignado a cadafeaturey se emparejan entre distintos cuadros usan- do el m´etodo de mutual consistency check propuesto por Nister, Naroditsky, y Bergen

(2004). A estosmatches se les asigna un scoreseg´un la f´ormula SAD (sum-of-absolute- differences) de sus descriptores propuesta por Howard (2008). El match con menorscore que se encuentre dentro de la ventana definida por la estimaci´on de rotaci´on inicial se declara como el par.

La ubicaci´on de los features en el cuadro nuevo es refinada a nivel de subpixel mi- nimizando la suma de error cuadr´atico de todos los features. Para esto, el m´etodo ESM propuesto por Malis (2004) es usado para resolver el problema de iteraci´on no lineal de m´ınimos cuadrados.

e) Detecci´on deinliers

Este es otro paso en el que matchesincorrectos son descartados. Se ocupa la aproxi- maci´on de Howard (2008) para generar un gr´afico de los matches y descartar outliers bas´andose en el hecho de que para movimientos de cuerpos r´ıgidos se mantienen las dis- tancias, por lo que la distancia euclideana entre features se debe mantener en cuadros distintos. Esto se detalla en las publicaciones de Howard (2008) y Hirschmuller, Innocent, y Garibaldi (2002).

f) Estimaci´on de Movimiento

El m´etodo de orientaci´on absoluta propuesto por Horn (1987) es utilizado para ob- tener una estimaci´on inicial. Este m´etodo minimiza la distancia euclideana entre matches clasificados comoinliers.

La estimaci´on inicial es refinada minizando el error de reproyecci´on de losfeaturesa trav´es del algoritmo para problemas no lineales de m´ınimos cuadrados de Malis (2004). Matches que excedan cierto error de reproyecci´on son descartados delset deinliersy la estimaci´on se refina nuevamente.

Finalmente, con el objetivo de reducirdriftde baja escala se utiliza lo que es denomi- nado“keyframe technique”. Inicialmente se tiene un cuadro de referencia, y se consideran tres posibilidades:

Si el movimiento de la c´amara relativo al cuadro de referencia se calcula con suficientesinliers, se mantiene el cuadro de referencia.

Si no se cumple con el n´umero deinliers, se reemplaza la referencia por el nuevo cuadro.

Si la estimaci´on de movimiento con respecto a la referencia falla, se reintenta usando el segundo cuadro m´as reciente.