Resumen de aprendizaje del algoritmo SVO

Introducción

SVO: Odometría visual monocular semidirecta rápida
SVO: Odómetro visual monocular semidirecta

SVO: Odometría visual semidirecta para sistemas monoculares y multicámara
SVO: Odometría visual semidirecta para sistemas monoculares y multicámara

El algoritmo SVO fue originalmente una odometría visual semidirecta propuesta basada en la aplicación de vehículos aéreos no tripulados.

1. Introducción al proceso de algoritmo

El algoritmo se divide principalmente en dos subprocesos paralelos: seguimiento y mapeo;
el subproceso de seguimiento se divide principalmente en tres pasos, que incluyen coincidencia entre fotogramas, coincidencia de proyección de punto espacial 3D y optimización BA. Primero, optimice la pose de la cámara de acuerdo con el mínimo fotométrico error, luego optimice la posición del bloque de píxeles de puntos característicos por separado y, finalmente, realice la optimización BA en la pose del cuadro actual de acuerdo con la nueva posición y la pose inicial.

• La coincidencia entre fotogramas consiste en utilizar el fotograma anterior como fotograma de referencia y la pose del fotograma anterior como pose inicial. La posición del punto característico en el fotograma actual se obtiene a través del seguimiento de flujo óptico y se puede obtener la característica del fotograma anterior. reproyectando el punto 3D del marco de referencia. Las coordenadas proyectadas del punto en el marco actual, la pose inicial del marco actual se puede obtener optimizando el error fotométrico mínimo del bloque de píxeles (4x4) de todos los puntos característicos en el imagen.
• Después de obtener la coincidencia de pose inicial entre fotogramas, las coordenadas de píxeles de las características del fotograma actual se refinan aún más. Al proyectar los puntos de mapa correspondientes en el cuadro clave de vista común a la imagen del cuadro actual y al minimizar el error fotométrico entre el bloque de píxeles de puntos característicos en el cuadro clave de referencia y el bloque de píxeles de puntos característicos en el cuadro actual, las coordenadas precisas de se puede obtener el punto característico en el marco actual
• Luego se fijan las coordenadas de los puntos coincidentes y se obtiene la pose del cuadro actual minimizando el residuo de reproyección de los puntos del mapa en el cuadro actual.

El subproceso de mapeo es principalmente responsable de crear y mantener un filtro de profundidad para fotogramas clave. Siempre que se realiza un seguimiento de un fotograma de la imagen, se seleccionarán algunos puntos del mapa en el momento actual para calcular la paralaje del punto del mapa entre el fotograma de referencia anterior y el Si la distancia desde el fotograma clave de referencia anterior es superior a cierto umbral, el fotograma actual se establece como un fotograma clave.

SVO solo extrae puntos característicos para fotogramas clave e inicializa los puntos característicos como puntos iniciales del filtro de profundidad, y estima la profundidad correspondiente al punto inicial en función del principio de búsqueda límite y coincidencia de bloques de imágenes. del punto semilla es diferente después de múltiples observaciones Si la certeza es inferior a cierto umbral, se considera convergente y se juzga como un punto candidato. Si el punto se observa con éxito 10 veces, se considera un buen punto del mapa.

2. Alineación de imagen/coincidencia entre fotogramas

1. El proceso principal
   (1) encuentra los puntos del mapa p1, p2, p3 vistos en el cuadro anterior (K-1).
   (2) Repruebe la imagen bidimensional del cuadro actual (k) para obtener coordenadas de píxeles 2D
   (3) Luego, minimice la función de error de escala de grises (optimizada entre el bloque de píxeles rastreado por el flujo óptico y el bloque de píxeles obtenido por reproyección El nivel de gris diferencia) para obtener la pose
   El proceso consiste principalmente en fijar las coordenadas de los puntos coincidentes y optimizar el movimiento entre fotogramas T
   

Cuando se utiliza el método de Gauss-Newton para la optimización iterativa, la posición de proyección del marco de referencia en el marco actual se selecciona para cada optimización, de modo que pueda expandirse en la misma posición cada vez que se deriva, y luego la matriz jacobiana es fijo

SVO considera que el movimiento entre imágenes de fotogramas adyacentes es pequeño, por lo que al calcular el error fotométrico, se toma un bloque de 4x4 píxeles alrededor del punto característico.

