Come derivare Camera Jacobian

Question

Ho a che fare con una situazione di filtro di Kalman, cercando di tracciare punti in 3D utilizzando telecamere, ognuna delle quali può rappresentare un punto 3D come proiezione 2D secondo:

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

Dove $u$ e $v$ sono le coordinate in pixel di un punto nell'inquadratura della fotocamera; $ \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} $sono le due righe superiori della matrice della fotocamera $K$, $R_{3x3}$è una matrice di rotazione che descrive la rotazione della cornice delle coordinate mondiali rispetto alla telecamera; e$\vec{T}_{3x1}$ è una traduzione che descrive la posizione del fotogramma delle coordinate mondiali rispetto alla telecamera.

Mi piacerebbe davvero che questo fosse nella forma

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

L'ho visto $H$chiamata "matrice di osservazione" e in altri casi "Jacobian". Ma Jacobian implicherebbe una struttura simile

$$ \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 non è chiaro per me che questo ti darebbe effettivamente $\vec{y}$ a partire dal $\vec{x}$.

Quindi immagino di avere alcune domande:

• Come posso derivarne un singolo $H$ mettere la mia trasformazione in una forma in grado di gestire un filtro di Kalman?

• Devo invece usare coordinate omogenee (virata su alcuni 1)? L'aggiornamento del filtro di Kalman allo stato manterrebbe in modo affidabile l'ultima voce a 1?

• Potrei fare il mio $\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}$, ma questo non è davvero quello che voglio.

• Mantenere il termine additivo a destra rende questo non lineare? $\vec{y} = H_{2\times3} \vec{x} + K_{2\times3}\vec{T}_{3\times1} \rightarrow \vec{y} = \tilde{H}(\vec{x}) $? In che modo questo si collega quindi allo Jacobiano?

Answer 1

Ho fatto dei progressi.

In primo luogo, l'equazione della mia fotocamera era decisamente sbagliata: mi mancava una divisione per distanza dall'oggetto. Questo video è ciò che mi ha fatto vedere la luce. Veramente:

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

dove

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

Il primed ($'$) rappresentano le coordinate dell'oggetto che stai tentando di proiettare sulla fotocamera nel fotogramma di riferimento della fotocamera e le variabili non adescate rappresentano le coordinate nel fotogramma del mondo. Sto cercando di trovare$H$ rispetto alla cornice del mondo.

Questo fa

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

Se segui l'equazione giacobiana che ho lasciato sopra nella domanda (con molta attenzione, prendendo le derivate rispetto a $x$ e amici piuttosto che $x'$ e altri amici) attraverso una pagina e mezza di calcolo e semplificando l'algebra lineare, alla fine si finisce con

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

dove $ \vec{x'} = \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} $, $R_3$ è la terza riga di $R$ matrice e $\otimes$ è un prodotto esterno.

Nota che puoi opzionalmente includere la traduzione prima della rotazione nell'equazione (1) e non fa differenza per le derivate perché $x$, $y$, e $z$ non compaiono nel termine di traduzione.

In secondo luogo, è necessario fare una distinzione tra evoluzione del sistema e funzioni di osservazione e Jacobiani. Questi tendono a confondersi perché nel caso lineare moltiplicare per lo Jacobiano equivale a valutare la funzione. Prendiamo l'esempio$y_1 = ax_1 + bx_2$, $y_2 = cx_1 + dx_2$. Quindi possiamo scrivere il sistema come:

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

Ma anche

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

Questo è comodo se devi trovare entrambi $\vec{y}$ a partire dal $\vec{x}$e fai altre cose come matrici di covarianza del progetto , ma in un sistema non lineare fai queste cose con metodi separati:

Notare che la prima riga usa $h(x)$, ma nelle righe successive usiamo lo Jacobiano $H$ valutato a $x$. Allo stesso modo nella fase di aggiornamento che utilizziamo$f(x)$ per far evolvere il sistema e $F$ valutato a $x$ per aggiornare la covarianza.

Ora per rispondere in modo specifico alle mie domande:

• Non ne hai solo uno $H$; hai bisogno di entrambi$H(\vec{x})$ e $h(\vec{x})$, valutato a $\vec{x}$ perché la non linearità fa variare la forma da un luogo all'altro.

• No, non utilizzare coordinate omogenee. Questo in realtà è così non lineare (grazie a quella divisione di$z'$) di cui avrai sicuramente bisogno per usare lo Jacobiano comunque. Non sono sicuro che un filtro possa mai essere considerato attendibile per mantenere stazionaria una variabile di stato. Se non ci fosse rumore in quella variabile e le equazioni di aggiornamento fossero così, forse? Irrilevante qui adesso.

• Quella sottrazione non è..nope.

• Penso che la linearità non sia effettivamente violata da quel termine additivo, poiché è solo un offset. Ma la linearità è violata qui dopo tutto.