La Visual Odometry si pone come obiettivo quello di ricavare la posa relativa che ha assunto una camera (o una coppia stereoscopica) che si muove nello spazio analizzando due immagini in sequenza. Il problema dell’odometria visuale per una sola telecamera si risolve normalmente con il calcolo della matrice essenziale e la sua successiva decomposizione. In questo caso, come gi`a indicato in precedenza, non `e possibile conoscere la scala del movimento, ma solo mettere in relazione tra loro i vari movimenti. Discorso differente nel caso in cui si ha a disposizione una coppia stereoscopica.
Data una serie di osservazioni temporali di punti mondo ricavati dalla ricostruzione tridimensionale (xi, x0i) `e possibile ricavare in maniera lineare una trasformazione di rototraslazione (R, t) che trasforma i punti del mondo all’istante di tempo t all’istante di tempo t0 in modo da poter essere espressi con una equazione del tipo:
x0i= Rxi+ t (9.85)
Tale approccio `e generale e non dipende dal particolare sensore utilizzato per ricavare i punti.
La rototraslazione eseguita dalla coppia di sensori pu`o essere ricavata minimizzando la quantit`a:
X
i
kx0i− Rxi− tk2 (9.86)
La soluzione a 12 parametri, lineare da dati sovradimensionati, trover`a un minimo assoluto ma non `e lo stimatore ottimo, in quanto minimizza una quantit`a algebrica e in ogni caso non garantisce che la matrice di rotazione sia ortonormale. Partendo dalla soluzione lineare, l’utilizzo di un minimizzatore non-lineare (per esempio Levenberg-Marquardt, sezione 3.2.5) sulla funzione costo di equazione (9.86) permette di determinare i 6 parametri (3 rotazioni e 3 traslazioni) in modo pi`u preciso.
Questa `e algoritmo `e indicato come 3D-to-3D perch´e ricava il movimento partendo da coppie di punti tridimensionali. Come alternativa alla soluzione lineare `e possibile anche una soluzione in forma chiusa [Hor87].
L’approccio mostrato ora `e generale ma mal si adatta al caso di punti mondo ottenuti da una ricostruzione tridimensionale da immagini. La funzione costo mostrata, infatti, ottimizza quantit`a in coordinate mondo e non in coordinate immagine: il rumore sui punti dell’immagine si propaga in maniera non lineare durante la fase di triangolazione e perci`o solo in coordinate immagine `e possibile supporre che il rumore di individuazione dei punti sia gaussiano a media nulla. Non `e pertanto possibile realizzare uno stimatore a massima verosimiglianza sfruttando solamente i punti in coordinate mondo. Un approccio pi`u raffinato `e quello indicato come 3D-to-2D dove si cerca di minimizzare la riproiezione di un punto del passato in coordinate immagine:
X
i
kpi− ˆpik2 (9.87)
dove ˆp1 `e la proiezione, rototraslata, del punto tridimensionale xi ottenuto dal fotogramma precedente. Questo problema
`
e anche conosciuto come perspective from n points (PnP ) in quanto molto simile al problema gi`a visto in precedenza della calibrazione di una camera in ambiente statico.
Chiaramente anche questo approccio `e inficiato dal fatto che il punto tridimensionale xinon `e un dato del problema ma
`
e conosciuto con una certa quantit`a di errore. Per questa ragione `e necessario fare un ulteriore passo minimizzando entrambi gli errori in coordinate immagine (`e la Maximum Likelihood Estimation):
X
i
kp1− ˆp1k2+ kp2− ˆp2k2+ kp01− ˆp01k2+ kp02− ˆp02k2 (9.88)
avendo imposto ˆp1= K1R1(ˆxi− t1), ˆp2= K2R2(ˆxi− t2), ˆp01= K1R1( ˆx0i− t1) e ˆp02= K2R2( ˆx0i− t2) a cui va aggiunto il vincolo di equazione (9.85), mantendendo l’incognita sull’effettiva posizione del punto ˆxi nei due sistemi di riferimento. In questo modo viene sia minimizzato lo spostamento che eseguono le camere, sia la coordinata tridimensionale di ogni singola feature nel mondo. Anche in questo caso la soluzione alla massima verosimiglianza richiede di risolvere un problema non lineare di dimensioni notevoli. Nel caso di una coppia stereo rettificata, la funzione costo pu`o essere di molto semplificata.
L’odometria visuale `e un algoritmo di dead-reckoning e pertanto `e affetto da deriva. `E possibile estendere questi ragio-namenti al caso in cui non siano solo due gli istanti di tempo coinvolti nella minimizzazione ma molteplici. In questo caso si entra in un discorso complicato per cercare di ridurre il pi`u possibile gli errori di deriva nel comporre le diverse trasformazioni.
Un tutorial che affronta queste tematiche `e [SF11].
Quando si vuole affrontare il problema dal punto di vista bayesiano, sfruttando l’equazione (9.88), e si intendono processare contemporaneamente tutti i fotogrammi, invece che odometria visuale si preferisce parlare di Bundle Adjustment.
Il concetto di Bundle Adjustment, introdotto dalla fotogrammetria e poi acquisito dalla Computer Vision (si veda l’ottimo survey [TMHF00]), indica una minimizzazione multivariabile in modo da ottenere contemporaneamente una ricostruzione tridimensionale, le pose relative della camere in una sequenza di immagini ed eventualmente i parametri intrinseci delle camere stesse.
Si tratta di una estensione alle tecniche non-lineari che stimano i parametri attraverso la minimizzazione di una funzione di costo adeguata basata sugli errori di riproiezione dei punti individuati, nella stessa forma di equazione (9.88).
Siccome la stessa feature pu`o essere vista da diverse immagini, il processo di stima condiziona tutte le pose e di conseguenza il problema non si pu`o scomporre in n problemi separati di odometria visuale: tutte le immagini della sequenza devono essere
minimizzate contemporaneamente. Per questo motivo il problema della Bundle Adjustment `e un problema dimensionalmente elevato, sicuramente non-convesso, che richiede una ottimizzazione non semplice e fa ricorso a minimizzazione sparsa per preservare memoria e migliorare la precisione.
Un’approccio alternativo al Bundle Adjustment, sicuramente non il miglior stimatore alla massima verosimiglianza ma che introduce un numero minore di incognite, `e quello del Pose Graph Optimization [GKSB10] che, sfruttando informazione della medesima posa ottenuta da pi`u percorsi ovvero avendo individuato dei Loop, permette di ottimizzare solamente le pose rispetto a quelle ottenute dall’odometria visuale. Sia x = (x1, . . . , xn) un vettore di parametri dove l’elemento xirappresenta la posa del nodo i -esimo. Siano zij e Ωij la misura e la matrice di precisione dell’osservazione virtuale della posa relativa tra i nodi i e j. L’obiettivo `e ottenere una stima dei parametri x date le osservazioni virtuali zij. Siccome le pose relative vengono ottenute come confronto di due pose assolute, parametri da ottenere, si pu`o definire la funzione costo
eij = eij(xi, xj) = zij− ˆzij(xi, xj) (9.89) misura dell’errore tra la posa relativa virtuale misurata zij e quella predetta ˆzij(xi, xj) date le configurazioni xi e xj da valutare rispettivamente per i nodi i e j. Sfruttando l’informazione sulla precisione della stima della singola posa relativa `e possibile definire una funzione costo globale
F (x) = X
<i,j>
e>ijΩijeij (9.90)
di fatto somma delle singole distanze di Mahalanobis tra tutte le coppie (i, j) su cui `e stata fatta una misura di posa relativa.
La funzione F (x), minimizzata rispetto a x, fornisce la miglior stima delle pose assolute del problema, tutto senza coinvolgere i singoli elementi di cui `e composta la singola osservazione reale.
Appendice A
Matrici di Rotazione
v v0 y
x ϑ
v0 ≡ v y
x y0
x0 ϑ
Figura A.1: Rappresentazione dell’applicazione di una rotazione ad un vettore e a un sistema di assi: a sinistra una Inner/Active/Alibi Transformation, a destra una Outer/Passive/Alias Transformation.
Le rotazioni sono trasformazione isometriche dello spazio euclideo ovvero che trasformano vettori preservandone la lun-ghezza e lasciano un luogo di punti inalterato nello spazio pari ad un iperpiano (il centro di rotazione nel caso bidimensionale o un asse di rotazione nel caso tridimensionale).
L’insieme di tutte le Matrici di Rotazione SO(n) in Rn `e definito come Speciale Ortogonale SO(n) =R ∈ Rn: RR>= R>R = I, det R = +1
(A.1) ovvero R−1= R>.
Esistono due possibili convenzioni per definire una matrice di rotazione: alcuni autori preferiscono scrivere la matrice che trasforma da coordinate sensore a coordinate mondo, altri invece l’opposto. La matrice stessa di rotazione ha la duplice veste di indicare una rotazione all’interno di un sistema di riferimento (Active o Alibi ), o la trasformazione di coordinate da un sistema di riferimento a un secondo sistema di riferimento (Passive o Alias). In questo libro le matrici sono prevalentemente usate per rappresentare cambi di base e, quando possibile, il sistema di riferimento sorgente e quello di destinazione sono ben evidenziati.
Per discutere delle matrici di rotazione e fare qualche considerazione interessante risulta comodo partire ad analizzare il caso bidimensionale, schematizzato in figuraA.1.
Si pu`o verificare che SO(2) ha un solo grado di libert`a. La matrice Rϑ, che rappresenta una rotazione bidimensionale, pu`o essere scritta nella forma
Rθ=cos ϑ − sin ϑ sin ϑ cos ϑ
(A.2) Come si pu`o vedere dalla figuraA.1quando si parla di una rotazione di un angolo ϑ la stessa trasformazione pu`o essere vista in modi differenti, a seconda di quale sistema di riferimento l’osservatore si posizioni solidale. La matrice Rθpermette di ruotare un vettore in senso antiorario (rispetto all’origine del sistema di riferimento) di un angolo ϑ (figura a sinistra inA.1)1. La matrice di forma (A.2) oltre che a ruotare un vettore in senso antiorario permette anche di ottenere le cosidette coordinate “mondo” di un punto conoscendo la coordinate “sensore” e sapendo che tale sensore `e ruotato di un angolo ϑ
1Come gi`a accennato all’inizio, bisogna fare attenzione perch´e la trasformazione inversa/trasposta, ovvero la matrice generata dall’angolo −ϑ, in letteratura pu`o essere indicata come “matrice di rotazione” e indicata anch’essa con la lettera R.
130
(legge della mano destra) nel sistema di riferimento “mondo”. La matrice (A.2) permette perci`o di passare da coordinate
“sensore” a coordinate “mondo”, mentre l’inversa di questa matrice permette di passare da coordinate “mondo” a coordinate
“sensore”.
In questo libro, lavorando con sensori e non con bracci robotici, tutte le matrici sono di fatto matrici di cambiamento di base.
Passando al caso tridimensionale il discorso `e simile ma non pi`u univoco: esistono infinite forme matematiche per esprimere una rotazione partendo da 3 parametri so(3).
E sempre possibile definire una rotazione come composizione di 3 rotazioni elementari intorno a uno dei 3 assi ma essendo` la moltiplicazione tra matrici non commutativa esistono comunque 24 modi per comporre tra loro queste 3 matrici. Le combinazioni di matrici vengono indicate come sequenze di Eulero seguite da 3 numeri per indicare l’ordine di combinazione delle rotazioni: 1 per l’asse x, 2 per l’asse y e 3 per l’asse z. In ambito robotico sono ampiamente diffuse la rappresentazione di Angoli di Eulero (sequenza ZYZ) o quella degli angoli di Tait-Bryan (sequenza di Eulero 321 o ZYX) e si veda la sezione seguente A.1 per i dettagli. Nella letteratura italiana i sei gruppi (XYZ, YZX, ZXY, XZY, ZYX, YXZ) vengono definiti angoli di Cardano.
Questo sistema di angoli tuttavia presenta alcune singolarit`a che ne limitano l’utilizzo. Alternativamente la sintassi proposta da Rodrigues (sezioneA.2) o i quaternioni (sezioneA.3) possono essere usati per superare questo problema.
Indipendentemente dal significato geometrico a cui si vuole dare la matrice di rotazione `e possibile comunque fare diverse considerazioni.
Come gi`a detto in precedenza, la definizione della matrice R nell’equazione della pin-hole camera `e stata definita, sia per comodit`a che per tradizione, in modo tale da non ruotare un vettore (sarebbe stata ovvero una conversione da coordinate
“sensore” a coordinate “mondo”) ma all’opposto rimuove la rotazione di punti del mondo conoscendo l’orientazione della camera stessa ovvero permette di convertire da coordinate “mondo” a coordinate “camera”.
Ricavare una espressione della matrice R nella forma espressa nel modello della pin-hole camera vuol dire trovare una matrice che trasforma un punto da coordinate “mondo” a coordinate “camera” ovvero bisogna sempre usare la matrice inversa delle matrici di rotazioni che si possono trovare nelle sezioni seguenti.
Sia pertanto una generica rotazionewRbche trasforma da coordinate locali, mobili, “sensore” (body coordinates nel caso generico) a coordinate globali, fisse, “mondo”: la matrice (wRb)−1=bRwsar`a pertanto la matrice che converte da coordinate mondo a coordinate sensore.
Siccome per`o il sistema di riferimento camera/immagine `e un sistema Left-Bottom-Front (X crescente verso destra, Y crescente verso il basso, Z la profondit`a come in figura 8.3) che `e diverso dal sistema di riferimento Front-Left-Up sen-sore/mondo (Z crescente verso l’alto, X profondit`a e Y crescente verso sinistra come in figura 8.4) tipico dell’ambiente automotive, `e necessario definire una matrice
cΠb=
0 −1 0
0 0 −1
1 0 0
(A.3)
matrice di permutazione degli assi. La matrice di permutazione ha determinante +1 pertanto `e ancora una rotazione che preserva la chiralit`a dello spazio (trasforma sistemi destrorsi in sistemi destrorsi).
Lavorando in ambito areonautico o navale potrebbe invece essere necessario passare dal sistema camera/immagine a un sistema Front-Right-Down (ad esempio il NED). In questa situazione la matrice di permutazione `e
cΠb0 =
0 1 0 0 0 1 1 0 0
(A.4)
Sotto queste considerazioni, la matrice R che converte da “mondo” a “camera”, formalismo usato normalmente nell’e-quazione della camera pin-hole, ha come espressione
R =cRw=cΠb(wRb)−1 (A.5)