Cómo derivar Camera Jacobian

Question

Estoy lidiando con una situación de filtro de Kalman, tratando de rastrear puntos en 3D usando cámaras, cada una de las cuales puede representar un punto 3D como una proyección 2D de acuerdo con:

$$ \begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} \cdot R_{3\times3} \cdot \begin{bmatrix} x \\ y \\ z \end{bmatrix} + \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} \cdot \vec{T}_{3\times1} $$

Dónde $u$ y $v$ son las coordenadas de píxeles de un punto en el marco de la cámara; $ \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} $son las dos filas superiores de la matriz de la cámara $K$, $R_{3x3}$es una matriz de rotación que describe la rotación del marco de coordenadas mundial con respecto a la cámara; y$\vec{T}_{3x1}$ es una traducción que describe la posición del marco de coordenadas mundial en relación con la cámara.

Realmente me gustaría que esto estuviera en la forma

$$ \vec{y} = H \vec{x} $$

He visto esto $H$llamada la "matriz de observación" y en otros casos el "jacobiano". Pero jacobiano implicaría una estructura como

$$ \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_1}{\partial x_2} & \frac{\partial y_1}{\partial x_3} \\ \frac{\partial y_2}{\partial x_1} & \frac{\partial y_2}{\partial x_2} & \frac{\partial y_2}{\partial x_3} \end{bmatrix} $$

y no tengo claro si esto realmente te daría $\vec{y}$ desde $\vec{x}$.

Así que supongo que tengo algunas preguntas:

• ¿Cómo puedo derivar un solo $H$ poner mi transformación en una forma que un filtro de Kalman pueda manejar?

• ¿Debería usar coordenadas homogéneas (agregar algunos 1) en su lugar? ¿La actualización del filtro de Kalman al estado mantendrá de manera confiable su última entrada en 1 entonces?

• Podría hacer mi $\vec{y} = \begin{bmatrix} u \\ v \end{bmatrix} - \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} \cdot \vec{T}_{3\times1}$, pero esto realmente no es lo que quiero.

• ¿Mantener el término aditivo a la derecha hace que esto no sea lineal? $\vec{y} = H_{2\times3} \vec{x} + K_{2\times3}\vec{T}_{3\times1} \rightarrow \vec{y} = \tilde{H}(\vec{x}) $? Entonces, ¿cómo se relaciona esto con el jacobiano?

Answer 1

He hecho algunos progresos.

Primero, la ecuación de mi cámara era definitivamente incorrecta: me faltaba una división por rango para el objeto. Este video es lo que me hizo ver la luz. De Verdad:

$$ \begin{bmatrix} u \\ v \end{bmatrix} = K_{2\times3} \cdot \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix}/z' $$

dónde

$$ \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} = R_{3\times3} \cdot \begin{bmatrix} x \\ y \\ z \end{bmatrix} + T \tag{1}$$

El cebado ($'$) las variables representan las coordenadas de lo que está intentando proyectar en la cámara en el marco de referencia de la cámara, y las variables no cebadas representan coordenadas en el marco del mundo. Estoy tratando de encontrar$H$ wrt el marco del mundo.

Esto hace

$$ u = \frac{\alpha_x x'}{z'} + u_0 $$ $$ v = \frac{\alpha_x x'}{z'} + u_0 $$

Si sigue la ecuación jacobiana que dejé arriba en la pregunta (con mucho cuidado, tomando derivadas wrt $x$ y amigos en lugar de $x'$ y otros amigos) a través de una página y media de cálculo y simplificando el álgebra lineal, eventualmente terminas con

$$ Jacobian_{2\times3} = \frac{KR}{z'} - \frac{K\vec{x'} \otimes R_3}{z'^2} $$

dónde $ \vec{x'} = \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} $, $R_3$ es la tercera fila de la $R$ matriz, y $\otimes$ es un producto externo.

Tenga en cuenta que, opcionalmente, puede incluir la traslación antes de la rotación en la ecuación (1), y no hace ninguna diferencia en las derivadas porque $x$, $y$y $z$ no aparecen en el término de traducción.

En segundo lugar, hay que hacer una distinción entre la evolución del sistema y las funciones de observación y los jacobianos. Estos tienden a difuminarse porque en el caso lineal multiplicar por el jacobiano es lo mismo que evaluar la función. Toma el ejemplo$y_1 = ax_1 + bx_2$, $y_2 = cx_1 + dx_2$. Entonces podemos escribir el sistema como:

$$ \begin{bmatrix} y_1 \\ y_2 \end{bmatrix} = \begin{bmatrix} a & b \\ c & d \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} $$

Pero también

$$ \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_1}{\partial x_2} \\ \frac{\partial y_2}{\partial x_1} & \frac{\partial y_2}{\partial x_2} \end{bmatrix} = \begin{bmatrix} a & b \\ c & d \end{bmatrix} $$

Esto es conveniente si necesita encontrar $\vec{y}$ desde $\vec{x}$y hacer otras cosas como proyectar matrices de covarianza , pero en un sistema no lineal, hace estas cosas mediante métodos separados:

Observe que la primera línea usa $h(x)$, pero en las líneas siguientes usamos el jacobiano $H$ evaluado en $x$. Asimismo, en el paso de actualización utilizamos$f(x)$ evolucionar el sistema y $F$ evaluado en $x$ para actualizar la covarianza.

Ahora para responder específicamente a mis preguntas:

• No solo obtienes uno $H$; necesitas ambos$H(\vec{x})$ y $h(\vec{x})$, evaluado en $\vec{x}$ porque la no linealidad hace que la forma varíe de un lugar a otro.

• No, no uses coordenadas homogéneas. Esto es realmente tan no lineal (gracias a esa división por$z'$) que definitivamente necesitarás usar el jacobiano de todos modos. No estoy seguro de si alguna vez se puede confiar en un filtro para mantener estacionaria una variable de estado. Si no hubiera ruido en esa variable y las ecuaciones de actualización fueran así, ¿quizás? Irrelevante aquí ahora.

• Esa resta no es ... no.

• Creo que la linealidad en realidad no es violada por ese término aditivo, ya que eso es solo una compensación. Pero la linealidad se viola aquí después de todo.