Como derivar Camera Jacobian

Question

Estou lidando com uma situação de Filtro de Kalman, tentando rastrear pontos em 3D usando câmeras, cada uma das quais pode representar um ponto 3D como uma projeção 2D de acordo com:

$$ \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} $$

Onde $u$ e $v$ são as coordenadas de pixel de um ponto no quadro da câmera; $ \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} $são as duas linhas superiores da matriz da câmera $K$, $R_{3x3}$é uma matriz de rotação que descreve a rotação do quadro de coordenadas mundiais em relação à câmera; e$\vec{T}_{3x1}$ é uma tradução que descreve a posição do quadro de coordenadas mundiais em relação à câmera.

Eu realmente gostaria que isso estivesse na forma

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

Eu vi isso $H$chamada de "matriz de observação" e em outros casos de "Jacobiana". Mas Jacobian implicaria uma estrutura 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} $$

e não está claro para mim se isso realmente daria a você $\vec{y}$ de $\vec{x}$.

Então, acho que tenho algumas perguntas:

• Como posso derivar um único $H$ colocar minha transformação em uma forma que um filtro de Kalman possa manipular?

• Devo usar coordenadas homogêneas (aderência em alguns 1s) em vez disso? O filtro de Kalman atualizado para o estado manteria de forma confiável sua última entrada 1 então?

• Eu poderia fazer meu $\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}$, mas isso realmente não é o que eu quero.

• Manter o termo aditivo à direita torna isso não linear? $\vec{y} = H_{2\times3} \vec{x} + K_{2\times3}\vec{T}_{3\times1} \rightarrow \vec{y} = \tilde{H}(\vec{x}) $? Como isso se relaciona com o Jacobiano?

Answer 1

Eu fiz algum progresso.

Em primeiro lugar, a equação da minha câmera estava definitivamente errada: estava faltando uma divisão por alcance para o objeto. Este vídeo é o que me fez ver a luz. Mesmo:

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

Onde

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

O preparado ($'$) variáveis ​​representam coordenadas do que você está tentando projetar para a câmera no quadro de referência da câmera, e as variáveis ​​não programadas representam coordenadas no quadro mundial. Estou tentando encontrar$H$ wrt o quadro mundial.

Isto faz

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

Se você seguir a equação Jacobiana que deixei acima na questão (com muito cuidado, tomando as derivadas $x$ e amigos em vez de $x'$ e outros amigos) por meio de uma página e meia de cálculo e simplificando a álgebra linear, você eventualmente acaba com

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

Onde $ \vec{x'} = \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} $, $R_3$ é a terceira linha do $R$ matriz, e $\otimes$ é um produto externo.

Observe que você pode incluir opcionalmente a translação antes da rotação na equação (1), e isso não faz diferença para as derivadas porque $x$, $y$, e $z$ não aparecem no termo de tradução.

Em segundo lugar, há uma distinção a ser feita entre a evolução do sistema e as funções de observação e os jacobianos. Eles tendem a se confundir porque, no caso linear, a multiplicação pelo Jacobiano é o mesmo que avaliar a função. Veja o exemplo$y_1 = ax_1 + bx_2$, $y_2 = cx_1 + dx_2$. Então, podemos escrever o 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} $$

Mas também

$$ \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} $$

Isso é conveniente se você precisa encontrar $\vec{y}$ de $\vec{x}$e fazer outras coisas como projetar matrizes de covariância , mas em um sistema não linear você faz essas coisas por métodos separados:

Observe que a primeira linha usa $h(x)$, mas nas linhas subsequentes usamos o Jacobiano $H$ avaliado em $x$. Da mesma forma, na etapa de atualização que usamos$f(x)$ para evoluir o sistema e $F$ avaliado em $x$ para atualizar a covariância.

Agora, para responder especificamente às minhas perguntas:

• Você não consegue apenas um $H$; você precisa de ambos$H(\vec{x})$ e $h(\vec{x})$, avaliado em $\vec{x}$ porque a não linearidade faz com que a forma varie de um lugar para outro.

• Não, não use coordenadas homogêneas. Na verdade, isso é tão não linear (graças a essa divisão por$z'$) que você definitivamente vai precisar usar o Jacobiano de qualquer maneira. Não tenho certeza se um filtro pode ser confiável para manter uma variável de estado estacionária. Se não houvesse ruído nessa variável e as equações de atualização fossem justas, talvez? Irrelevante aqui agora.

• Essa subtração não é ... nada.

• Acho que a linearidade não é realmente violada por esse termo aditivo, já que isso é apenas um deslocamento. Mas a linearidade é violada aqui, afinal.