Movimento della telecamera dalle immagini corrispondenti

Question

Sto provando a calcolare una nuova posizione della telecamera in base al movimento delle immagini corrispondenti. le immagini sono conformi al modello di telecamera pinhole.

In effetti, non ottengo risultati utili, quindi cerco di descrivere la mia procedura e spero che qualcuno possa aiutarmi.

Corrispondono alle caratteristiche delle immagini corrispondenti con SIFT, le combino con FlannBasedMatcher di OpenCV e calcoliamo la matrice fondamentale con findFundamentalMat (metodo RANSAC) di OpenCV.

Poi calcolare la matrice essenziale dalla telecamera matriciale intrinseca (K):

Mat E = K.t() * F * K; 

I decompongono la matrice essenziale di rotazione e traslazione con singolare decomposizione in valori:

SVD decomp = SVD(E); 
Matx33d W(0,-1,0, 
      1,0,0, 
      0,0,1); 
Matx33d Wt(0,1,0, 
      -1,0,0, 
      0,0,1); 
R1 = decomp.u * Mat(W) * decomp.vt; 
R2 = decomp.u * Mat(Wt) * decomp.vt; 
t1 = decomp.u.col(2); //u3 
t2 = -decomp.u.col(2); //u3 

Poi provo per trovare la soluzione corretta per triangolazione. (questa parte è da http://www.morethantechnical.com/2012/01/04/simple-triangulation-with-opencv-from-harley-zisserman-w-code/ quindi penso che dovrebbe funzionare correttamente).

La nuova posizione viene quindi calcolato con:

new_pos = old_pos + -R.t()*t; 

dove New_Pos & old_pos sono vettori (3x1), R la matrice di rotazione (3x3) e T il vettore di traslazione (3x1).

Purtroppo non ho ottenuto risultati utili, quindi forse qualcuno ha un'idea di cosa potrebbe essere sbagliato.

Ecco alcuni risultati (solo nel caso in cui qualcuno può confermare che nessuno di loro è sicuramente sbagliato):

F = [8.093827077399547e-07, 1.102681999632987e-06, -0.0007939604310854831; 
    1.29246107737264e-06, 1.492629957878578e-06, -0.001211264339006535; 
    -0.001052930954975217, -0.001278667878010564, 1] 

K = [150, 0, 300; 
    0, 150, 400; 
    0, 0, 1] 

E = [0.01821111092414898, 0.02481034499174221, -0.01651092283654529; 
    0.02908037424088439, 0.03358417405226801, -0.03397110489649674; 
    -0.04396975675562629, -0.05262169424538553, 0.04904210357279387] 

t = [0.2970648246214448; 0.7352053067682792; 0.6092828956013705] 

R = [0.2048034356172475, 0.4709818957303019, -0.858039396912323; 
    -0.8690270040802598, -0.3158728880490416, -0.3808101689488421; 
    -0.4503860776474556, 0.8236506374002566, 0.3446041331317597] 

Answer 1

Prima di tutto si dovrebbe verificare se

x' * F * x = 0 

del punto di corrispondenze x' e x. Questo dovrebbe essere, naturalmente, solo il caso degli inlier della stima della matrice fondamentale con RANSAC.

Successivamente, è necessario trasformare le corrispondenze tra punti di coordinate immagine normalizzate (NCC) simili

xn = inv(K) * x 
xn' = inv(K') * x' 

dove K' è la matrice telecamera intrinseca della seconda immagine e x' sono i punti della seconda immagine. Penso che nel tuo caso sia K = K'.

Con questi NCC è possibile scomporre la matrice essenziale come descritto. Triangoli le coordinate della telecamera normalizzate e controlli la profondità dei tuoi punti triangolari. Ma fai attenzione, in letteratura dicono che un punto è sufficiente per ottenere la rotazione e la traduzione corrette. Dalla mia esperienza dovresti controllare alcuni punti poiché un punto può essere un outlier anche dopo RANSAC.

Prima di scomporre la matrice essenziale, assicurarsi che sia E=U*diag(1,1,0)*Vt. Questa condizione è richiesta per ottenere risultati corretti per le quattro possibili scelte della matrice di proiezione.

Quando si ottiene la rotazione e la traslazione corrette è possibile triangolare tutte le corrispondenze punti (gli inlier della stima della matrice fondamentale con RANSAC). Quindi, dovresti calcolare lo reprojection error. In primo luogo, si calcola la posizione riproiettata simili

xp = K * P * X 
xp' = K' * P' * X 

dove X è la posizione 3D computerizzata (omogenea). P e P' sono le matrici di proiezione 3x4. La matrice di proiezione P viene normalmente fornita dall'identità. P' = [R, t] è dato dalla matrice di rotazione nelle prime 3 colonne e righe e la traduzione nella quarta colonna, in modo che P sia una matrice 3x4. Funziona solo se si trasforma la posizione 3D in homogeneous coordinates, ovvero i vettori 4x1 invece di 3x1. Quindi, xp e xp' sono anche coordinate omogenee che rappresentano le posizioni 2D (riproiettate) dei punti corrispondenti.

penso che il

new_pos = old_pos + -R.t()*t; 

non è corretto in quanto in primo luogo, si traduce solo il old_pos e non ruotano e in secondo luogo, si traducono con un vettore sbagliata. Il modo corretto è dato sopra.

Quindi, dopo aver calcolato i punti riproiettati, è possibile calcolare l'errore di riproiezione. Dato che stai lavorando con coordinate omogenee, devi normalizzarle (xp = xp/xp(2), dividere per ultima coordinata). Ciò è dato dalla

error = (x(0)-xp(0))^2 + (x(1)-xp(1))^2 

Se l'errore è grande come 10^2 la calibrazione fotocamera intrinseca o la vostra rotazione/traslazione sono corretti (forse entrambi). A seconda del tuo sistema di coordinate puoi provare a invertire le tue matrici di proiezione. Per questo motivo è necessario trasformarli in coordinate omogenee prima, dato che non è possibile invertire una matrice 3x4 (senza lo pseudo inverso). Quindi, aggiungi la quarta riga [0 0 0 1], calcola l'inverso e rimuovi la quarta riga.

C'è ancora una cosa con errore di riproiezione. In generale, l'errore di riproiezione è la distanza quadrata tra la corrispondenza del punto originale (in ogni immagine) e la posizione riproiettata. Puoi prendere la radice quadrata per ottenere la distanza euclidea tra i due punti.

Answer 2

Per aggiornare la posizione della telecamera, è necessario aggiornare prima la traduzione, quindi aggiornare la matrice di rotazione.

t_ref += lambda * (R_ref * t); 
R_ref = R * R_ref; 

dove T_REF e R_ref sono il vostro stato della fotocamera, R e T sono nuovi rotazione della telecamera calcolato e la traduzione, e lambda è il fattore di scala.