3. Alineación de puntos característicos

Este proceso es una corrección adicional de las coordenadas de los puntos característicos en el cuadro actual. La pose obtenida anteriormente se obtiene a través del seguimiento de flujo óptico. Aquí hay un punto 3D fijo y optimizado. Al minimizar el punto 3D en el cuadro clave, el correspondiente bloque de píxeles y el actual Los bloques de píxeles correspondientes en el cuadro refinan aún más la posición de los puntos característicos en el cuadro actual.

3.1 Selección de fotogramas clave de vista común y puntos del mapa local

SVO aquí usa un método de 5 puntos para seleccionar el cuadro clave de vista común, que consiste en seleccionar un punto característico en las cuatro esquinas y el centro del cuadro clave. Si alguno de estos 5 puntos característicos se puede proyectar en el cuadro actual , inicialmente se considera que la clave El fotograma se visualiza conjuntamente con el fotograma actual.

Luego, de acuerdo con el orden de la distancia del cuadro clave de cerca a lejos, los puntos del mapa correspondientes a los puntos característicos de estos cuadros clave se proyectan en el cuadro actual a su vez, y el mismo punto del mapa solo se proyecta una vez. Si la posición de proyección del punto del mapa en el cuadro actual puede obtener un mosaico de 8x8, almacene el punto del mapa en la cuadrícula de la posición de proyección del cuadro actual.

Luego, proyecte todos los puntos candidatos del mapa en el cuadro actual. Si se puede obtener un mosaico de 8x8 en la posición proyectada en el cuadro actual, almacene el punto candidato del mapa en la cuadrícula en la posición proyectada del cuadro actual.

3.2 Correspondencia entre puntos del mapa y fotogramas clave

Después de encontrar el punto del mapa, es hora de encontrar el bloque de píxeles correspondiente a la característica 2D del punto del mapa. Cada punto del mapa será observado por múltiples fotogramas clave. Conecte el centro óptico del punto del mapa y el fotograma clave para obtener un vector de dirección Seleccione el vector de dirección con el cuadro actual El cuadro clave con el ángulo incluido más pequeño y, a continuación, seleccione el bloque de píxeles correspondiente al punto 3D en el cuadro clave para realizar la alineación de puntos característicos.

3.3 Minimización del error fotométrico basado en el método de flujo óptico inverso

La estrategia adoptada según el método de flujo de luz de fondo es expandir en la misma posición de la imagen de referencia cada vez, evitando así operaciones repetidas como la derivación. Después de cada optimización, las coordenadas u1' del punto característico ajustado se obtendrán en el marco de referencia, y luego el punto característico u2 correspondiente al marco actual se moverá en la misma cantidad, y el valor real de u2 abordarse continuamente a través del proceso anterior para cada iteración.

4. Optimización de poses

Después de obtener las coordenadas coincidentes precisas, las coordenadas de los puntos característicos del cuadro actual se fijan y luego se optimiza la pose de la cámara en el cuadro actual. Sigue siendo el error fotométrico de reproyección 3D, y finalmente se puede determinar la pose precisa. obtenido.
La función residual es la siguiente:

Después de obtener la nueva pose, los puntos del mapa se optimizarán aún más, es decir, se calculará el error de reproyección correspondiente a los puntos del mapa 3D.

5. Extracción de puntos del mapa

SVO solo extrae puntos clave cuando la imagen es un cuadro clave y luego encuentra las coordenadas de los puntos característicos que coinciden con el cuadro clave de referencia en el cuadro actual a través de la búsqueda epipolar y la coincidencia de bloques. A continuación, restaure la información de profundidad del punto 3D en función del filtro de profundidad y utilice el punto 3D recién creado como punto semilla (la semilla es el punto de píxel cuya profundidad no ha convergido)

6. Actualice la semilla según el filtro de profundidad

Al realizar la profundidad de puntos característicos, la profundidad calculada una vez tiene un cierto error.Para la profundidad del mismo punto característico, es necesario realizar varias mediciones para restaurar el valor de profundidad con un error menor. SVO asume que la información de profundidad de los puntos de píxel obedece a la distribución gaussiana, y la profundidad correspondiente del punto característico se calcula para cada fotograma de la imagen, y luego la información de profundidad del punto semilla se actualiza continuamente con nuevas mediciones hasta la incertidumbre de profundidad. del punto de semilla converge.

Para una semilla, cada filtrado de profundidad (también llamado fusión de profundidad, usando un nuevo marco para actualizar el valor de profundidad de la semilla) necesita pasar por el siguiente proceso:

7. Discusión sobre el método de flujo de luz de fondo

En términos generales, las ventajas del método de flujo óptico inverso se reflejan principalmente en los cálculos de matriz jacobiana y hessiana durante el proceso de solución de gauss-newton Para el gauss-newton convencional en el proceso iterativo, la posición de la variable independiente x en el objetivo la imagen está constantemente Realice la expansión de Taylor, porque cada punto de expansión es diferente, por lo que cada iteración debe volver a calcularse para calcular la matriz jacobiana y hessiana, etc. La estrategia adoptada según el método de flujo de luz de fondo es expandir en la misma posición de la imagen de referencia cada vez, evitando así operaciones repetidas como la derivación.

7.1 Problema de mínimos cuadrados en el método de flujo óptico

El método de flujo óptico se implementa en base a la suposición de invariancia fotométrica (el valor de gris del punto de imagen en cada imagen en el mismo punto espacial es el mismo), es decir, la siguiente fórmula:

El significado físico de esta fórmula es que un punto x en la imagen de referencia T sufre una transformación de coordenadas p para obtener x', y la escala de grises del píxel en la coordenada x' de la imagen actual del cuadro I es igual al valor de la escala de grises del píxel en x en la imagen T.

En la fórmula, T(x) representa 参考图象el valor de píxel gris del punto medio x. W representa una función sobre x y p, y W(x;p) representa las coordenadas del punto x después de la transformación (la transformación puede ser de traslación pura o afín). p es un vector de parámetros que se utiliza para construir la transformación asociada. I(.) representa 当前图像el valor de píxel gris de un cierto punto en . Finalmente, la fórmula anterior se establece ajustando el valor de p.

La ecuación (1) es solo para un punto, pero en la práctica, se considerarán las restricciones de solución de múltiples puntos en p. Por lo tanto, el método de flujo óptico general resuelve la siguiente fórmula:

Este es el proceso de obtener la solución de mínimos cuadrados de p a la fórmula (2), que se puede resolver mediante un método iterativo (como el método de Gauss-Newton). Para la fórmula (2), las fórmulas de actualización de p y W(.) son (estas fórmulas de actualización no son difíciles de entender, son una simple superposición de p y W(.)):

La solución iterativa inevitablemente requiere la derivación de la fórmula original. Primero encuentre la expansión de Taylor de primer orden en I(W(x;p+△p)) para la fórmula (2):

▽I(W) en la fórmula es el gradiente de píxeles en el punto de coordenadas de píxeles W(x;p) en la imagen I, que está relacionado con la coordenada de píxeles actual, es decir, relacionado con p. Al mismo tiempo, la matriz jacobiana de W con respecto a p también está relacionada con el valor actual de p. Por lo tanto, el método anterior necesita encontrar una nueva derivada después de cada iteración de optimización. Después de eso, se obtiene el incremento △p mediante la construcción de la matriz hessiana y se completa la operación de solución iterativa. Los pasos generales son los siguientes:

Haga un pequeño resumen: puede ver que después de construir la función de pérdida anterior, es un proceso de resolución de los mínimos cuadrados basado en el método de Gauss-Newton.Basado en las coordenadas de x en el marco de referencia, iterativamente transforma p para encontrar una nueva coincidencia en el marco actual. El punto x' se actualiza en el nuevo punto x' del marco actual en este proceso de iteración continua. Cada actualización necesita calcular la matriz jacobiana y la matriz hessiana correspondientes. Este proceso desperdicia muchos recursos informáticos ., se propone el método de flujo óptico inverso para este fenómeno.

7.2 El problema de los mínimos cuadrados en el método de flujo óptico inverso

El método de flujo óptico inverso también se implementa en base a la suposición de invariancia de grises, pero resuelve diferentes problemas de optimización iterativos. El objetivo de su solución es la siguiente fórmula:

En comparación con la fórmula (2), las imágenes T e I en la fórmula (6) han cambiado de posición. El objeto de △p ha cambiado de la imagen I a la imagen T (esta debería ser la razón por la que el algoritmo se llama flujo óptico inverso). En este punto, la fórmula de actualización de W(.) se convierte en:

Permítanme hablar sobre mi comprensión de la fórmula (7). Primero, marque dos pares de puntos que se estén emparejando en las dos imágenes T e I, y regístrelos como Pt y Pi respectivamente. Dado que la transformación afín actualmente estimada de las dos imágenes es W(x;p), podemos usar la siguiente figura para representar la situación actual:

En este momento, realizamos un ajuste fino en la imagen T, es decir, realizamos la operación W(x;△p) en Pt y registramos el punto transformado como Pt'. En este momento, la situación se muestra en la siguiente figura:

Suponemos que después de esta actualización de △p, el error de escala de grises entre Pt' y Pi es el más pequeño, y se puede considerar que Pt' es el punto de coincidencia correspondiente a Pi. Bueno, después de mucho tiempo encontré un punto coincidente para Pt'. Pero no importa, al menos sabemos que Pi no será un punto de coincidencia para Pt. Por tanto, es necesario seguir buscando en la imagen I, es decir, mover la posición de Pi para obtener un nuevo punto Pi'. ¿Cómo moverlo? Lo pensamos de esta manera, dado que Pt' se obtiene mediante la transformación △p, luego usamos la transformación en la dirección opuesta a △p para Pi (a partir de aquí se puede ver que la limitación del método de flujo óptico inverso requiere W(. ;△p) ser reversible !) es posible obtener puntos que coincidan con Pt. Este paso se realiza mediante la fórmula (9), y la situación se muestra en la siguiente figura:

Entonces, ¿cuáles son los beneficios de tal enfoque? Primero realizamos una expansión de Taylor de primer orden en la fórmula (8), y las ventajas de este método se pueden ver en la expansión:

▽T en la fórmula representa el gradiente de píxeles en el punto W(x;0), que es una cantidad fija. La matriz jacobiana de W sobre p se obtiene en (x;0) (esto significa que p no influye en el cálculo del jacobiano), y además es una cantidad fija. Después de eso, se obtiene el incremento △p mediante la construcción de la matriz hessiana y se completa la operación de solución iterativa. Los pasos generales son los siguientes:

En la figura se puede ver que, dado que algunas cantidades son fijas durante todo el proceso de iteración, sus valores se pueden calcular al comienzo del algoritmo y se pueden usar todo el tiempo durante el proceso de iteración sin actualizar. Esta cantidad fija es la clave para reducir la cantidad de cálculo de todo el algoritmo.

8. Filtro de profundidad especial

8.1 Búsqueda epipolar y coincidencia de bloques

La siguiente figura es un diagrama esquemático de la búsqueda de límites.

1. La línea epipolar busca
un determinado punto característico p1 del marco de referencia conocido (imagen de la izquierda).De acuerdo con el principio de geometría de la cámara, el punto del espacio 3D correspondiente a p1 está en algún lugar del rayo que conecta el centro óptico O1 y p1, asumiendo la profundidad del punto 3D El rango es (d_min, +∞).

De manera similar, el punto 3D también tiene un punto de proyección p2 en la imagen del cuadro actual adyacente, y p2 también es el punto de intersección de la línea que conecta el punto 3D y el centro óptico O2 del cuadro actual. Debido a que se desconoce la posición del punto 3D, la proyección de las posibles posiciones de todos los puntos 3D dentro del rango (d_min, +∞) en el cuadro actual formará una línea epipolar l2, sabemos que p2 debe estar en esta línea epipolar , solo necesitamos El punto de coincidencia p2 de p1 en el marco actual se puede encontrar buscando a lo largo de la línea epipolar. Este proceso es la búsqueda límite.

2. Coincidencia de bloques
De acuerdo con la idea del método directo, al comparar el valor de gris de cada punto de píxel en la línea epipolar l2 con p1, cuanto menor sea la diferencia de valor de gris, mayor será la posibilidad de un punto de coincidencia. Sin embargo, si hay muchos puntos similares a p1 en la línea epipolar, puede ocurrir un gran desajuste al comparar las diferencias entre píxeles individuales, por lo que se introduce el concepto de coincidencia de bloques. Es decir, comparando las diferencias entre los bloques de píxeles en la vecindad de los puntos característicos para mejorar el grado de discriminación y encontrar el punto de coincidencia p2 correspondiente.

3. Método de coincidencia de bloques
Ahora tome pequeños bloques de píxeles alrededor de p1, asumiendo que el tamaño del bloque de píxeles es w×w, y también se toman muchos bloques pequeños en la línea epipolar. Es aconsejable registrar los pequeños bloques alrededor de p1 como Bi A ∈ Rw×w, y los n pequeños bloques en la línea epipolar como Bi, i=1,...,n.

(1) SAD (Suma de Diferencia Absoluta). Como su nombre lo indica, se toma la suma de los valores absolutos de las diferencias entre dos pequeños bloques

(2) SSD (Suma de distancia al cuadrado), que es la suma de los cuadrados de la diferencia entre los valores de gris de dos bloques de píxeles:

(3) NCC (correlación cruzada normalizada) (correlación cruzada normalizada). De esta manera calcula la correlación de dos pequeños bloques.
El numerador es la suma de la multiplicación de los elementos correspondientes de la matriz A y la matriz B, y el denominador es la raíz cuadrada de la suma de los cuadrados de los elementos de la matriz A y la suma de los cuadrados de los elementos de la matriz B.

Al mismo tiempo, se puede realizar la operación de valor medio, y la operación de valor medio se realiza en la matriz AB respectivamente, y luego se utiliza el método anterior para tratar los cambios de luz y oscuridad entre fotogramas adyacentes.

8.2 Filtros de profundidad gaussiana

Ahora, calculamos la medida de similitud entre A y cada Bi en la línea epipolar. Para facilitar la descripción, asumiendo que usamos NCC, obtendremos una distribución NCC a lo largo de la línea epipolar. La forma de esta distribución depende en gran medida del aspecto de la imagen en sí. En el caso de largas distancias de búsqueda, generalmente obtenemos una función no convexa: hay muchos picos en esta distribución, pero debe haber solo un punto real correspondiente. En este caso, tendemos a usar una distribución de probabilidad para describir el valor de profundidad, en lugar de un solo valor para describir la profundidad. Entonces, nuestra pregunta es cómo cambiará nuestra distribución de profundidad estimada cuando sigamos haciendo búsquedas epipolares en diferentes imágenes: este es el llamado filtro de profundidad .

1. La idea central del filtro de profundidad basado en la distribución gaussiana
es asumir que el valor de profundidad del punto característico y el valor de profundidad observado (obtenido por la triangulación de múltiples cuadros) están sujetos a la distribución de probabilidad gaussiana, y la profundidad del punto característico se actualiza continuamente a través de la observación de la profundidad.La distribución de probabilidad del valor, cuando la incertidumbre de la profundidad es menor que el umbral, se considera convergente, y el valor de profundidad en este momento es la profundidad del punto característico. Sin embargo, la traslación entre dos marcos se utiliza para la estimación de la profundidad. Dado que la traslación calculada por coincidencia monocular no tiene información de escala real, la profundidad estimada en este momento tampoco tiene escala.

Proceso principal:

1. Suponga que la profundidad de todos los píxeles satisface una distribución gaussiana inicial;
2. Cuando se generan nuevos datos, la posición del punto de proyección se determina mediante la búsqueda epipolar y la coincidencia de bloques;
3. Calcular la profundidad y la incertidumbre después de la triangulación según la relación geométrica;
4. Fusionar la observación actual con la estimación anterior. Detenga el cálculo si converge, de lo contrario devuelva 2.

Suponga que la profundidad de un punto característico obedece a una distribución gaussiana con media μ y varianza σ:P(d) ~ N(μ，σ^2)

Cuando llegan nuevos datos (se genera un nuevo emparejamiento), el valor observado se puede obtener de acuerdo con el principio geométrico, asumiendo que el valor observado también obedece a la distribución de probabilidadP(d_obs) ~ N(μ_obs，σ_obs^2)

Luego, la información de observación actual se puede usar para actualizar la distribución de probabilidad anterior de los puntos característicos. De acuerdo con las propiedades de la distribución gaussiana, se pueden realizar las siguientes actualizaciones:

Cuando la incertidumbre de la distribución fusionada converge a un cierto umbral, se considera que el valor estimado en este momento es la profundidad del punto característico correspondiente.

2. La explicación de He Jiajia sobre el filtro de profundidad
La estimación de profundidad más básica es la triangulación, que es el contenido básico de la geometría multivista. Sabemos que el valor de profundidad de este punto se puede calcular haciendo coincidir puntos de dos fotogramas de imágenes.Si hay varias imágenes, se pueden calcular varios valores de profundidad de este punto. Es como si hubiéramos realizado múltiples mediciones sobre la misma variable de estado, por lo tanto, la estimación bayesiana se puede utilizar para fusionar múltiples valores de medición para reducir la incertidumbre de la estimación. Como se muestra abajo:

Al principio, la incertidumbre de la estimación de profundidad es grande (parte verde claro), después de obtener una estimación de profundidad por triangulación, esta incertidumbre puede reducirse considerablemente (parte verde oscuro).

8.3 Uso de filtros de profundidad en SVO

Se hace referencia al siguiente contenido desde: https://zhuanlan.zhihu.com/p/85190014

Aquí, primero introduzca brevemente el proceso de triangulación de profundidad de cálculo en svo, principalmente la búsqueda epipolar para determinar el punto de coincidencia. En el marco de referencia Ir, conocemos la posición de la imagen de una característica, asumiendo que su valor de profundidad está entre [d_min,d_max], luego de acuerdo con los valores de profundidad de estos dos puntos finales, podemos calcular su posición en el marco actual Ik , como el segmento de línea en el círculo verde hierba en la figura anterior.

Después de determinar la posición del segmento epipolar donde aparece la característica, se puede realizar la búsqueda y coincidencia de características. Si el segmento epipolar es muy corto, menos de dos píxeles, el uso directo del método de flujo óptico de alineación de características mencionado anteriormente al calcular la pose puede predecir la posición de la característica con mayor precisión.

Si el segmento de línea epipolar es muy largo, se divide en dos pasos. El primer paso es muestrear a intervalos en el segmento de línea epipolar, y hacer coincidir los bloques de características muestreados con los bloques de características en el marco de referencia uno por uno, y usar el método de la suma media cero de las diferencias al cuadrado para cada puntaje de bloque de características de muestreo, cualquiera que tenga el puntaje más alto indica que coincide mejor con el bloque de características en el marco de referencia. El segundo paso es usar la Alineación de funciones cerca del punto de puntuación más alto para obtener la posición del punto de función con una precisión de subpíxeles. Una vez que se determina la posición del píxel, la profundidad se puede calcular por triangulación.

Finalmente, después de obtener una nueva estimación de profundidad, se puede usar el modelo de probabilidad bayesiano para actualizar el valor de profundidad.

En el proceso de estimación de profundidad, además de calcular el valor de profundidad, también se debe calcular la incertidumbre de este valor de profundidad, que se utilizará en muchos lugares, como determinar la posición inicial y la longitud de la línea epipolar en el epipolar. la búsqueda de línea, como el uso de la probabilidad bayesiana, se usa en el proceso de actualización de la profundidad para determinar el peso de actualización (al igual que el papel que desempeña la matriz de covarianza en el filtro de Kalman), como juzgar si el punto de profundidad ha convergido, insertando el mapa si converge, y así sucesivamente.

8.4 Profundidad inversa

Al buscar líneas epipolares, puede haber varios valores extremos. Para determinar el valor de profundidad real, el valor de profundidad se describe utilizando la distribución de probabilidad de profundidad.

(1) El error de profundidad inversa puede reducir el error causado por los puntos de mayor profundidad en el mundo real. Por ejemplo, para diferentes puntos de profundidad a 100m y 50m, puede aparecer como una distancia de varios píxeles en la imagen.Si se usa el error de profundidad, la diferencia es 50, pero el error de profundidad inverso es solo 1/50-1/ 100 = 0.01
(2) Cuando se usa la profundidad inversa, la expresión homogénea del factor de profundidad inversa y las características se pueden obtener normalizando las coordenadas, y las variables de optimización se reducen (3) La expresión de la profundidad inversa puede
expresar puntos más lejanos y está más cerca de la distribución gaussiana