Sviluppo e confronto di tecniche di stima della traiettoria di sensori 3D

Universita degli Studi di TriesteDIPARTIMENTO DI INGEGNERIA E ARCHITETTURA

Corso di Laurea Magistrale in Ingegneria Informatica

Tesi di Laurea inIngegneria Informatica

Sviluppo e confronto

di tecniche di stima della traiettoria

di sensori 3D

Laureando:Andrea Bidinost

Relatore:Prof.Felice Andrea Pellegrino

Anno Accademico 2014 - 2015

Indice

Introduzione 3

1 Formazione dell’immagine 3D da sensore a luce strutturata 5I sensori a luce strutturata . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5Modelli di riferimento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

Camera stenopeica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6Rappresentazione matriciale . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

Calcolo delle coordinate spaziali da copertura puntiforme . . . . . . . . . . . . . . . . . . . . . . . 8Dallo schema infrarosso all’immagine di disparita . . . . . . . . . . . . . . . . . . . . . . . . . 9Dall’immagine di disparita all’immagine di profondita . . . . . . . . . . . . . . . . . . . . . . 10Dall’immagine di profondita alla nuvola di punti . . . . . . . . . . . . . . . . . . . . . . . . . 12

2 L’algoritmo Iterative Closest Point (ICP) 15Descrizione dell’algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

Selezione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16Accoppiamento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17Pesatura e rigetto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18Stima delle normali . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19Dall’immagine di profondita alla nuvola di punti . . . . . . . . . . . . . . . . . . . . . . . . . 19Iterazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20

Complessita computazionale . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20Allocazione di memoria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20Risorse temporali . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

3 Stima della posizione nello spazio di profondita inversa 23Lo spazio di profondita inversa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23Descrizione dell’algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24

Selezione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25Accoppiamento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25Pesatura e Rigetto . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

Stima delle normali . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26Minimizzazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26Iterazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28


4 Stima della posizione con approccio Frame Based 31Descrizione dell’algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

Selezione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33Accoppiamento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33Pesatura e Rigetto e Minimizzazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33Stima delle normali . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

INDICE 1

Iterazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34Complessita computazionale . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

Allocazione di memoria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34Risorse temporali . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

5 Stima della posizione con approccio Color Fusion 37Calibrazione stereo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37L’algoritmo SIFT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

Estrazione di estremi su scala e spazio . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38Localizzazione e raffinamento dei punti notevoli . . . . . . . . . . . . . . . . . . . . . . . . . . 38Valutazione dell’orientamento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39Valutazione dei descrittori . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

Descrizione dell’algoritmo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40Estrazione ed accoppiamento tramite SIFT . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41Mappatura stereo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41Stima della rototraslazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42


6 Risultati sperimentali e confronti 45Risultati sperimentali . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47

ICP standard . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47Inverse ICP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50ICP Frame Based . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51Color Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

Confronto sull’errore di allineamento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56Confronto sulla complessita computazionale . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

Allocazione di memoria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57Risorse temporali . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

Conclusioni 61

Appendice A - Complessita Computazionale 63

Appendice B - Codici sorgente 67Inverse ICP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67Frame Based ICP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78Color Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93

Bibliografia

2 INDICE

Introduzione

Nel presente lavoro di tesi si vuole affrontare il problema della stima della traiettoria imposta ad un sensore3D in grado di acquisire immagini di profondita (come un comune Kinect) ed immagini a colori.

La traiettoria viene stimata in un sistema di riferimento che ha origine nella posizione iniziale del sensoree viene costruita attraverso la stima delle rototraslazioni effettuate tra immagini consecutive di una stessaregistrazione.

Questo tipo di problemi (ricostruzione della scena circostante e localizzazione del dispositivo di acqui-sizione) e risolto attraverso tecniche generalmente conosciute come SLAM (Simultaneous Localization AndTracking). Molti di questi metodi sono computazionalmente onerosi e vengono utilizzati per una ricostruzioneoff-line.

Allo stato dell’arte i metodi piu leggeri si basano sull’algoritmo Iterative Closest Point (ICP, Capitolo2): puo essere utilizzato sia per la ricostruzione di una scena (o di un oggetto acquisito mediante scansioni3D) stimando la rototraslazione presente tra due acquisizioni, sia per valutare la traiettoria effettuata da unoggetto in movimento (compreso il sensore stesso).

Questo algoritmo e stato utilizzato come base per i confronti dei metodi sviluppati.Un’interessante evoluzione del metodo e data dall’utilizzo delle coordinate espresse in profondita inversa

(Inverse ICP, Capitolo 3), che dimostra un notevole miglioramento rispetto a ICP in termini di precisionedel risultato e di tempo d’esecuzione.

La stima della traiettoria e in generale utilizzata nella robotica mobile. Nel campo dell’odometria adesempio un robot ha il compito di stimare la traiettoria seguita per porre correzioni attraverso una retefeedback oppure conoscere la propria posizione attuale rispetto al punto iniziale. Il lavoro compiuto e natodall’idea di poter pilotare il movimento di un braccio meccanico semplicemente muovendo manualmente unacamera 3D.

Nel progetto sono stati sviluppati due metodi per la stima della rototraslazione tra due acquisizioni:Frame Based (Capitolo 4) e Color Fusion(Capitolo 5).

Frame Based sfrutta l’informazione intrinseca di un’immagine di profondita sulla vicinanza tra i puntitridimensionali acquisiti.

Color Fusion sfrutta l’informazione dell’immagine a colori e si basa sul metodo SIFT per riconosce-re lo stesso punto in due acquisizioni consecutive (operazione molto delicata per la corretta stima dellarototraslazione).

Nel presente lavoro viene dapprima illustrato il formalismo teorico alla base del funzionamento di unafotocamera di acquisizione.

Successivamente vengono illustrati da un punto di vista algoritmico ICP, Inverse ICP ed i due metodisviluppati.

Il sesto capitolo riporta i risultati sperimentali dell’esecuzione di questi algoritmi su una sequenza di ac-quisizioni. Il confronto vuole essere quantitativo nei termini dell’errore ottenuto ma qualitativo nei terminidell’onerosita di ogni metodo. Non si sono quindi effettuate misure legate al tempo di esecuzione ma piut-tosto e stata valutata la complessita computazionale (Appendice A) di ogni singolo metodo, lasciando adun’eventuale nuova implementazione l’obiettivo dell’efficienza temporale.

Escluso il vincolo temporale, tutti i sorgenti sono stati sviluppati in Matlab sfruttando una versione diICP di riferimento per gli articoli visionati.

Nell’ultimo capitolo si traggono alcune conclusioni e si suggeriscono gli sviluppi futuri basati sulle nuovetecniche presentate.

3

4 INDICE

Capitolo 1

Formazione dell’immagine 3D dasensore a luce strutturata

I sensori a luce strutturata

Un sensore a luce strutturata e un dispositivo che permette l’acquisizione di informazioni relative allaprofondita degli oggetti presenti in una scena.

Tale dispositivo si basa sulla proiezione di uno schema ottico noto sull’area coperta dalla proiezione esulla rilevazione, mediante una seconda camera, delle distorsioni dello schema stesso causate dalla presenzadi profondita diverse.

Lo schema proiettato si rifa principalmente ad una delle seguenti combinazioni:

righe monodirezionali: ripetizione di linee orizzontali o verticali

copertura puntiforme: proiezione di un insieme di punti di cui e nota la posizione originale

Esempi di proiezioni sono illustrati nelle seguenti figure:

Figura 1.1: Schema di proiezione: a sinistra un tema monodirezionale, a destra una copertura puntiforme.

La luce proiettata puo appartenere sia ad una banda cromatica appartenente al cosiddetto “spettro delvisibile” (dai 380 ai 760 nm) sia allo spettro infrarosso (da 700 nm a 1mm).

Come illustrato successivamente, la distorsione subita dallo schema viene rilevata da una seconda camerae fornisce informazioni riguardanti la distanza degli oggetti presenti nella scena rispetto al sensore.

La tecnologia utilizzata nel presente lavoro di tesi utilizza la proiezione di una struttura puntiforme.

I successivi paragrafi mostreranno come acquisire informazioni sulle coordinate spaziali dei punti dellascena a partire dallo schema proiettato.

5

6 CAPITOLO 1. FORMAZIONE DELL’IMMAGINE 3D DA SENSORE A LUCE STRUTTURATA

Modelli di riferimento

Camera stenopeica

Il sistema di ricezione segue il modello di una camera pinhole (o camera stenopeica), ovvero un contenitoreimmaginario sulla cui faccia anteriore e presente un foro infinitesimale (foro stenopeico) attraversato dai raggidi luce riflessi dagli oggetti inquadrati. Tali raggi vengono proiettati all’interno del contenitore sulla facciaposteriore (piano immagine), riproducendo l’immagine in modo speculare.

Figura 1.2: Modello di camera stenopeica: il punto M dello spazio tridimensionale viene proiettato nel puntoM ′ del piano immagine.

La distanza f tra il piano immagine ed il piano su cui e presente il foro prende il nome di lunghezza focale.Siano (X,Y, Z) le coordinate spaziali del generico punto M e (U, V, Z ′) le coordinate del punto M ′,

proiezione di M nel piano immagine. Sia posta l’origine del sistema coincidente con il foro stenopeico,orientato come in figura. Allora la relazione tra la posizione dei punti dell’oggetto nel mondo reale e lerelative proiezioni nel piano immagine segue la similitudine dei triangoli:

U

f=X

ZeV

f=Y

Zovvero:

U =fX

Ze V =

fY

Z(1.1)

In un modello piu accurato il foro stenopeico (irrealizzabile nella realta) viene sostituito da una lentesottile: l’uso di una lente permette di raccogliere piu luce dalla scena, ma introduce il problema della sfocaturadell’immagine.

Una lente sottile si puo modellizzare come in figura:

Figura 1.3: Modello di lente sottile

7

dove C e il centro ottico, F e il fuoco (ovvero il punto per cui vengono deviati tutti i raggi di luce paralleliall’asse ottico) e D e la distanza focale. Alla distanza Z ′ il raggio di luce parallelo all’asse ottico (passante peril fuoco) incide il raggio di luce passante per il centro ottico: se in questa posizione si trovasse una superficie, ilpunto raccolto sarebbe l’esatta proiezione di M e si direbbe a fuoco. Tuttavia il piano immagine puo trovarsiad una distanza dal centro ottico diversa da Z ′: in questo caso la luce riflessa da M inciderebbe su un’areacircolare del piano detto cerchio di confusione. Finche il cerchio di confusione ha dimensioni relativamentepiccole, il punto M a distanza Z si puo ancora considerare a fuoco.

Data la distanza tra il centro ottico ed il piano immagine (assimilabile alla lunghezza focale f), la distanzaZ entro cui un oggetto (ovvero l’insieme dei suoi punti) risulta a fuoco nel piano immagine viene dettaprofondita di campo, inversamente proporzionale al diametro della lente.

Il punto di intersezione tra l’asse ottico ed il piano immagine viene detto punto principale.Infine, per ottenere un modello che meglio approssima un sistema reale, il piano immagine viene sostituito

da una matrice di celle fotosensibili, nella realta realizzata attraverso un sensore CCD o un CMOS.I raggi riflessi dalla lente sottile colpiscono una o piu celle della matrice con una relativa intensita luminosa.

Ad un determinato numero di celle confinanti corrisponde un singolo pixel nell’immagine digitale: l’intensitaluminosa di un pixel e il risultato della combinazione delle intensita luminose delle celle che lo compongono.L’area (tipicamente rettangolare) fisicamente occupata dalle celle relative ad un pixel e detta dimensioneefficace del pixel.

Rappresentazione matriciale

Le grandezze precedentemente introdotte possono essere legate tra loro da un sistema lineare.Dato un punto M nel sistema di riferimento mondo di coordinate(XM , YM , ZM ), sia m la sua proiezione

nel piano immagine avente coordinate (um, vm). Dalla 1.1 e possibile ricavare la seguente relazione:

ZM

umvm1

=

−f 0 0 00 −f 0 00 0 1 0

·XM

YMZM1

(1.2)

ovvero, posto m in coordinate omogeneez ·m = P ·Mdove P viene chiamata matrice di proiezione prospettica.In generale il centro ottico della telecamera e traslato rispetto all’origine del piano immagine di un vettore

(u0, v0), le cui componenti sono le coordinate del punto principale. Considerando inoltre le dimensioni efficacidi un pixel (ku, kv) lungo le direzioni u e v si puo scrivere il seguente sistema affine:

ZM

umvm1

=

−fku 0 u0 00 −fkv v0 00 0 1 0

·XM

YMZM1

(1.3)

Le grandezze presenti in questa forma semplificata della matrice di proiezione prospettica prendono ilnome di parametri intrinseci.

Una descrizione completa della relazione tra i punti della scena reale e le loro proiezioni nel piano immagineintroduce una componente rototraslazionale tra il sistema di riferimento della telecamera ed il sistema diriferimento del mondo reale.

Sia SM il sistema di riferimento mondo con origine nel centro ottico e sia M un generico punto di SM dicoordinate (XM , YM , ZM ).

Sia SC il sistema di riferimento della camera, generalmente rototraslato rispetto a SM attraverso unamatrice[R|T ], e sia m un punto di SC di coordinate (um, vm).

Allora si puo riscrivere la 1.3:

ZM

umvm1

=

−fku 0 u0 00 −fkv v0 00 0 1 0

· [R T0 1

]·

XM

YMZM1

= [KI |0] ·[R T0 1

]·

XM

YMZM1

(1.4)


Le componenti di [R|T ] prendono il nome di parametri estrinseci, mentre i parametri intrinseci vengonoinclusi nella matrice

KI =

−fku 0 u00 −fkv v00 0 1

La matrice di proiezione prospettica e data quindi da

KI ·[R T0 1

]

Calcolo delle coordinate spaziali da copertura puntiforme

La costruzione della nuvola di punti a partire dallo schema infrarosso proiettato segue il flusso qui rappre-sentato:

Figura 1.4: Schema di costruzione di una nuvola di punti dall’acquisizione dal sensore

9

L’immagine acquisita viene elaborata utilizzando un’immagine di riferimento memorizzata nel sistema infase di costruzione e parametri ottenuti dalla calibrazione delle camere. Il risultato e una nuvola di puntinello spazio euclideo. Come si vedra successivamente, la nuvola di punti potra essere anche costruita a partiredall’immagine di disparita.

I risultati presenti in questo capitolo fanno riferimento ad un’operazione di reverse engineering presentein [2],[3] e [4].

Dallo schema infrarosso all’immagine di disparita

Il primo passo nel calcolo degli oggetti presenti in una scena rispetto al sensore richiede la costruzione diun’immagine detta di disparita.

La disparita e una particolare misura di scostamento tra due immagini: l’immagine ad infrarossi acquisita(I(u, v)) ed un’immagine di riferimento (R(u, v)). L’immagine di riferimento e una particolare acquisizionememorizzata nel sensore (presumibilmente in una memoria ROM) effettuata ad una data distanza z0 dauna superficie piana ortogonale all’asse ottico del sensore. Per il dispositivo utilizzato z0 e stato stimato a30cm. Le seguenti figure mostrano rispettivamente un’acquisizione a 30cm di due superfici piane confinanti eortogonali all’asse ottico del sensore ed un’acquisizione in cui uno dei due piani e stato allontanato di qualchecentimetro.

Figura 1.5: Riproduzione dell’immagine di riferimento (A) e modifica della profondita di una parte dellascena (B)

La struttura della luce presente in R(u, v) e tale per cui, data una finestra f(u, v) di n × n pixel (con nmolto inferiore alle dimensioni dell’immagine), esiste un intorno If (u, v) di dimensione m× h (con m > n edh > n) in cui la correlazione bidimensionale tra f(u, v) e le sottofinestre di If (u, v) e massima al centro diIf (u, v) e diminuisce nelle zone piu distanti. Data la coppia di immagini I(u, v) ed R(u, v), per ogni finestraf(u′, v′)⊂R(u, v) centrata in (u′, v′) vengono calcolati i valori di piu correlazioni bidimensionali tra f(u′, v′)e le sottofinestre contenute in un intorno di I(u′, v′) di dimensione m×h. Il valore di correlazione piu elevatoidentifica la nuova posizione (u′′, v′′) in cui e stato proiettato f(u′, v′).

La distanza tra (u′, v′) ed (u′′, v′′) definisce il valore di disparita del pixel (u′′, v′′).

Nelle seguenti figure e evidenziata una finestra f(u, v) in R(u, v), la relativa finestra in I(u, v) con valoredi correlazione piu elevato e l’immagine di disparita ottenuta.


(a) Finestra f(u, v) nell’immagine di riferimen-to.

(b) Finestra con valore di correlazionemaggiore in I(u, v)

(c) Immagine di disparita ottenuta: la zonarossa indica la disparita nulla (regione a di-stanza z0), la zona blu indica un valore didisparita molto basso

Il valore di disparita cosı ottenuto per ogni pixel non e direttamente accessibile: il sensore restituisce unvalore normalizzato d

′a 11 bit (da 0 a 2047) d = qd′ + p.

Dall’immagine di disparita all’immagine di profondita

Data una matrice con valori di disparita, attraverso alcuni parametri tipici del sensore e possibile calcolarela distanza del punti proiettati nell’immagine.

Per convenzione si assume il punto centrale tra il proiettore ed il ricevitore come punto dal quale calcolarele distanze delle superfici presenti nella scena inquadrata.

La conversione tra disparita e profondita richiede la modellizzazione della camera ricevente attraverso unacamera di pinehole (o stenopeica), avente lunghezza focale f .

Tra il centro ottico della camera cosı modellata ed il centro ottico del proiettore di luce strutturata vieneconsiderata la distanza b (baseline).

Dato il valore di disparita d e nota la distanza di acquisizione del frame di riferimento z0, il calcolo dellaprofondita z di un pixel si serve delle seguenti equazioni provenienti dalla geometria euclidea:

D

b=z0 − zz0

(1.5)

11

d

f=D

z(1.6)

dove D rappresenta la lunghezza del segmento che congiunge i due punti di intersezione tra il piano idealeposto a z0, il raggio proiettato ed il raggio riflesso dalla superficie, posta a distanza z, che determinano ilvalore di disparita d.

I valori introdotti sono rappresentati in figura:

Figura 1.6: Rappresentazione schematica dei parametri utilizzati nella conversione tra immagine di disparitaed immagine di profondita

Dalle due equazioni introdotte viene ricavata la seguente formula per il calcolo della profondita in unpunto dato il valore di disparita d:

z =z0

1 + z0fbd

(1.7)

Per chiarezza ed attinenza al modello introdotto, e necessario specificare che la disparita e la profonditasi riferiscono al pixel centrale (u′, v′) della finestra f(u′, v′) con la quale viene effettuata la correlazionebidimensionale.

Questo primo procedimento per il calcolo della profondita utilizza il valore di disparita d che e disponibilesolo all’interno del sensore.

Per il calcolo attraverso un valore normalizzato d′ restituito dal dispositivo, si richiede la sostituzione delvalore di disparita con l’affine valore normalizzato.

Passando all’inverso della profondita, la nuova formula assume una forma lineare di cui e possibileevidenziare i coefficienti caratteristici K1 e K2:

1

z=

q

fbd′ +

(1

z0+

p

fb

)= K1d

′ +K2 (1.8)

Questi coefficienti sono tipici di ogni sensore e sono stati stimati sperimentalmente nel progetto.La procedura di stima e stata effettuata nel seguente modo:

si pone un piano (e stata utilizzata una tavola di legno) perpendicolare al raggio ottico del sensore aduna distanza “lontana” (per il sensore utilizzato si e posta la tavola a 3m dal sensore, pari alla distanzamassima di acquisizione garantita dal produttore)

si acquisisce l’immagine di disparita

si associa l’inverso della distanza al valore di disparita del pixel centrale alla tavola

si ripetono le acquisizioni per distanze minori del piano (nel progetto si sono acquisite 10 immagini incui la tavola veniva avvicinata di 30 cm per acquisizione)

al termine viene calcolata la regressione lineare delle misurazioni


Il grafico ottenuto e il seguente:

Figura 1.7: Retta di regressione lineare tra le misure di disparita relative a distanze crescenti.

Dall’immagine di profondita alla nuvola di punti

Un ultimo importante oggetto utilizzato nel presente progetto e la nuvola di punti, ovvero un’insieme nonordinato di vettori, ognuno dei quali rappresenta un punto in uno spazio a tre dimensioni.

Dall’ultima equazione introdotta e possibile calcolare una nuvola di punti in uno spazio in cui coordinatesono relative all’inverso della distanza.

Piu precisamente, dato un sistema di riferimento con origine nel centro ottico della camera ricevente edate le coordinate euclidee di un generico punto P (XP , YP , ZP ), le prime due componenti del vettore delrelativo punto Pinv(Uinv, Vinv, Zinv) espresso nello spazio di profondita inversa rappresentano i seni di dueangoli e la terza e l’inverso della distanza ZP , come descritto dalle relazioni:

uinv = sin (α) = sin(

(xp, yp, zp) (0, yp, zp))

vinv = sin (β) = sin(

(0, yp, 0) (0, yp, zp))

zinv = 1zp

13

Figura 1.8: Punti e angoli notevoli per la costruzione delle coordinate di profondita inversa.

Anche se riferiti a coordinate nello spazio euclideo, le coordinate di profondita inversa vengono calcolateattraverso i parametri intrinseci secondo la seguente relazione:

(uinv, vinv, zinv) =

(i− u0fx

,j − v0fy

,1

z

)dove (i, j) sono le coordinate del pixel, (u0, v0) sono le coordinate del punto principale dell’immagine,

(fx, fy) sono le dimensioni efficaci dei pixel lungo le due direzioni moltiplicate per la lunghezza focale el’inverso di z e calcolato applicando la regressione lineare su d′(i, j).

La nuvola di punti nello spazio euclideo si puo ricavare da quella in profondita inversa moltiplicando ognivalore per l’inverso della terza componente.

Capitolo 2

L’algoritmo Iterative Closest Point(ICP)

L’algoritmo Iterative Closest Point e stato proposto per la prima volta in [5]. L’obiettivo proposto era laricostruzione in ambiente virtuale di un oggetto fisico di cui erano note piu nuvole di punti appartenenti adinquadrature acquisite da punti di vista differenti.

Il termine ICP venne introdotto ufficialmente in [6], in cui viene presentato uno studio piu approfonditodel metodo e della sua convergenza.

Date due nuvole di punti con un sottoinsieme comune, si vuole stimare la miglior rototraslazione chesovrappone i punti comuni a entrambe minimizzando iterativamente un particolare funzionale d’errore. Lamodalita di accoppiamento dei punti tra due nuvole, il modello di rototraslazione adottato, il numero diiterazioni eseguite e la scelta del funzionale d’errore sono i punti essenziali per la costruzione dell’algoritmo.

Alcune varianti proposte in [7] confrontano diverse forme e combinazioni di questi parametri, volendofornire un metodo generalmente migliore per velocita e qualita del risultato.

Descrizione dell’algoritmo

Il metodo generale prende in ingresso due nuvole di punti N1(nuvola sorgente) ed N2(nuvola di destinazione,di cui almeno un sottoinsieme rappresenta la stessa regione dello spazio di N1) legate da una non notarototraslazione [R|T ]1 della camera di acquisizione (approssimabile ad una camera stenopeica).

La stima di [R|T ] avviene con i seguenti passi:

1. Selezione dei punti di N1: dalla prima nuvola di punti viene selezionato un sottoinsieme di punti Ns1

2. Accoppiamento: per ogni punto diNs1 viene identificato un punto inN2 che ne rappresenti il rototraslato

3. Pesatura e rigetto: ad ogni coppia di punti viene associato un peso che ne descrive la “qualita”; alcunecoppie che non rispettano determinate caratteristiche vengono poi escluse dal calcolo

4. Minimizzazione dell’errore: minimizzando un’opportuna funzione di errore di allineamento si ottieneuna stima di [R|T ]

5. Aggiornamento della nuvola di punti sorgente, imponendo la rototraslazione stimata

1Diversamente dalla matrice di traslazione T (composta dalle sole tre componenti traslazionali lungo le direzioni dello spazio),la matrice di rotazione R puo seguire diversi modelli. La descrizione piu frequente fa uso della terna di angoli di Eulero (θx, θy , θz)ed assume la forma

R =

cosθzcosθy cosθxsinθycosθz − sinθzcosθx cosθzsinθycosθx + sinθzsinθxsinθzcosθy sinθzsinθysinθx + cosθzcosθx sinθzsinθycosθx − cosθzsinθx−sinθy cosθysinθx cosθycosθx

15

16 CAPITOLO 2. L’ALGORITMO ITERATIVE CLOSEST POINT (ICP)

6. Iterazione: se non si e raggiunta una certa soglia di tolleranza dell’errore o se non e stato eseguito unnumero prefissato di iterazioni, si ripete dal punto 2.

Di seguito e riportato un esempio di pseudocodice dell’algoritmo:

1 FUNCTION ICP(originalPC, destinationPC, maxIteration, minTresh)2

3 PCsrc = subsample(originalPC);4 k = 1;5 tresh = minTresh + 1;6

7 WHILE(k≤maxIteration AND tresh > minTresh)8

9 pairs = match(PCsrc, destinationPC);10 w = weight(pairs);11 goodPairs = reject(pairs);12

13 [R |T] = minimizeError(goodPairs, w);14

15 PCsrc = R*PCsrc + T;16 tresh = evaluateDistance(PCsrc, destinationPC);17

18 k=k+1;19

20 LOOP21

22 RETURN [R |T]23

24 END FUNCTION

Si noti che nel caso in cui siano disponibili solo le immagini di disparita o di profondita, per invocare ilmetodo e necessario costruire le nuvole di punti seguendo le formule presentate nel capitolo 1.

Nella parte seguente del capitolo vengono illustrate piu in dettaglio queste fasi.

Selezione

Nella fase di selezione viene estratto un campione di punti dalla nuvola sorgente.

Un primo criterio di selezione e il campionamento uniforme: N1 viene suddivisa in sotto regioni divolume identico e da ogni regione si estrae un punto, se presente. Questo metodo garantisce l’analisidi tutte le aree della nuvola sorgente, diminuendo la probabilita di esclusione di zone particolarmentecritiche per un corretto allineamento.

Il criterio di selezione piu diffuso e il campionamento casuale: da N1 viene estratta casualmente unapiccola percentuale di punti (solitamente al di sotto del 10%). Se, per ipotesi, la maggioranza dei puntidella nuvola sorgente compare (rototraslata) anche nella nuvola di destinazione, questo metodo forniscealle fasi successive punti che possono essere correttamente accoppiati.

Una selezione piu stringente si basa sulla distribuzione delle normali : vengono individuati in N1 queipunti il cui intorno (inteso come superficie) contiene piu normali con angolazioni differenti. Questastrategia sottocampiona punti considerati “notevoli” per la variabilita della superficie in cui sono con-tenuti (ad esempio spigoli o angoli). Nel caso di superfici completamente piane, tuttavia, e possibileottenere un sottocampione vuoto.

Un ultimo criterio di estrazione seleziona semplicemente tutti i punti della nuvola sorgente. In questomodo si ha la certezza che tutti i punti “notevoli” vengono valutati dall’algoritmo. Tuttavia l’errore diallineamento stimato aumenta proporzionalmente al numero di punti presenti nella nuvola sorgente enon inclusi dalla nuvola di destinazione. Inoltre l’accoppiamento e la pesatura richiedono una quantitadi tempo maggiore rispetto agli altri tipi di selezione.

17

I risultati sperimentali in [7] mostrano che per nuvole di punti molto regolari nessuna tecnica e particolarmenterilevante, escludendo l’utilizzo del campionamento per distribuzione delle normali.

Per nuvole di punti variabili o altamente irregolari (contenenti ad esempio un unico picco su una superficiepianeggiante), il campionamento per distribuzione delle normali fornisce i punti migliori sul quale eseguirele fasi successive.

In generale, per garantire un tempo di esecuzione minore, si preferisce l’utilizzo del campionamentocasuale, che non richiede ulteriori calcoli e che nel caso medio ha piu probabilita di selezionare punti efficaciper l’accoppiamento.

Accoppiamento

Nella fase di accoppiamento, per ogni punto n1 sottocampionato dalla nuvola sorgente viene individuato unpunto n2 nella nuvola di destinazione che ne rappresenti verosimilmente il rototraslato.

Il primo criterio associa il punto di N2 spazialmente piu vicino ad n1 (secondo la metrica euclidea).Questa scelta puo essere applicata su qualsiasi coppia di nuvole di punti, indipendentemente dalla lorodensita.

Un secondo metodo seleziona il punto che si incontra proiettando in N2 la normale con origine inn1

2. Tale approccio richiede che N2 sia talmente densa da contenere sempre un punto che interseca laproiezione della normale: in generale tuttavia le nuvole di punti acquisite da un sensore non rispettanoquesto requisito.

In [8] viene proposto un insieme delle due tecniche precedenti, estendendo lo sfruttamento della normaleanche alle nuvole di punti meno dense. La ricerca viene effettuata in direzione della normale ad n1,selezionando il punto in N2 piu vicino, la cui normale non differisca di piu di 45° dalla normale in n2.

Un approccio piu recente abbina una delle tecniche indicate con l’informazione sui colori presenti nellascena. In questo modo la ricerca viene ristretta ai soli punti di tonalita simile ad n1.

La ricerca del punto di N2 piu vicino ad n1 puo essere accelerata mediante l’utilizzo di un k-d tree3.

I risultati sperimentali in [7] mostrano che per nuvole di punti relativamente omogenee gli accoppiamentipiu corretti si hanno seguendo gli approcci che si basano sulle normali: la scelta del mero punto piu vicinogenera un errore di accoppiamento maggiore.

Tuttavia nelle nuvole di punti con variabilita maggiore solo la scelta del punto piu vicino garantisce unerrore di allineamento accettabile ed una convergenza del metodo alla corretta soluzione. Questo criterioviene in generale considerato il piu robusto in caso di superfici irregolari.

Poiche ICP vuole essere un algoritmo relativamente veloce nel raggiungere la convergenza al minimolocale, oltre all’errore di accoppiamento deve essere valutato anche il tempo richiesto da questa fase.

Sebbene gli approcci che prevedono una proiezione non garantiscano una convergenza rapida nel casomedio, ogni accoppiamento richiede un tempo inferiore rispetto alla selezione del punto piu vicino.

La tecnica basata sulle normali richiede un tempo costante, aggravato pero dal calcolo della normalestessa.

L’utilizzo di un k-d tree richiede un tempo proporzionale alla cardinalita di N2 per la costruzione del-l’albero (richiesta solo alla prima iterazione), ma garantisce maggiori prestazioni nella fase di ricerca di unpunto vicino.

In generale si preferisce l’utilizzo del punto piu vicino, poiche non richiede ulteriori computazioni oltre aquelle necessarie alla ricerca, che diventano comunque accettabili con l’utilizzo di un k-d tree.

2Con “normale con origine in un punto” si intende il vettore normale alla superficie contenuta nell’inviluppo convesso delpunto stesso e dei suoi punti piu vicini.

3Un “k-d tree” e una struttura dati ad albero k-dimensionale che, data una metrica, costruisce sottopartizioni di un insiemein base alle diverse regioni dello spazio. Ogni nodo dell’albero riferisce a una sottoregione ed ogni foglia contiene un insiemedi punti che, per cardinalita, non si vuole suddividere in ulteriori regioni. Questo modello e stato proposto in [9] e garantiscecomplessita di ricerca dell’ordine di O(logn) nel caso medio.


Pesatura e rigetto

Attraverso la fase di pesatura e rigetto e possibile escludere dalla stima della rototraslazione le coppieconsiderate “errate”, o almeno diminuirne il contributo.

L’assegnazione dei pesi piu banale e fatta mediante un peso costante: in questo modo ogni coppia portalo stesso contributo alla fase di minimizzazione.

Un secondo approccio assegna alle coppie un peso proporzionale alla distanza tra i due punti: le coppiei cui punti distano maggiormente daranno un contributo minore alla fase successiva. La funzione dipesatura puo seguire il modello:

w = 1− Dist(p1, p2)

Distmax

Un terzo approccio si basa sulla “compatibilita” delle normali:

w = n1 · n2

Se le normali non sono state gia calcolate nella fase di accoppiamento, questo metodo aumenta il tempodi esecuzione dell’algoritmo dovuto alla stima delle normali ai punti.

La fase di rigetto puo essere evitata se si e utilizzata una pesatura molto drastica, mediante la quale allecoppie da escludere si associa un peso molto basso.

Data una soglia, un primo criterio di rigetto esclude le coppie i cui punti distano piu della soglia fornita

Un secondo approccio elimina una certa percentuale di punti, ordinati secondo una certa metrica(solitamente tramite la distanza euclidea)

Se si vuole che il rigetto si adatti al particolare insieme di coppie da elaborare, e possibile calcolare ladeviazione standard σ delle distanze dei punti e rigettare le coppie i cui punti distano piu di un certomultiplo di σ. In questo modo il rigetto e adattativo e nel caso migliore mantiene tutte le coppie efficacialla stima della rototraslazione. Nel caso peggiore, ovvero in presenza di una variabilita molto elevatadella distanza delle coppie, e possibile che solo pochi punti sopravvivano al rigetto.

Un ulteriore metodo adattativo esclude le coppie i cui punti distano “piu del solito” rispetto alle coppievicine. Date due corrispondenze (p1, q1) e (p2, q2), dove la seconda e una coppia “vicina” alla prima, edato un coefficiente k ∈ [0, 1], si mantengono le coppie che per ogni vicino soddisfano:

|Dist(p1, q1)−Dist(p2, q2)| ≤ k ·max (Dist(p1, q1), Dist(p2, q2))

Un ultimo criterio (che puo essere combinato coi precedenti) esclude semplicemente i punti situati suangoli e spigoli

I risultati sperimentali in [7] mostrano che in generale nessun meccanismo di pesatura e preferibile agli altri inquanto il peso sembra non influire particolarmente sulla rapidita di convergenza. Tuttavia e lecito supporreche una buona pesatura possa fornire contributi che portino ICP ad un minimo locale migliore rispetto aduna pesatura costante.

Per quanto riguarda il rigetto, l’esclusione aggressiva e non adattativa di un certo numero di punti (con-siderati outlier per la minimizzazione) influisce negativamente sulla rapidita di convergenza, aumentando ilnumero di iterazioni necessarie per raggiungere un minimo locale. Tuttavia il minimo raggiunto in questomodo si mostra piu stabile e piu vicino al minimo assoluto.

19

Stima delle normali

Nel caso la metrica d’errore selezionata sia punto-piano, e necessario fornire al metodo le componenti delvettore normale alla superficie nel punto n1 campionato dalla nuvola di punti sorgente.

Il calcolo della superficie normale puo essere effettuato attraverso l’analisi delle componenti principali(PCA).

Il metodo PCA e stato ideato agli inizi del 1900 da Pearson e poi sviluppato da Hotelling ed estraele direzioni degli autovettori di una particolare matrice costruita dai dati per minimizzarne la ridondanzaattraverso un cambiamento di base.

Minimizzazione

ICP richiede la definizione di un funzionale che stimi l’errore di allineamento tra due nuvole di punti. L’erroresara calcolato tra una rototraslazione della nuvola sorgente (o un campione di essa) ed i relativi puntiaccoppiati nella nuvola di destinazione.

In quanto problema di ottimizzazione, e bene scegliere un funzionale che permetta una minimizzazioneattraverso una forma chiusa (escludendo la necessita di nuove iterazioni per trovare il punto di minimo).Si adotta in generale un funzionale di tipo quadratico, i cui punti di minimo sono ottenibili attraversol’applicazione del metodo dei minimi quadrati4.

Le due metriche principali di minimizzazione sono la metrica punto-punto e la metrica punto-piano.La metrica punto-punto minimizza la somma dei quadrati delle distanze nelle singole coppie di punti

rispetto alle incognite di rototraslazione. L’errore e dato da:

Ept−pt =∑∥∥[R|T ] · pi − qi

∥∥2La metrica punto-piano minimizza la somma dei quadrati delle distanze tra i punti di N1ed i piani tangenti

ai punti associati in N2. La formula puo essere espressa in:

Ept−pl =∑

d2s ([R|T ] · pi, Sqi) =∑

[([R|T ] · pi − qi) · ηi]2

dove d2s(q, S) rappresenta la distanza di un punto q dal piano S, Sqi rappresenta il piano tangente a qi eηi e il versore normale alla superficie Sqi .

La minimizzazione di questi funzionali puo avvalersi di una opportuna rappresentazione matriciale. Tut-tavia la presenza di termini non lineari in [R|T ] rende non lineare il sistema ottenuto. Questo problema puoessere aggirato in tre modi:

1. Ogni termine della matrice R viene considerato un’incognita indipendente dagli altri termini, facendopassare da 6 a 12 le incognite del sistema

2. Si utilizzano i minimi quadrati non lineari : data una soluzione a priori (solitamente il vettore nullo),si itera una discesa lungo il gradiente della funzione da minimizzare

3. In caso di angoli piccoli, inferiori a 0.17 rad, si approssimano i seni a 0 ed i coseni a 1, ottenendo unsistema completamente lineare

I risultati sperimentali suggeriscono l’utilizzo della metrica punto-piano, che si dimostra essere piu robustarispetto agli errori di acquisizione ed agli accoppiamenti errati. Questo funzionale converge piu rapidamentead un minimo locale.

Per superfici molto regolari, tuttavia, le due metriche si comportano in maniera simile.Il vantaggio nella scelta della metrica punto-punto si ha nel minor tempo di computazione (che altrimenti

richiederebbe il calcolo delle normali).

4Dato un sistema lineare Ax = b, dove A ∈ Rn×m e la matrice (anche non invertibile) dei coefficienti, x ∈ Rm×1 e il vettoredelle incognite e b ∈ Rn×1 e il vettore dei termini noti, si definisce soluzione ai minimi quadrati il vettore

x =(ATA

)−1AT b


Iterazione

Al termine della fase di minimizzazione viene ricalcolata una nuova nuvola di punti rototraslando N1 attra-verso [R|T ]. Questa sara utilizzata come nuvola sorgente per l’iterazione successiva.

Dopo alcune iterazioni e possibile tentare di prevedere il vettore (nelle incognite di [R|T ]) che portadirettamente al minimo locale dell’intero algoritmo.

Infatti data una terna di vettori di stato (le cui componenti sono [θx, θy, θz, tx, ty, tz]) risultanti dalleultime tre iterazioni, si puo notare un “percorso” nello spazio degli stati. Ad ogni stato, inoltre, e associatoun valore della funzione di errore (che si suppone decrescere tra uno stato ed il successivo). L’idea in [6] el’estrapolazione lineare o parabolica dell’andamento dell’errore, cercando l’intersezione tra questa curva (inuno spazio di dimensione 7) e lo spazio degli stati (di dimensione 6).

Questa “accelerazione” viene effettuata solo se il percorso nello spazio degli stati e “abbastanza regolare”.Si consideri il segmento che congiunge gli ultimi due stati e se ne calcoli l’angolazione δθ(k) rispetto allostato nullo. Si consideri il segmento che congiunge i due stati precedenti δθ(k− 1). Dato uno scarto massimoconsentito δθmax viene effettuata l’estrapolazione solo se

‖δθ(k − 1)− δθ(k)‖ ≤ δθmaxLa scelta tra linea o parabola e adattativa: se la differenza tra la piu recente coppia di errori e minore rispettoalla coppia precedente viene utilizzata l’estrapolazione parabolica, altrimenti si ricorre al metodo lienare.

La seguente figura mostra questa idea come proposta nell’articolo di riferimento:

Figura 2.1: Estrapolazione dell’errore presentata in [6]. Per semplicita lo spazio degli stati e rappresentatobidimensionalmente. In questa figura q(i) e il vettore di stato riferito all’i-esima iterazione, d(i) e l’erroreassociato e δθ(i) e l’angolazione del segmento che congiunge lo stato i-esimo con lo stato precedente. Leintersezioni tra la retta (o la parabola) e lo spazio degli stati forniscono lo stato in cui si prevede il minimolocale di ICP.

Complessita computazionale

L’analisi della complessita computazionale di ICP segue la notazione di Landau descritta in Appendice A.

Allocazione di memoria

In merito alla quantita di memoria da allocata, le nuvole di punti fornite in ingresso occupano una quantitadi risorse proporzionale a Θ (N +M), dove N e la cardinalita della nuvola sorgente ed M e la cardinalitadella nuvola di destinazione.

Il funzionale di complessita e dato da:

TICP (N,M) = N +M

21

L’eventuale costruzione di un k-d tree per la ricerca dei vicini nella nuvola di destinazione necessita di unulteriore Θ (M), che pero non influisce sulla complessita totale espressa in “theta”.

Il funzionale di complessita in questo caso e dato da

TICP (N,M) = N + 2M

Per velocizzare il calcolo delle normali nella nuvola sorgente e possibile costruire un ulteriore k-d tree efacilitare la ricerca dei punti piu vicini. Anche in questo caso la complessita totale (espressa in “theta”) noncambia, ma viene aggiunto un ulteriore termine N a TICP .

Risorse temporali

In merito all’analisi del tempo di esecuzione, e opportuno inizialmente considerare la complessita di ciascuncriterio illustrato in questo capitolo.

Per una valutazione piu efficace vengono suddivise due casistiche in cui e eseguito l’algoritmo: il “casomedio” ed il “caso peggiore”.

Questi casi hanno significato solo nell’utilizzo di un k-d tree.Il caso medio si riferisce ad una nuvola di punti in cui per ogni punto i suoi vicini si trovano in nodi vicini

dell’albero.Il caso peggiore si riferisce ad una delle seguenti situazioni:

ogni foglia dell’albero contiene un punto solo (ovvero nel caso la nuvola di punti sia estremamentesparsa e non sia possibile identificare delle regioni abbastanza piccole)

tutti i punti della nuvola risultano contenuti nella stessa foglia

I diversi approcci sono stati riportati in tabella, seguita dalla spiegazione dei caratteri utilizzati:

Criterio Caso Medio Caso Peggiore

Selezione

Campionamento Uniforme Θ (n) Θ (n)Campionamento Casuale Θ (n) Θ (n)Distribuzione di Normali(1) Θ (N) Θ (N)Selezione Completa Θ (N) Θ (N)

AccoppiamentoPunto piu vicino Θ (η ·M), [kd-tree(2):O (η · log(M))] Θ (η ·M), [kd-tree(2): O (η ·M)]Proiezione della Normale(1) Θ (η) Θ (η)Proiezione e Ricerca(1) Θ (η ·M), [kd-tree(2): O (η · log(M))] Θ (η ·M), [kd-tree(2): O (η ·M)]

PesaturaCostante - -Proporzionale alla distanza Θ (η) Θ (η)Compatibilita di Normali(1) Θ (η) Θ (η)

Rigetto

A soglia Θ (η) Θ (η)Percentuale (r) Θ (η − r · η) Θ (η − r · η)Adattativo, per varianza Θ (η) Θ (η)Adattativo, per similitudine Θ

(η2)

Θ(η2)

(in aggiunta) Angoli e spigoli Ω (∗) Ω (∗)

MinimizzazioneMinimi quadrati punto-punto Θ

(η3)

Θ(η3)

Minimi quadrati punto-piano(1) Θ(η3)

Θ(η3)

Stima delle normali con PCA Θ (η ·N), [k-d tree(2):O (η · log (N))] Θ (η ·N), [k-d tree(2):O (η ·N)]

Costruzione del k-d tree sulla nuvola sorgente(3) Θ (N) Θ (N)

Costruzione del k-d tree sulla nuvola di destinazione(3) Θ (M) Θ (M)

1 Richiede in aggiunta la stima delle normali ai punti2 Richiede la costruzione a priori di un k-d tree sulla nuvola di punti riferita3 Eseguito solo alla prima iterazione

Tabella 2.1: Complessita computazionale delle fasi di ICP

i cui simboli sono:

N : numero di punti della nuvola sorgente

M : numero di punti della nuvola di destinazione


n: numero di punti effettivamente sottocampionati dalla nuvola sorgente in fase di selezione

η: numero di punti estratti dalla nuvola sorgente in fase di selezione (pari a N o n)

r ∈ [0, 1]: frazione di punti (tra 0 e 1) da rigettare

∗: l’asintoto gia seguito diventa un asintoto inferiore

La complessita effettiva totale viene calcolata moltiplicando la complessita di un ciclo di fasi per il numerodi iterazioni complessive Imax (che puo essere impostato come parametro per l’algoritmo).

Considerando la combinazione suggerita da [7], il funzionale di complessita e dato da:

TICP (N,M) = M + Imax ·[2n+O (n · log (M)) + n3

]per il caso medio e

TICP (N,M) = M + Imax ·[2n+O (n ·M) + n3

]per il caso peggiore. Si aggiunga fattore N+O(n · log (N)) nel caso vengano stimate le normali (N+O (n ·N)nel caso peggiore).

Considerato che il numero dei punti e superiore almeno di un ordine di grandezza rispetto al numerodi iterazioni totali (Imax min (N,n, η,M)), per semplicita si puo omettere quest’ultima variabile dallacomplessita totale.

Inoltre, nel caso si utilizzi un sensore 3D per acquisire le immagini e questo non metta a disposizione lenuvole di punti, e necessaria un’elaborazione dell’ordine di Θ(N +M) per costruire le nuvole di punti.

Per qualsiasi combinazione tra criteri si scelga di adottare, si puo notare come la fase di minimizzazionerichieda comunque un ordine cubico del numero di punti campionati.

Poiche nelle implementazioni standard dell’algoritmo si utilizzano i minimi quadrati, non e possibilescendere al di sotto di tale complessita. L’unica possibilita di minimizzare la complessita si ha evitando discegliere, in fase di selezione, la selezione completa.

In definitiva, la complessita computazionale (riferita al tempo di esecuzione) dell’algoritmo ICP e dell’or-dine di Θ

(η3), vincolata dall’utilizzo del metodo dei minimi quadrati.

Capitolo 3

Stima della posizione nello spazio diprofondita inversa

L’algoritmo ICP in origine venne utilizzato per la ricostruzione di oggetti o di scene di cui era disponibile uninsieme di nuvole di punti.

Nel corso degli anni nacque l’esigenza di stimare il movimento libero di una telecamera sulla base delleimmagini acquisite. Tra le tecniche proposte piu utilizzate vi sono quelle che fanno uso di ICP per stimare larotazione e la traslazione tra due nuvole di punti consecutive (generate mediante stereovisione o attraversosensori 3D).

In [12] viene proposto un metodo basato su ICP e sullo spazio di profondita inversa che permette distimare rotazioni e traslazioni di “piccoli1” spostamenti, migliorando la robustezza del risultato attraversouna pesatura.

Sia per verificarne i risultati, sia per la necessita di utilizzare questo metodo come “termine di paragone”e stato realizzato uno script Matlab seguendo le indicazioni dell’articolo. Cio si e reso indispensabile a causadella mancata disponibilita di un codice sorgente fornito dagli autori.

In questo capitolo vengono presentate inizialmente alcune caratteristiche dello spazio di profondita inversache ne giustificano l’utilizzo nel nuovo approccio. Dopodiche viene illustrata nel dettaglio la struttura delmetodo (che segue quella di ICP) e ne viene analizzata la complessita computazionale.

Lo spazio di profondita inversa

Lo spazio di profondita inversa presentato nel capitolo 1 gode di due particolari benefici che ne rendonosignificativo l’utilizzo attraverso l’algoritmo ICP.

La piu semplice considerazione e la possibilita di ricavare i valori di profondita inversa direttamentedall’immagine di disparita: cio rende superflua un’ulteriore conversione delle coordinate nello spazio euclideo.Un punto in profondita inversa viene infatti calcolato attraverso la seguente formula

(uinv, vinv, zinv) =

(i− u0fx

,j − v0fy

,1

k1d′ + k2

)dove (i, j) sono le coordinate del pixel, (u0, v0) sono le coordinate del punto principale dell’immagine, (fx, fy)sono le dimensioni efficaci dei pixel lungo le due direzioni moltiplicate per la lunghezza focale, d′e il valoredi disparita normalizzato in posizione (i, j) e k1 e k2 sono i coefficienti della retta di regressione lineare checonverte valori di disparita in valori di profondita inversa.

La seconda considerazione riguarda la proprieta di quasi linearita dell’incertezza di misura.

L’errore di misura si assume seguire una distribuzione gaussiana a media nulla.

1Lo spostamento “piccolo” e riferito alla rotazione: nel metodo si assumono angoli di rotazione inferiori ai 10° (circa 0.17rad). La traslazione, invece, puo avere modulo arbitrario.

23

24 CAPITOLO 3. STIMA DELLA POSIZIONE NELLO SPAZIO DI PROFONDITA INVERSA

Gli studi presentati in [10] e [11] mostrano come nello spazio euclideo questa approssimazione non si possaconsiderare realistica per punti “distanti2” dal sensore. Viene infatti introdotto un coefficiente L di linearitadella funzione di conversione da punti nello spazio di riferimento a punti nel piano immagine f : R3 → R2 .Il coefficiente e calcolato sul generico errore ρ attraverso la seguente formula:

L(ρ) =

∣∣∣∣∣∣∣∂2f∂ρ2

∣∣∣ρ=0· 2σρ

∂f∂ρ

∣∣∣ρ=0

∣∣∣∣∣∣∣dove σρ e la deviazione standard dell’errore. Questo valore esprime la linearita di f rispetto all’errore

di misura, che e una proprieta essenziale per propagare mantenere valida l’ipotesi di distribuzione normaledell’incertezza anche nel piano immagine. Infatti, per L(ρ) ≈ 0 la funzione f non altera la distribuzionedell’errore per ρ ∈ [ρ− 2σρ, ρ+ 2σρ].

La funzione f assume forme diverse in relazione al tipo di coordinate utilizzate nello spazio di riferimento:i risultati degli studi mostrano come l’ipotesi L(ρ) ≈ 0 sia piu ragionevole per lo spazio di profondita inversarispetto allo spazio euclideo proprio nel caso in cui nella scena siano presenti punti distanti.

In generale la linearita di f rispetto all’errore e maggiore nello spazio di profondita inversa che nello spazioeuclideo. Cio permette di ritenere veritiera l’ipotesi di errore di misura gaussiano a media nulla indipendentedalla distanza del punto.


Il metodo richiede due prerequisiti:

1. Le nuvole di punti sono espresse nello spazio di profondita inversa

2. La componente rotazionale tra due nuvole di punti temporalmente consecutive e piccola, ovvero al disotto dei 10° (circa 0.17 rad).

La prima richiesta e stata rispettata calcolando le coordinate di profondita inversa direttamente dall’immaginedi disparita.

La seconda assunzione permette di approssimare la matrice di rototraslazione [R|T ] ponendo cos (θ) ≈ 1e sin (θ) ≈ θ:

[R T0 1

]≈

1 −θz θy txθz 1 −θx ty−θy θx 1 tz

0 0 0 1

Le fasi di questo metodo seguono lo pseudocodice riportato e verranno presentate in dettaglio.

1 FUNCTION INV ICP(originalInvPC, destinationInvPC, maxIteration, minTresh)2

3 PCsrc = subsample(originalInvPC);4 k = 1;5 tresh = minTresh + 1;6

7 WHILE(k≤maxIteration AND tresh > minTresh)8

9 pairs = match(PCsrc, destinationInvPC);10 w = weight(pairs);11 goodPairs = reject(pairs);12

13 [R |T] = minimizeError(goodPairs, w);14

2Un punto viene considerato distante quando il suo angolo di parallasse e basso tra due frame consecutivi. L’angolo diparallasse e l’angolo tra i segmenti con origine nel punto e termine nel centro ottico della camera.

25

15 PCsrc = R*PCsrc + T;16 tresh = evaluateDistance(PCsrc, destinationInvPC);17

18 k=k+1;19

20 LOOP21

22 RETURN [R |T]23

24 END FUNCTION

Si noti che l’algoritmo necessita di nuvole di punti espresse gia nello spazio di profondita inverso, chedovranno essere calcolate prima dell’invocazione a partire da nuvole di punti nello spazio euclideo o dall’im-magine di disparita.

Selezione

La nuvola di punti sorgente viene campionata attraverso una selezione casuale in modo da estrarre dai 500ai 1000 punti. Questa quantita e consigliata in [7]: campionare un numero superiore di punti non altera ilcomportamento dell’algoritmo ICP.

Nel progetto viene campionato lo 0.3% dei punti dei frame a risoluzione 640x480 ed lo 0.7% dei punti deiframe a risoluzione 320x240.

Poiche le immagini acquisite dal sensore possono essere affette da errori palesi di misura3, viene fornitala possibilita di escludere dal campionamento i punti soggetti a tali errori.

Poiche questo tipo di errore “di saturazione” si ripresenta nella stessa regione in due scene consecutive,la scelta di un punto “saturo” in fase di selezione puo migliorare la fase di accoppiamento. Tuttavia l’erroreporta un’informazione falsata sulla profondita (e quindi sulla posizione) del punto, quindi introduce un errorenel calcolo della rototraslazione effettiva.

Accoppiamento

La tecnica di accoppiamento scelta e la ricerca del punto piu vicino attraverso la costruzione di un k-d tree.Questa parte e la medesima seguita da ICP.

Pesatura e Rigetto

La pesatura segue un criterio adattativo, basato sugli errori di misura stimati nell’iterazione precedente.Il peso di una generica coppia (p, q) e definito calcolato attraverso la formula:

3In zone in cui la luce del sole e abbagliante, lungo alcuni bordi degli oggetti, su superfici particolari (come vetro o plasticamolto lucida) o in presenza di oggetti troppo vicini al sensore, la misura di disparita (e quindi di profondita o di profonditainversa) “satura” al suo massimo valore di 2047. Cio e dovuto dall’assenza o dalla mancata cattura dei raggi infrarossi riflessi inqueste zone. Un esempio e rappresentato in figura, dove le zone di colore blu scuro rappresentano i valori saturi della disparita:


w (p, q) =1

σ + ∆r2p,q

dove ∆rp,q e la distanza tra il punto p della nuvola di punti sorgente ed il punto q appartenente allanuvola di punti di destinazione e σ e la varianza di queste distanze a seguito dell’allineamento precedente.

Inoltre vengono rigettate le coppie i cui punti superano una determinata soglia di distanza, consigliata di0.01 m−1.

Stima delle normali

In alternativa all’utilizzo di PCA e possibile sfruttare la profondita inversa per stimare le normali ai punti.Un piano nello spazio euclideo e descritto dall’uguaglianza

ax+ by + cz + d = 0

che in coordinate inverse diventa

αu+ βv + γ = q

dove α = −ad , β = − bd , γ = − c

d .Dati i k punti vicini al punto scelto, la normale [α, β, γ] puo essere stimata attraverso la risoluzione ai

minimi quadrati del sistema ui,j vi,j 1...

. . ....

· · · · · · · · ·

αβγ

=

qi,j......

L · η = Q

attraverso la forma chiusa

η =(LT · L

)−1 · LT ·QPer ogni punto della nuvola e possibile pre-calcolare la matrice L: assumendo che i punti piu vicini

spazialmente siano i punti riferiti dai pixel vicini nell’immagine, per ogni posizione (i, j) del frame e possibile

individuare le coordinate dei k punti piu vicini e costruire di conseguenza L e(LT · L

)−1·LT . La costruzione diquesta matrice e indipendente dalla composizione dei frame e puo essere calcolata in un momento precedenteall’avvio del metodo. Tuttavia essendo fisse le posizioni dei vicini, e necessario campionare punti i cui vicininon saturino il valore letto dal sensore ed e quindi necessario un ulteriore controllo.

Minimizzazione

La procedura propone le due metriche di errore presentate in ICP: punto-punto e punto-piano.La minimizzazione punto-punto fa riferimento all’errore espresso dall’equazione presentata nel capitolo

precedente

Ept−pt =∑∥∥[R|T ] · pi − qi

∥∥2 =∑

([R|T ] · pi − qi)2

Viene utilizzata una particolare matrice Jacobiana, costruita attraverso le coordinate di profondita inversa.Ad ogni punto Pi = (u, v, q) viene associata la matrice

JPi=

q 0 −uq −uv 1 + u2 −v0 q −vq −1− v2 uv u0 0 −q2 −qv qu 0

Da questa viene costruita la matrice

27

J =

JP1

JP2

...JPn

che sara utilizzata per la risoluzione ai minimi quadrati (eventualmente pesati) attraverso la seguente

formula:

B =(JT ·W · J

)−1 · JTW · Ydove

B =

θxθyθztxtytz

e il vettore delle incognite

Y =

u′

1 − u1v′

1 − v1q′

1 − q1...

u′

n − unv′

n − vnq′

n − qn

e il vettore dei termini noti, ovvero le differenze tra le componenti dei punti (ui, vi, qi)

della nuvola sorgente con(u′

i, v′

i, q′

i

)della nuvola di destinazione

W e la matrice diagonale dei pesi (se calcolati), in cui compaiono i contributi delle singole componenti(ui, vi, qi)

Nella metrica punto-piano l’errore da minimizzare e il seguente:

Ept−pl =∑[(

[R|T ] · pi − p′

i

)· ηi]2

Il metodo dei minimi quadrati viene applicato attraverso la formula

B =(KT ·W ·K

)−1 ·KT ·W · Ydove

B =

θxθyθztxtytz

e il vettore delle incognite

K =

q1α1 · · · qnαnq1β1 · · · qnβnq1γ1 · · · qnγn

v1γ1 − β1 · · · vnγn − βnα1 − u1γ1 · · · αn − unγnu1β1 − v1α1 · · · unβn − vnαn

e la matrice dei coefficienti, contenente le componenti dei punti

p (ui, vi, qi) della nuvola sorgente, delle loro normali ηi (α, βi, γi) e dei punti associati p′

i

(u′

i, v′

i, q′

i

)della

nuvola di destinazione


Y =

−(p1 − p

′

1

)η1

...

−(pn − p

′

n

)ηn

e il vettore dei termini noti

Iterazione

Al termine delle fasi precedenti viene costruita la matrice [R|T ] a partire dalla soluzione ai minimi quadrati,come descritto all’inizio del capitolo.

Viene poi ricalcolata la nuvola di punti da fornire alla fase successiva imponendo la nuova rototraslazionealla nuvola di punti utilizzata nell’iterazione corrente.

La sequenza delle fasi precedenti viene iterata finche l’errore di allineamento non diminuisce sotto unasoglia prefissata ed in ogni caso l’esecuzione si tronca una volta raggiunto il massimo numero di iterazionifornito dall’utilizzatore.


L’analisi della complessita computazionale segue la struttura presentata nel capitolo precedente, suddivisa incomplessita nell’allocazione di memoria e complessita nel tempo di esecuzione.


In merito alla quantita di memoria da allocata, valgono le stesse considerazioni espresse nel capitolo prece-dente.

Le nuvole di punti fornite in ingresso occupano una quantita di risorse pari a Θ (N +M), dove N e lacardinalita della nuvola sorgente ed M e la cardinalita della nuvola di destinazione.

La costruzione di un k-d tree per la ricerca dei vicini nella nuvola di destinazione necessita di un ulterioreΘ (M), che pero non influisce sulla complessita totale espressa in “theta”.

Per velocizzare la stima delle normali nella nuvola sorgente e possibile costruire un ulteriore k-d tree efacilitare la ricerca dei punti piu vicini. Anche in questo caso la complessita totale non varia.

Il funzionale di complessita e lineare nella dimensione dell’ingresso ed e dato da:

TINV ICP (N,M) = N +M

A questo va aggiunto un eventuale termine M per la costruzione del k-d tree nella nuvola di destinazioneed un ulteriore N per un k-d tree nella nuvola sorgente.

Risorse temporali

In merito all’analisi del tempo di esecuzione, viene inizialmente riassunta la valutata di ciascuna fase,distinguendo “caso medio” dal “caso peggiore”.

29

Fase Caso Medio Caso Peggiore

Selezione Θ (n) Θ (n)

Accoppiamento(2) O (n · log (M)) O (n ·M)

Pesatura Θ (n) Θ (n)

Rigetto Θ (n) Θ (n)

MinimizzazionePunto-Punto O

(n3)

O(n3)

Punto-Piano(1) O(n3)

O(n3)

Stima delle normaliΘ (n ·N),

[k-d tree:O (n · log (N)) ]Θ (n ·N),

[k-d tree:O (n ·N) ]Costruzione del k-d treesulla nuvola sorgente(3)

Θ (N) Θ (N)

Costruzione del k-d treesulla nuvola didestinazione(3)

Θ (M) Θ (M)

1 Richiede in aggiunta la stima delle normali ai punti2 Richiede la costruzione a priori di un k-d tree sulla nuvola di punti riferita3 Eseguito solo alla prima iterazione

Tabella 3.1: Complessita computazionale di ICP nello spazio di profondita inverso

i cui simboli sono:



n: numero di punti sottocampionati dalla nuvola sorgente in fase di selezione

Un’ulteriore elaborazione non ancora considerata e la costruzione della nuvola di punti nello spazio diprofondita inversa. Questa operazione richiede una complessita dell’ordine di Θ(N +M).

Considerando la combinazione suggerita dall’articolo, il funzionale di complessita e dato da:

TINV ICP (N,M) = N + 2 ·M + Imax ·[2n+O (n · log (M)) + n3


TINV ICP (N,M) = N + 2 ·M + Imax ·[2n+O (n ·M) + n3

]per il caso peggiore. Si aggiunga fattore N+O(n · log (N)) nel caso vengano stimate le normali (N+O (n ·N)nel caso peggiore).

La complessita totale per una singola iterazione e data dalla somma delle complessita indicate ed ecomunque limitata superiormente dalla complessita del metodo dei minimi quadrati.

Il metodo appartiene quindi agli algoritmi di ordine O(n3).

Capitolo 4

Stima della posizione con approccioFrame Based

In questo progetto di tesi e stato sviluppato un metodo per la stima della rototraslazione basato sulla strutturadei frame.

Questo metodo “frame based” sfrutta una variazione dell’algoritmo ICP adattato ad operare su frametridimensionali piuttosto che su nuvole di punti.

Il particolare frame su cui si basa l’algoritmo e costruito a partire dall’immagine di disparita D(i, j).Questo frame e costituito da tre componenti bidimensionali. Ognuna di queste componenti contiene i valoridi una delle tre dimensioni dello spazio di riferimento (euclideo o di profondita inversa).

Figura 4.1: Struttura tridimensionale del frame per coordinate euclidee e coordinate di profondita inversa

Il frame F (i, j) viene costruito attraverso le seguenti formule:

F (i, j) =

ui,j =

vi,j =

qi,j =

i− u0fx

j − v0fy

k1D(i, j) + k2

in profondita inversa e

31

32 CAPITOLO 4. STIMA DELLA POSIZIONE CON APPROCCIO FRAME BASED

F (i, j) =

xi,j =

yi,j =

zi,j =

i− u0fx

· zi,jj − v0fy

· zi,j1

k1D(i, j) + k2

nello spazio euclideo, dove (i, j) sono le coordinate del pixel, (u0, v0) sono le coordinate nel piano immaginedel punto principale, (fx, fy) sono le dimensioni efficaci dei pixel lungo le due direzioni moltiplicate per lalunghezza focale e (k1, k2) sono i coefficienti di regressione della retta che trasforma i punti D (i, j) di disparitain punti di profondita inversa.

L’utilizzo di questa struttura e giustificato dall’ipotesi che punti vicini nel piano immagine siano puntivicini anche nello spazio di riferimento.

La ricerca dei punti da accoppiare e l’eventuale ricerca dei punti per la costruzione delle normali verraeffettuata in una finestra centrata nel punto selezionato. In questo modo si evita la costruzione di un k-dtree sull’intera nuvola di punti.

Questo nuovo approccio ha lo scopo di mettere a disposizione di ICP una struttura diversa dalla nuvola dipunti: non pone dunque vincoli sui criteri scelti per le diverse fasi dell’algoritmo. In fase di avvio e necessarioquindi fornire parametri di configurazione al metodo che ne specifichino:

il comportamento rispetto ai valori “saturi” di disparita: mantenerli o escluderli dal campionamento

il metodo di pesatura e di rigetto

la metrica di errore considerata

il metodo di minimizzazione applicato

il metodo di costruzione delle normali

Nel seguito del capitolo viene descritta la struttura generale del nuovo approccio e ne viene valutata lacomplessita computazionale.


Le fasi seguite dalla procedura, come gia evidenziato, sono le stesse presenti in ICP.Tuttavia il metodo differisce per il tipo di struttura dati su cui opera (il frame di disparita) che deve

essere costruito prima dell’invocazione.Lo pseudocodice del metodo e riportato di seguito.

1 FUNCTION FRAME BASED ICP(originalFrame, destinationFrame, maxIteration, winSize)2

3 FrameSrc = *subsample(origina);4 k = 1;5

6 WHILE(k≤maxIteration)7

8 pairs = *match(FrameSrc, destinationFrame, winSize);9 w = *weight(pairs);

10 goodPairs = *reject(pairs);11

12 [R |T] = *minimizeError(goodPairs, w);13

14 FrameSrc = rebuildFrame(FrameSrc, [R |T]);15

16 k=k+1;17

18 LOOP

33

19

20 RETURN [R |T]21

22 END FUNCTION

In questo codice i metodi contrassegnati dal carattere “*” indicano la possibilita di essere configuratinell’invocazione del metodo.

Le fasi vengono di seguito illustrate in dettaglio.

Selezione

La nuvola di punti sorgente viene campionata attraverso una selezione casuale in modo da estrarre dai 500ai 1000 punti.

Nel progetto viene campionato lo 0.3% dei punti dei frame a risoluzione 640x480 ed lo 0.7% dei punti deiframe a risoluzione 320x240.

Viene fornita la possibilita di escludere i punti che rendono “saturo” il valore di disparita.

Accoppiamento

La tecnica di accoppiamento scelta e la ricerca del punto piu vicino all’interno di una finestra di dimensionepredefinita.

Altezza e larghezza della finestra vengono scelte sulla base di considerazioni qualitative: nel metodosviluppato si e supposto che i punti piu vicini spazialmente al punto p(i, j) siano contenuti in una finestra didimensione 41x41 centrata in p. Cio e verificato con ragionevole certezza nel caso in cui la rototraslazionetra due frame consecutivi sia piccola.

Figura 4.2: Finestra Wp di ricerca dei vicini del punto p nel frame F

Proprio per questo motivo per le fasi seguenti e possibile scegliere anche tra gli approcci presentati nelcapitolo 3.

Pesatura e Rigetto e Minimizzazione

Come spiegato in precedenza, il criterio per la pesatura, per il rigetto, per la stima delle normali e per laminimizzazione e arbitrario. Nel progetto sono state testate tutte le combinazioni degli approcci presentatinei capitoli precedenti.


Stima delle normali

Anche il metodo di stima delle normali non e legato all’approccio frame based, tuttavia il vantaggio di operaresu una struttura a frame e evidente per la possibilita di cercare i punti vicini in una finestra delimitatanell’intorno del punto.

Iterazione

Dalla matrice [R|T ] ottenuta nei passaggi precedenti vengono ricalcolate le nuove posizioni dei punti campio-nati.

Dato il punto pk(i, j, h) ∈ R3del frame di origine all’iterazione k, il punto su cui eseguire l’iterazione alpasso k + 1 e dato da:

pk+1 =

[R T0 1

]·

ukvkqk1

in profondita inversa o

pk+1 =

[R T0 1

]·

xkykzk1

in coordinate euclidee.Questo punto dovra poi essere inserito in una nuova posizione (i

′, j′) del frame, per aggiornare la posizione

della finestra di ricerca dell’accoppiamento. La nuova posizione e data da:i′

=

j′

=

buk · fx + u0cbvk · fy + v0c

in profondita inversa o i′

=

j′

=

⌊xk

zk· fx + u0

⌋⌊ykzk· fy + v0

⌋in coordinate euclidee.Il punto pk+1 verra dunque inserito in posizione (i

′, j′).

La sequenza delle fasi precedenti viene iterata fino al raggiungimento del massimo numero di iterazionifornito dall’utilizzatore.



Sebbene la struttura dei dati su cui operare passa da una nuvola di punti ad un frame tridimensionale, ilnumero di punti su cui operare non varia

In merito alla quantita di memoria da allocata, i frame costruiti occupano una quantita di risorse propor-zionale a Θ (hF · wF ), dove hF × wF e la dimensione delle due immagini di disparita o di profondita fornitecome immagine sorgente ed immagine di destinazione.

Per velocizzare il calcolo delle normali nella nuvola sorgente e possibile costruire un ulteriore k-d tree efacilitare la ricerca dei punti piu vicini. Anche in questo caso la complessita totale (espressa in “theta”) noncambia, ma vengono aggiunti hF · wF punti all’interno della memoria.

Il funzionale di complessita e lineare nella dimensione dell’input:

TFB(hF , wF ) = 2 · hF · wF

35

Risorse temporali

In merito all’analisi del tempo di esecuzione, vengono riassunti i valori di complessita tipici delle fasi di ICP.


Selezione Θ (n) Θ (n)

Accoppiamento(2) O (n · log (hW · wW )) O (n · hW · wW )

Pesatura Θ (n) Θ (n)

Rigetto Θ (n) Θ (n)

MinimizzazionePunto-Punto O

(n3)

O(n3)

Punto-Piano(1) O(n3)

O(n3)

Stima delle normali Θ (n · hW · wW ), Θ (n · hW · wW ),

Costruzione del k-d tree sulla finestra di ricerca(3) Θ (hW · wW ) Θ (hW · wW )1 Richiede in aggiunta la stima delle normali ai punti2 Richiede la costruzione a priori di un k-d tree sulla nuvola di punti riferita3 Eseguito solo alla prima iterazione

Tabella 4.1: Complessita computazionale di ICP nello spazio di profondita inverso

i cui simboli sono:

n: numero di punti sottocampionati dal frame sorgente in fase di selezione

hW × wW : dimensioni della finestra entro cui cercare i vicini nella costruzione delle normali

La costruzione del frame tridimensionale richiede una complessita dell’ordine di Θ(hF ·wF ), dove hF ×wF ela dimensione delle due immagini di disparita o di profondita fornite come immagine sorgente ed immaginedi destinazione.

Definito N il numero di pixel hF · wF di un frame e W il numero hW · wW di pixel di una finestra diricerca, il funzionale di complessita e dato da:

TFB(N) = N + Imax ·[2n+O (n · log (W )) + n3


TFB(N) = N + Imax ·[2n+O (n ·W ) + n3

]La complessita totale e limitata superiormente dalla complessita del metodo dei minimi quadrati.Anche questo metodo appartiene quindi agli algoritmi di ordine O

(n3).

Capitolo 5

Stima della posizione con approccioColor Fusion

L’ultimo metodo sviluppato si basa sull’idea di sfruttare l’informazione disponibile dall’immagine a colori percostruire accoppiamenti validi tra i punti delle due scene acquisite.

Anche in questo approccio la struttura dati fornita all’algoritmo e un frame invece di una nuvola di punti.Per ogni scena deve essere fornito sia il frame di profondita (o di disparita), sia il frame a colori.

Tra i punti contenuti nei due frame e possibile stabilire una relazione biunivoca: dato il punto (XM , YM , ZM )

della scena reale e mappato in(u′, v′)

nell’immagine a colori e possibile ricavare le coordinate(u′′, v′′)

dello

stesso punto nell’immagine acquisita dalla camera a infrarossi e viceversa.Questa operazione e permessa dai risultati della calibrazione effettuata sulle due camere, illustrata nel

corso del capitolo.Per l’accoppiamento dei punti tra le immagini si e fatto ricorso alla tecnica SIFT, presentata da D.G.

Lowe in [13] e di seguito illustrata brevemente.Anche per questo algoritmo si suppone che la rotazione tra due frame consecutivi sia piccola, di modo

che ogni punto dell’immagine sorgente possa essere ricercato in un intorno della sua posizione originale anchenell’immagine di destinazione.

Calibrazione stereo

Come presentato nel capitolo 1, attraverso i parametri estrinseci ed intrinseci di una camera e possibilestabilire la seguente relazione tra i punti (XM , YM , ZM ) del “mondo reale” e le loro coordinate (um, vm) nelpiano immagine.

Siano C′

e C′′

le due camere dalle quali acquisire le immagini. Dalla calibrazione di entrambi i dispositivisi ottengono le seguenti matrici:

K′

I =

−f ′k′u 0 u′

0

0 −f ′k′v v′

0

0 0 1

, matrice dei parametri intrinseci della camera C′

K′′

I =

−f ′′k′′u 0 u′′

0

0 −f ′′k′′v v′′

0

0 0 1

, matrice dei parametri intrinseci della camera C′′

[R′ |T ′ ], matrice di rototraslazione tra il sistema di riferimento del mondo reale e quello della camera

C′, contenente i parametri estrinseci della prima camera

[R′′ |T ′′ ], matrice di rototraslazione tra il sistema di riferimento del mondo reale e quello della camera

C′′, contenente i parametri estrinseci della seconda camera

37

38 CAPITOLO 5. STIMA DELLA POSIZIONE CON APPROCCIO COLOR FUSION

[RC |TC ], matrice che descrive la rototraslazione tra il sistema di riferimento della prima camera rispettoalla seconda.

Dato il punto di coordinate(u′, v′)

di un generico frame acquisito dalla prima camera, la conversione nel

sistema di riferimento del mondo reale e data da:XM

ZMYM

ZM

11ZM

= R′−1

−f ′k′u 0 u′

0

0 −f ′k′v v′

0

0 0 1

−1 ·u′v′

1

− T ′

La posizione(u′′, v′′)

dello stesso punto proiettato nel piano immagine della seconda camera e data da

u′′v′′1

=

−f ′′k′′u 0 u′′

0 0

0 −f ′′k′′v v′′

0 00 0 1 0

· [R′′ T′′

0 1

]·[RC TC0 1

]·

XM

ZMYM

ZM

11ZM

u′′v′′

1

=[K′′

I 0]·[R′′

T′′

0 1

]·[RC TC0 1

]·

R′−1K−1 ·u′v′

1

− T ′

Attraverso questa relazione (o la sua inversa) e possibile stabilire la relazione biunivoca che lega i puntidei due piani immagine.

Nel progetto la calibrazione e stata realizzata attraverso il tool “calib gui” disponibile in http://www.

vision.caltech.edu/bouguetj/calib_doc/.

L’algoritmo SIFT

L’algoritmo Scale Invariant Feature Transform permette di identificare i punti di un’immagine che manten-gono alcune caratteristiche invarianti a determinate trasformazioni dell’immagine, come la rototraslazione, ilcambiamento di scala e la variazione della luminosita.

Il metodo si compone delle quattro fasi brevemente illustrate.

1. Estrazione di estremi su scala e spazio

All’immagine vengono applicate convoluzioni con filtri gaussiani di scala diversa.Dati due filtri di scala consecutiva (kiσ e ki+1σ, dove σ e un valore standard di scala, ki e un fattore

moltiplicativo e ki < ki+1), viene calcolata la differenza di gaussiane tra le due immagini filtrate, ottenendouna nuova immagine DoGi.

Le nuove immagini date dalla differenza di gaussiane vengono raggruppate in insiemi di dimensioneprestabilita e per ogni insieme vengono valutati i punti “di estremo su scala e spazio”.

Questi punti sono dati dai pixel che presentano un valore di massimo o di minimo locale. Per “locale” siintende sia rispetto ai pixel vicini in termini spaziali (quindi nello stesso frame) sia rispetto agli stessi pixelpresenti nei frame appartenenti allo stesso gruppo (quindi in frame risultanti da convoluzioni dell’immagineoriginale con filtri gaussiani di scala “vicina”).

2. Localizzazione e raffinamento dei punti notevoli

I punti estratti al passo precedente sono generalmente molti ed instabili rispetto a trasformazioni dell’imma-gine. I punti notevoli dovranno essere minimi locali abbastanza isolati, presentare un buon valore di contrastoe non appartenere a bordi di oggetti. Inoltre la loro posizione dovra essere ricalcolata nelle tre componentidi spazio e scala. Per ottenere questo raffinamento si ricorre a tre passi:

http://www.vision.caltech.edu/bouguetj/calib_doc/

http://www.vision.caltech.edu/bouguetj/calib_doc/

39

1. Eliminazione degli estremi non isolati: introducendo un opportuno funzionale

Dx(x) = DoG(x) +∂DoGT

∂x

∣∣∣∣x

· x+xT

2· ∂

2DoGT

∂x2

∣∣∣∣x

· x

dato dallo sviluppo in serie di Taylor della differenza di gaussiane rispetto a x = (u, v, σ) con centronel punto notevole x = (uN , vN , σN ), e possibile stabilire l’isolamento del punto x. Infatti il valore xche annulla la derivata prima di D rappresenta lo scostamento da x tale per cui si ha un massimo o unminimo nella differenza di gaussiane. E’ stato stabilito che per valori delle componenti di x inferiori a0.5, il punto x risulta essere un punto “abbastanza” isolato, mentre per valori maggiori si suppone chex si trovi nelle vicinanze di un punto notevole piu “accentuato” e quindi viene scartato.

2. Eliminazione dei punti a basso contrasto: dato il valore x calcolato nel passo precedente, i punti taliper cui Dx(x) > 0.03 vengono scartati poiche considerati di basso contrasto. Il punto notevole x vienesostituito con x+ x.

3. Eliminazione dei punti di bordo: attraverso la matrice hessiana

H =

[Du,u Du,v

Dv,u Dv,v

]viene calcolato il rapporto r = Tr(H)2

Det(H) : date le curvature principali della DoG, questo funzionale e

maggiore per curvature molto diverse in modulo (caratteristica di un bordo). Per valori di r > 12.1 sie stabilito che la DoG contenga un bordo in posizione (u, v) e questo punto viene scartato.

3. Valutazione dell’orientamento

In questa fase ai punti notevoli viene assegnato un “orientamento” che ne permette l’invarianza rispetto allarotazione.

Per ogni punto notevole vengono calcolati modulo m(u, v) e angolazione θ(u, v) dei punti vicini:

m(u, v) =

√(L(u+ 1, v)− L(u− 1, v))

2+ (L(u, v + 1)− L(u, v − 1))

2

θ(u, v) = arctan (L(u+ 1, v)− L(u− 1, v), L(u, v + 1)− L(u, v − 1))

dove L(u, v) e la convoluzione dell’immagine originale con un particolare filtro gaussiano.Le possibili orientazioni vengono suddivise in 10 insiemi (ognuna contenente 36° di ampiezza) e per ogni

insieme viene valutato il numero di punti aventi un’orientazione compresa nei suoi valori. Dopo aver effettuatouna pesatura rispetto al modulo, l’insieme contenente il maggior numero di punti stabilisce l’orientazione delpunto notevole. In Figura 5.1 vengono evidenziati gli orientamenti dei punti notevoli estratti.

4. Valutazione dei descrittori

L’ultima fase di SIFT consiste nell’associare ad ogni punto notevole un vettore di descrittori invarianti avariazioni di luminosita che ne renda univoco il riconoscimento a seguito di una trasformazione dell’immagine.

Per ogni punto notevole viene considerata una finestra 16 × 16 centrata in esso. Questa finestra vienesuddivisa in ulteriori 16 sotto-finestre di dimensione 4×4. Per ogni sotto-finestra, si stabiliscono le orientazionidei punti contenuti (come nel passo precedente), divise questa volta in 8 insiemi. Ogni punto appartenentead un insieme porta un contributo proporzionale al suo modulo m(u, v). Per ogni insieme di orientazionesi otterra un valore proporzionale ai moduli dei punti contenuti. Al termine verranno valutati quindi 128valori (uno per ogni sotto-insieme di rotazione), che saranno normalizzati rispetto alla media dei moduli dellafinestra 16 × 16. Quest’ultima normalizzazione rende i descrittori indipendenti dalla particolare luminositapresente in quella regione.

In Figura 5.2 per ogni punto notevole si mostrano le 16 sotto-finestre che lo circondano ed il relativoorientamento.


Figura 5.1: Orientamento dei punti notevoli

Figura 5.2: Orientamento delle sotto-finestre dei punti notevoli

Nel progetto l’implementazione di SIFT utilizzata e stata sviluppata da A. Vedaldi nel tool VLFeat ed edisponibile in http://www.vlfeat.org/overview/sift.html.


Le fasi del metodo si differiscono da quelle di ICP e degli altri approcci presentati fin’ora.Infatti l’utilizzo di SIFT permette di stabilire accoppiamenti validi con ragionevole certezza. Un altro

vantaggio legato al suo utilizzo e evidente dalla necessita di un’unica iterazione per ottenere la stima dellamatrice di rototraslazione.

http://www.vlfeat.org/overview/sift.html

41

Il metodo riceve in ingresso due immagini a colori e due immagini di profondita (o di disparita), rappre-sentanti la scena di origine e la scena di destinazione e restituisce la stima della rototraslazione del sensoretra le due viste.

Le fasi sono le seguenti:

1. Estrazione e accoppiamento dei punti notevoli attraverso SIFT

2. Mappatura dei punti notevoli nell’immagine di profondita (o di disparita)

3. Stima della rototraslazione

4. Valutazione dell’errore di allineamento

Lo pseudocodice del metodo e esemplificato di seguito:

1 FUNCTION FRAME BASED(srcColorFrame, srcDepthFrame, tgtColorFrame, tgtDepthFrame)2

3 matchedColorPoints = SIFT match(srcColorFrame, tgtColorFrame);4 matchedDepthPoints = mapColorToDepth(matchedColorPoints, srcDepthFrame, tgtDepthFrame);5

6 [R |T] = evaluateRototranslation(matchedDepthPoints);7

8 evaluateAlignmentError([R |T], matchedDepthPoints);9

10 RETURN [R |T];11

12 END FUNCTION

La valutazione dell’errore di allineamento e opzionale e viene effettuata solo per permettere un confrontocon i metodi proposti nei capitoli precedenti.

Le altre fasi vengono spiegate in dettaglio di seguito.

Estrazione ed accoppiamento tramite SIFT

Data l’immagine a colori sorgente Cs(u, v) e l’immagine a colori di destinazione CD(u, v), attraverso l’algo-ritmo SIFT vengono estratti i punti notevoli da entrambe le immagini.

Per ogni punto notevole dell’immagine sorgente pNS viene cercato un corrispondente punto notevole pNDnell’immagine di destinazione.

Poiche SIFT e stato proposto per identificare gli stessi punti anche dopo una trasformazione dell’immagine,l’accoppiamento proposto da VLFeat non considera la vicinanza spaziale dei punti. Poiche il movimento delsensore tra le due acquisizioni e molto piccolo, gli eventuali accoppiamenti (pNS , pND) tra punti distantivengono scartati.

Nell’implementazione si considerano “distanti” due punti che differiscono di piu di 10 pixel nel pianoimmagine.

Mappatura stereo

Questa fase ha l’obiettivo di associare le informazioni relative alla profondita (o alla profondita inversa) ele coordinate nello spazio euclideo (o nello spazio di profondita inversa) ai punti notevoli estratti nel passoprecedente.

Utilizzando le matrici calcolate attraverso la calibrazione stereo e possibile stabilire in quale punto del-l’immagine di profondita (o di disparita) e stato mappato un punto notevole della relativa immagine acolori.

Dato un punto notevole pN di coordinate (uN , vN ), la relativa posizione (u′

N , v′

N ) nel frame di profonditae data da u′Nv′N

1

=[KIRI 0

]·[RIR T IR

0 1

]·[Rc Tc0 1

]·

RC−1KC−1I ·

uNvN1

− TC


dove l’apice C indica un parametro riferito alla camera a colori, l’apice IR e riferito alla camera ad infrarossi(da cui viene acquisito il frame di profondita o di disparita) e la matrice [Rc|Tc] e la matrice di rototraslazionetra i sistemi di riferimento delle due camere.

Data la profondita o la profondita inversa del punto notevole (eventualmente calcolata dall’immagine didisparita), le rimanenti coordinate sono calcolate attraverso la formula

u =

v =

u′N − uIR0f IRkIRuv′

n − vIR0f IRkIRv

in profondita inversa e x =

y =

u′N − uIR0f IRkIRu

· z

v′

n − vIR0f IRkIRv

· z

Le immagini seguenti mostrano l’acquisizione a colori e l’immagine di profondita di una scena, con ilrisultato della mappatura.

Figura 5.3: Da sinistra: immagine a colori, immagine di profondita e fusione delle due acquisizioni. Le zonenere indicano i punti in cui i valori di profondita sono saturi (in questo caso a causa della luce solare e dizone non raggiunte dai raggi infrarossi)

Stima della rototraslazione

Date le coppie di punti notevoli, sono stati implementati tre metodi diversi per la stima della matrice [R|T ]:

l’utilizzo dei minimi quadrati considerando indipendenti le componenti della matrice [R|T ]

l’utilizzo dei minimi quadrati con l’approssimazione a piccole rotazioni

l’utilizzo dei minimi quadrati attraverso la scomposizione SVD



L’approccio Color Fusion richiede uno spazio per l’allocazione superiore rispetto ai metodi analizzati inprecedenza.

43

Infatti ogni scena viene rappresentata sia attraverso un’immagine a colori sia attraverso la corrispondenteimmagine di disparita (o di profondita). Se la dimensione di un frame e di hF ×wF , lo spazio di allocazionerichiesto e di

T (hF , wF ) = 4 · hF · wFcorrispondente a

Θ (hF · wF )

Risorse temporali

In merito alla complessita relativa al tempo di esecuzione, i passi principali sono riportati in tabella:


SIFT Θ (hF · wF · hg · wg) Θ (hF · wF · hg · wg)Mappatura stereo Θ (η) Θ (hF · wF )

Stima dellarototraslazione

Θ(η3)

Θ(

(hF · wF )3)

Tabella 5.1: Complessita computazionale di color fusion

dove:

hF × wF e la dimensione di un frame

hg × wg e la dimensione della finestra di convoluzione gaussiana attraverso la quale si applica il filtro

η e il numero di punti notevoli estratti dall’immagine a colori

Dalla tabella si puo notare come il numero di punti notevoli η influisca significativamente sulla complessitadel metodo.

Il funzionale di complessita e dato da:

TCF (N,G,W, l) = N · [(l + 1) ·G+ 1] + η + η[W + 256 + η + η2

]dove:

N e il numero di pixel di un frame hF × wF

G e il numero di pixel della finestra del filtro gaussiano hg × wg

W e il numero di vicini considerati per il calcolo dell’orientamento in SIFT

l e il numero di immagini costruite attraverso la differenza di gaussiane

Infatti, per grandi valori di η (prossimi al numero di pixel hF ·wF dell’immagine) la stima della rototraslazionerisulta la fase piu onerosa. A differenza dei metodi precedenti, in cui il numero di punti campionati potevaessere stabilito a priori, non e possibile predire quanti punti saranno considerati “notevoli” da SIFT.

Si e scelto quindi di limitare il numero di punti notevoli attraverso i quali stimare la rototraslazione,prediligendo quelli con scostamento minore tra i relativi descrittori.

La limitazione posta garantisce che η3 risulti “molto minore” rispetto al numero di pixel dell’immagine,fornendo al metodo una complessita di Θ (hF · wF · hg · wg).

Capitolo 6

Risultati sperimentali e confronti

Nel presente capitolo vengono illustrati i risultati degli allineamenti effettuati con le diverse tecniche presen-tate nei capitoli precedenti.

La traiettoria del sensore e data dalla composizione delle rototraslazioni valutate tra coppie consecutivedi acquisizioni.

Si e voluto analizzare il “caso reale”, registrando dal sensore un movimento campione composto da 523immagini, acquisendo contemporaneamente sia i frame a colori che le immagini di disparita.

La sequenza e qualitativamente cosı composta:

frames [1 . . . 20]: sensore fermo

frames [20 . . . 220]: movimento prevalentemente traslazionale lungo l’asse orizzontale,

frames [220 . . . 320]: movimento rotazionale prevalentemente di rollio sull’asse ottico

frames [320 . . . 370]: movimento traslazionale prevalentemente lungo l’asse verticale e movimento rota-zionale prevalentemente di rollio

frames [370 . . . 420]: movimento traslazionale prevalentemente lungo l’asse orizzontale

frames [420 . . . 440]: movimento prevalentemente di beccheggio

frames [440 . . . 500]: movimento traslazionale prevalentemente lungo l’asse orizzontale

frames [500 . . . 523]: sensore fermo

45

46 CAPITOLO 6. RISULTATI SPERIMENTALI E CONFRONTI

Figura 6.1: Alcuni frame della sequenza campione acquisita

Poiche il video e stato acquisito manualmente, ogni movimento e affetto da piccole componenti rototra-slazionali dovute all’inevitabile instabilita della mano.

Attraverso le formule presentate nel Capitolo 1, per ogni frame si e calcolata la relativa immagine diprofondita.

Su questo insieme di immagini sono stati eseguiti tutti i metodi sviluppati e per ogni coppia di scene si evalutato l’errore medio di allineamento.

La bonta di un metodo e valutata confrontando i risultati ottenuti e la sua complessita con i risultati diICP, che e al momento l’approccio piu diffuso per questo tipo di problema.

In merito alla qualita dell’allineamento si e fatto ricorso al calcolo dell’errore medio di allineamento,rendendo il confronto indipendente dal numero di punti saturi (e quindi palesemente errati) catturati dalsensore e presenti nell’immagine di profondita.

L’errore e cosı calcolato:

1. se non e gia presente, per ogni frame viene costruita l’immagine di profondita

2. dall’immagine di profondita viene costruita una nuvola di punti nello spazio euclideo

3. la nuvola di punti sorgente viene rototraslata utilizzando la matrice risultante dall’approccio scelto

4. viene costruita una nuvola di punti dell’immagine di destinazione e per ogni punto della nuvola sorgenteviene cercato il punto piu vicino della nuvola di destinazione; vengono trascurati i punti che saturanoil valore di profondita

5. il risultato e la distanza media tra le coppie di punti

47

Di seguito sono riportati i risultati ed i confronti degli allineamenti delle sequenze di test, con riferimentoall’errore di allineamento e a particolari considerazioni sui singoli metodi.

Infine viene confrontata la complessita computazionale degli approcci sviluppati, dal punto di vistadell’allocazione delle risorse e dal punto di vista del numero di operazioni richieste.

Risultati sperimentali

In questa sezione vengono riportati i risultati dell’esecuzione dei diversi approcci sulla sequenza campione,con considerazioni sui singoli metodi.

ICP standard

Iterative Closest Point e il metodo standard su cui basare il confronto degli altri algoritmi.

Se ne e utilizzata l’implementazione sviluppata da Kroon disponibile su http://www.mathworks.com/

matlabcentral/fileexchange/30242-kinect-matlab. Si e scelta questa versione poiche utilizzata gia in[12] per il confronto dei risultati.

Sulla sequenza di test sono state ripetute 10 stime della traiettoria, per evitare di basare i risultati sulparticolare campionamento (fortunato o meno) dei punti in fase di selezione. Sono stati effettuati test separatiper la metrica d’errore punto-punto [ICPpt] e la metrica punto-piano [ICPpl].

La ripetizione delle stime non ha portato significativi cambiamenti all’errore ottenuto.

Di seguito e riportato l’andamento dell’errore medio di allineamento sulla sequenza campione, trasfor-mando i frame in nuvole di punti nello spazio euclideo.

Figura 6.2: Errore di allineamento (mm) per [ICPpt] sulla sequenza di frame

http://www.mathworks.com/matlabcentral/fileexchange/30242-kinect-matlab

http://www.mathworks.com/matlabcentral/fileexchange/30242-kinect-matlab


Figura 6.3: Errore di allineamento (mm) per [ICPpl] sulla sequenza di frame.

Gli errori sono compresi tra l’1 ed i 71 millimetri. Per la metrica punto-piano l’errore medio e di18.0044mm, mentre nella punto-punto e leggermente piu alto, pari a 18.7921mm.

Il metodo non mostra risposte particolarmente differenti al tipo di rototraslazione stimata (prevalente-mente rotazionale o traslazionale).

Per evidenziarne la distribuzione se ne e calcolata la funzione cumulativa, che permette di mostrare lapercentuale di allineamenti affetti “al massimo” da un determinato valore d’errore. In ascissa viene riportatol’errore medio, in ordinata il valore percentuale cumulato.

Figura 6.4: Distribuzione cumulata degli errori di allineamento per [ICPpt] ed [ICPpl]

Piu della meta dei frame e stata allineata con un’errore medio inferiore ai 20mm, mentre la quasi totalitadegli allineamenti (90%) ha un’errore medio al di sotto dei 50mm.

L’utilizzo della metrica punto-piano garantisce un risultato mediamente migliore (ovvero la percentuale dipunti soggetti al massimo ad un dato errore e superiore rispetto alla metrica punto-punto), ma la complessitacomputazionale e maggiore a causa della necessita del calcolo delle normali.

In generale [7] suggerisce l’approccio punto-piano come miglior compromesso tra errore di allineamento etempo di esecuzione.

ICP e poi stato rieseguito costruendo le nuvole di punti nello spazio di profondita inversa ([ICP invpt ],

[ICP invpl ]). I risultati sono i seguenti:

49

Figura 6.5: Errore di allineamento (mm) sulla sequenza di frame per [ICP invpl ]

Figura 6.6: Errore di allineamento (mm) sulla sequenza di frame per [ICP invpt ]

Gli errori sono compresi tra i 3 ed i 25 millimetri. La metrica punto-piano ha un’errore medio di 6.2690mm,maggiore rispetto alla punto-punto in cui vale 5.7120mm.

In entrambi i casi e possibile notare come l’errore di allineamento aumenti in corrispondenza dei movimentirotazionali.

La distribuzione cumulata dell’errore e la seguente:


Figura 6.7: Distribuzione cumulata degli errori di allineamento per [ICP invpt ] ed [ICP invpl ]

Piu della meta dei frame e stata allineata con un’errore medio inferiore ai 6mm, mentre la quasi totalitadegli allineamenti (90%) ha un’errore medio al di sotto dei 10mm.

La metrica leggermente piu affetta da errore e la punto-piano.

Inverse ICP

Il lavoro in [12] manca di un codice sorgente disponibile: cio ha reso necessaria la scrittura di un programmache rispettasse le specifiche presenti nel documento.

Anche in questo caso le stime sono state ripetute per ottenere un risultato indipendente dal campiona-mento dei punti.

Si e effettuata la minimizzazione sia con la metrica punto-punto [INVpt] sia con la metrica punto-piano[INVpl].

La traiettoria stimata nella sequenza campione ha prodotto il seguente andamento dell’errore:

Figura 6.8: Errore di allineamento (mm) sulla sequenza di frame per [INVpt]

51

Figura 6.9: Errore di allineamento (mm) sulla sequenza di frame per [INVpl]

Gli errori sono compresi tra i 3 ed i 40 millimetri. La metrica punto-piano ha un’errore medio di 6.2137mm,maggiore rispetto alla punto-punto in cui vale 5.5616mm.

E possibile notare come nel punto-piano si raggiungano valori di errore mai ottenuti nella punto-punto,principalmente in corrispondenza dei cambi di movimento tra traslazionale e rotazionale. Anche escludendoquesti “picchi”, l’errore si mantiene intorno ai 6 mm (5.9936 mm), comunque superiore all’errore per lapunto-punto.

La distribuzione cumulata dell’errore e la seguente:

Figura 6.10: Distribuzione cumulata degli errori di allineamento per [INVpt] ed [INVpl]

Piu della meta dei frame e stata allineata con un’errore medio inferiore ai 7mm, mentre la quasi totalitadegli allineamenti (90%) ha un’errore medio al di sotto dei 10 mm. In conformita con quanto mostratonei grafici di errore, la metrica punto-punto ha una pendenza maggiore, segno di un errore di allineamentomediamente inferiore.

ICP Frame Based

Come descritto nel Capitolo 4, l’approccio Frame Based nasce sull’idea di mantenere le informazioni sullavicinanza dei punti intrinsecamente presenti nel frame acquisito. Il metodo e stato eseguito utilizzando la


conversione nello spazio di profondita inversa.

Di queste se ne sono scelte quelle con risultati migliori:

[FB SV DPCApl ] pesatura proporzionale alla distanza, metrica di minimizzazione punto-piano con SVD,

stima delle normali con PCA

[FB SV DMinQpl ] pesatura proporzionale alla distanza, metrica di minimizzazione punto-piano con SVD,

stima delle normali con minimi quadrati per piccoli angoli (Capitolo 3)

[FB MinQPCApl ] pesatura proporzionale alla distanza, metrica di minimizzazione punto-piano conminimi quadrati su piccoli angoli, stima delle normali con PCA

[FB SV Dpt] pesatura proporzionale alla distanza, metrica di minimizzazione punto-punto con SVD

Anche in questo caso le stime sono state ripetute per ottenere un risultato indipendente dal campionamentodei punti.

Gli errori ottenuti sono i seguenti:

Figura 6.11: Errore di allineamento (mm) sulla sequenza di frame per FrameBased-ICP con metrica punto-

piano. Da sinistra: (A) [FB SV DPCApl ], (B) [FB SV DMinQ

pl ], (C) [FB MinQPCApl ]

53

Figura 6.12: Errore di allineamento (mm) sulla sequenza di frame per [FB SV Dpt]

Gli errori sono compresi tra l’1 ed i 20 millimetri. I valori medi dell’errore sono:

5.3313mm per [FB SV DPCApl ]

5.4452mm per la combinazione [FB SV DMinQpl ]

7.3177mm per la combinazione [FB MinQPCApl ]

5.6590mm per la combinazione [FB SV Dpt]

[FB SV DPCApl ] garantisce un errore inferiore rispetto alle altre. Anche in questo metodo si puo notare un

generale peggioramento dell’errore in corrispondenza dei movimenti rotazionali. Nelle ultime sequenze diimmagini (in cui il sensore e quasi fermo) l’errore e abbattuto a valori corrispondenti a quelli delle primesequenze.

Di seguito e riportata la distribuzione cumulata dell’errore:

Figura 6.13: Distribuzione cumulata degli errori di allineamento per [FB SV DPCApl ], [FB SV DMinQ

pl ],

[FB MinQPCApl ] ed [FB SV Dpt]


In generale, piu della meta dei frame e stata allineata con un’errore medio inferiore ai 7mm. Le combina-zioni punto-piano che utilizzano SVD risultano migliori. Infatti per queste la quasi totalita degli allineamenti(90%) ha un’errore medio al di sotto dei 9 mm. Per la metrica punto-punto questo valore sale a 10 mm,mentre per [FB MinQPCApl ] la soglia sale a 14mm. La combinazione di parametri migliore risulta quindi la

[FB SV DPCApl ].

Color Fusion

A differenza degli approcci precedenti che richiedono una fase iniziale di campionamento, il metodo Color-Fusion e deterministico e ripetibile: infatti la procedura di estrazione dei punti notevoli seleziona i medesimipunti.

Per questo motivo in questo test non e stato necessario ripetere piu volte la stima della traiettoria sullasequenza campione.

Implicitamente la metrica di minimizzazione del metodo e punto-punto.

Si sono eseguite quattro stime, combinando lo spazio di riferimento utilizzato ed il metodo di minimizza-zione dell’errore:

[CF 3DSVD]: spazio euclideo e minimizzazione con SVD

[CF 3DMinQ]: spazio euclideo e minimizzazione con minimi quadrati

[CF InvSV D]: spazio di profondita inversa e minimizzazione con SVD

[CF InvMinQ]: spazio di profondita inversa e minimizzazione con minimi quadrati

Gli errori di allineamento risultanti sono i seguenti:

Figura 6.14: Errore di allineamento (mm) sulla sequenza di frame per ColorFusion su spazio euclideo. Dasinistra: (A) [CF 3D

MinQ] (B) [CF 3DSVD]

55

Figura 6.15: Errore di allineamento (mm) sulla sequenza di frame per ColorFusion su spazio di profonditainversa. Da sinistra: (A) [CF InvMinQ] (B) [CF InvSV D]

Gli errori sono molto diversi: per lo spazio euclideo i valori sono compresi tra i 10 ed i 200 millimetri, perlo spazio di profondita inversa si va dai 3 ai 36 millimetri. I valori medi dell’errore sono:

27.0049mm per [CF 3DMinQ]

29.7766mm per [CF 3DSVD]

9.5247mm per [CF InvMinQ]

9.5269mm per [CF InvSV D]

L’utilizzo dello spazio euclideo porta un errore molto grande rispetto alla profondita inversa. Tuttavia sidimostra indipendente dal tipo di rototraslazione da stimare, mentre l’utilizzo dello spazio inverso comportapicchi d’errore in corrispondenza ai cambi di direzione rototraslazionale. Come ci si aspetta, le combinazioniche utilizzano la profondita inversa mostrano un abbattimento dell’errore in corrispondenza dei frame in cuiil sensore e pressoche fisso.

Di seguito e riportata la distribuzione cumulata dell’errore, distinta per spazio euclideo e profonditainversa:

Figura 6.16: Distribuzione cumulata degli errori di allineamento per ColorFusion su spazio euclideo (sinistra)e profondita inversa (destra)


Per quanto riguarda l’utilizzo dello spazio euclideo, piu della meta dei frame e stata allineata con unerrore medio inferiore ai 23 mm, mentre la quasi totalita degli allineamenti (90%) ha un’errore medio al disotto dei 63mm (60mm utilizzando SVD).

Per quanto riguarda l’utilizzo dello spazio di profondita inversa, piu della meta dei frame e stata allineatacon un errore medio inferiore ai 10mm, mentre la quasi totalita degli allineamenti (90%) ha un’errore medioal di sotto dei 16mm (15mm utilizzando SVD).

L’utilizzo di SVD garantisce quindi risultati migliori e la combinazione con lo spazio di profondita inversarisulta la scelta migliore.

Confronto sull’errore di allineamento

Nella seguente tabella e evidenziato il confronto dell’errore di allineamento tra tutti i metodi analizzati. Perogni metodo viene valutata la percentuale di allineamenti in cui e stato commesso un errore minore rispettoad un altro approccio.

ICPpt ICPpl ICP Invpt ICP Invpl INVpt INVplICPpt \ 69% 0% 27.8% 0% 28.7% 0% 31.8% 0% 30.9% 0%

ICPpl 31% 0% \ 32.4% 0% 32.8% 0% 33.9% 0% 34.5% 0%

ICP Invpt 72.2% 0% 67.6% 0% \ 59.2% 0% 38.5% 0% 63.8% 0%

ICP Invpl 71.3% 0% 67.2% 0% 40.8% 0% \ 37.9% 4.6% 59.4% 2.7%

INVpt 68.2% 0% 66.1% 0% 61.5% 0% 57.5% 4.6% \ 48.7% 22.2%

INVpl 69.2% 0% 65.5% 0% 36.2% 0% 37.9% 2.7% 29.1% 22.2% \FB SV DPCA

pl 72.6% 0% 68.4% 0% 52.7% 0% 59.9% 0% 42.9% 0% 62.1% 0%

FB SV DMinQpl 72.6% 0% 68.4% 0% 48.3% 0% 51.3% 0% 40.6% 0% 59.6% 0%

FB MinQPCApl 64.2% 61.3% 0% 38.9% 0% 39.7% 0% 33.9% 0% 42.3% 0%

FB SV Dpt 70.7% 0% 67% 0% 43.9% 0% 47.5% 0% 35.8% 0% 55.4% 0%

CF 3DSVD 35.8% 0% 34.3% 0% 2.7% 0% 3.7% 0% 0% 0% 2.5% 0%

CF 3DMinQ 37.5% 0% 36.2% 0% 2.7% 0% 3.8% 0% 0% 0% 8.4% 0%

CF InvSV D 62.4% 0% 59.6% 0% 10.8% 0% 13.0% 0% 0.6% 0% 2.5% 0%

CF InvMinQ 62.4% 0% 59.6% 0% 10.7% 0% 13.1% 0% 0.5% 0% 2.5% 0%

FB SV DPCApl FB SV DMinQ

pl FB MinQPCApl FB SV Dpt

ICPpt 27.4% 0% 27.4% 0% 35.8% 29.3% 0%

ICPpl 31.6% 0% 31.6% 0% 38.7% 0% 33% 0%

ICP Invpt 47.3% 0% 51.7% 0% 61.1% 0% 56.1% 0%

ICP Invpl 47.1% 0% 48.7% 0% 60.3% 0% 52.5% 0%

INVpt 57.1% 0% 59.4% 0% 66.1% 0% 64.2% 0%

INVpl 37.9% 0% 40.4% 0% 57.7% 0% 44.6% 0%

FB SV DPCApl \ 51.9% 0% 67.1% 0% 54.8% 0%

FB SV DMinQpl 48.1% 0% \ 64.4% 0% 51.9% 0%

FB MinQPCApl 32.9% 0% 35.6% 0% \ 40.6% 2%

FB SV Dpt 45.2% 0% 48.1% 0% 59.2% 2% \CF 3D

SVD 0.6% 0% 0.6% 0% 5.6% 0% 1.3% 0%

CF 3DMinQ 0.6% 0% 0.6% 0% 5.7% 0% 1.3% 0%

CF InvSV D 4.8% 0% 5.4% 0% 14.2% 0% 5% 0%

CF InvMinQ 4.8% 0% 5.5% 0% 14.2% 0% 4.9% 0%

57

CF 3DSVD CF 3D

MinQ CF InvSV D CF InvMinQ

ICPpt 64.2% 62.5% 37.6% 0% 37.6% 0%

ICPpl 65.7% 0% 63.8% 0% 40.4% 0% 40.4% 0%

ICP Invpt 97.3% 0% 97.3% 0% 89.2% 0% 89.3% 0%

ICP Invpl 96.7% 0% 96.2% 0% 87.0% 0% 86.9% 0%

INVpt 100% 0% 100% 0% 99.4% 0% 99.5% 0%

INVpl 97.5% 91.6% 0% 97.5% 0% 97.5% 0%

FB SV DPCApl 99.4% 0% 99.4% 0% 95.2% 0% 95.2% 0%

FB SV DMinQpl 99.4% 0% 99.4% 0% 94.6% 0% 94.5% 0%

FB MinQPCApl 94.4% 0% 94.3% 0% 85.8% 0% 85.8% 0%

FB SV Dpt 98.7% 0% 98.7% 0% 95% 0% 95.1% 0%

CF 3DSVD \ 22.6% 2% 7.5% 0% 7.5% 0%

CF 3DMinQ 77.2% 2% \ 7.7% 0% 7.7% 0%

CF InvSV D 92.5% 0% 92.3% 0% \ 44..4% 0%

CF InvMinQ 92.5% 0% 92.3% 0% 55.6% 0% \

Tabella 6.1: Percentuale di allineamenti con errore strettamente inferiore (a pedice la percentuale di erroridi valore idendico sullo stesso frame)

Dal confronto si puo notare come quasi tutti i metodi sviluppati diano risultati migliori di ICP, eccettol’approccio Color Fusion applicato sullo spazio euclideo, che risulta la scelta peggiore.

Il tentativo di eseguire la versione standard di ICP utilizzando nuvole di punti nello spazio di profonditainversa risulta una buona intuizione: nella maggior parte degli allineamenti l’errore commesso e inferiore aquello risultante da quasi tutti gli altri metodi.

Il metodo proposto in [12] risulta senza dubbio il piu efficace, in particolare nella versione punto-punto:l’errore commesso per piu della meta delle volte (se non in tutte in alcuni casi) e minore rispetto agli altriapprocci.

Il metodo Frame Based sviluppato in questo progetto si dimostra migliore di ICP nell’errore di allinea-mento ed e paragonabile agli altri approcci nel senso che mediamente commette lo stesso numero di errori deimetodi migliori. In particolare [FB SV DPCA

pl ] risulta la combinazione migliore, mentre da [FB MinQPCApl ]si ottengono i confronti piu scadenti.

Il metodo Color Fusion (secondo metodo interamente sviluppato nel progetto) eseguito sullo spazio eucli-deo e senza dubbio l’approccio peggiore: infatti e l’unico in cui si raggiunge un errore di decine di centimetri.La stima sullo spazio inverso invece fornisce risultati migliori rispetto ad ICP standard. Cio non si verificanel confronto con gli altri metodi, ma poiche l’errore medio e di poco superiore agli altri ottenuti, si puocomunque affermare che anche questo metodo e paragonabile agli approcci precedenti.

Un pregio di Color Fusion, che non si verifica in altri metodi, e la possibilita di stimare traslazioni lungol’asse verticale o orizzontale anche in scene formate solo da un piano posto frontalmente al sensore, purchenon monocromatico. Ponendo ad esempio il sensore di fronte ad una parete (ortogonale all’asse ottico) etraslandolo lateralmente o verticalmente, nessun metodo che si basi solo sulla profondita e in grado di fornireuna rotazione diversa dall’identita ed una traslazione non nulla, poiche tra due acquisizioni consecutive ledue nuvole di punti appaiono identiche. La presenza di diversi colori nella parete invece permette a SIFTdi localizzare lo stesso punto in posizioni diverse su frame consecutivi. Da qui Color Fusion e in grado distimare la traslazione pur in assenza di valori diversi nell’immagine di disparita.

Confronto sulla complessita computazionale


La seguente tabella riassume la valutazione del funzionale di complessita T (x) legata ai diversi approcci.La valutazione comprende sia il caso “base” sia i casi di costruzione di un albero di ricerca sulla nuvola didestinazione (per la ricerca dei vicini) e sulla nuvola sorgente (per la costruzione delle normali). Si precisache i simboli nella tabella rappresentano le seguenti quantita:




hF × wF : risoluzione di un frame (a colori e di disparita o profondita)

Metodo Base k-d tree su destinazione k-d tree su sorgenteICP N +M N + 2M 2 · (N +M)

Inverse ICP N +M N + 2M 2 · (N +M)ICP Frame Based 2 · hF · wF / /

Color Fusion 4 · hF · wF / /

Tabella 6.2: Confronto di T(x) sull’allocazione di memoria

In questo progetto. le quantita N . M e hF · wF si equivalgono, poiche per ogni pixel viene costruito unpunto della nuvola e cio avviene per ogni frame. Poiche i frame hanno la stessa risoluzione, il numero dipunti allocati per un’immagine si equivale.

Detto N il numero di pixel in un frame, la tabella puo essere riscritta:

Metodo Base k-d tree su destinazione k-d tree su sorgenteICP 2 · N 3 · N 4 · N

Inverse ICP 2 · N 3 · N 4 · NICP Frame Based 2 · N / /

Color Fusion 4 · N / /

Tabella 6.3: Confronto di T(x) sull’allocazione di memoria in funzione del numero di pixel di un frame

Attraverso quest’ultima considerazione e possibile stabilire che il metodo che richiede piu risorse di me-moria e l’approccio Color Fusion: infatti richiede quattro immagini per poter essere eseguito, a differenzadelle sole due del Frame Based.

Gli approcci meno onerosi sono quindi il Frame Based e la versione “base” di ICP e di ICP in profonditainversa.

Per quanto riguarda la notazione asintotica, tutti i metodi hanno complessita lineare

Θ(N)

Risorse temporali

La seguente tabella riassume la valutazione del funzionale di complessita riferito al numero di operazionirichieste.

Per ogni metodo viene valutato sia il caso medio sia il caso peggiore. Si precisa che i simboli nella tabellarappresentano le seguenti quantita:



n: numero di punti campionati dalla nuvola sorgente

Imax: massimo numero di iterazioni consentite

ω: numero di pixel presenti in una finestra di ricerca di Frame Based

Np: numero di pixel presenti in un frame (a colori. di disparita o profondita)

G: numero di punti nella finestra gaussiana utilizzata da SIFT per il filtro

59

W : numero di vicini considerati per il calcolo dell’orientamento in SIFT

l: numero di immagini costruite attraverso la differenza di gaussiane

η: numero di punti notevoli estratti da SIFT

η: numero di punti notevoli filtrati

Metodo Caso medio Caso peggiore

ICP TICP (N,M) = M + Imax ·[2n+O (n · log (M)) + n3

]TICP (N,M) = M + Imax ·

[2n+O (n ·M) + n3

]ICP profondita inversa TINV ICP (N,M) = N + 2 ·M + Imax ·

[2n+O (n · log (M)) + n3

]TINV ICP (N,M) = N + 2 ·M + Imax ·

[2n+O (n ·M) + n3

]ICP Frame Based TFB(Np) = Np + Imax ·

[2n+O (n · log (ω)) + n3

]TFB(Np) = Np + Imax ·

[2n+O (n · ω) + n3

]Color Fusion TCF (Np, G,W, l) = Np · [(l + 1) ·G+ 1] + η + η

[W + 256 + η + η2

]TCF (Np, G,W, l) = Np · [(l + 1) ·G+ 1] + η + η

[W + 256 + η + η2

]Tabella 6.4: Confronto di T(x) sul numero di operazioni richieste

Come nell’analisi delle risorse temporali e possibile diminuire le variabili utilizzate per permettere unconfronto piu agile.

Nel progetto sviluppato, le quantita N . Np ed M possono essere considerate uguali poiche per ogni pixeldi un frame viene costruito un punto della nuvola corrispondente. La dimensione delle finestre G e W e lequantita l e η sono determinate da SIFT e non e possibile influire su di esse. Le quantita Imax. n ed η sonoconfigurabili dall’utente.

Ulteriori considerazioni derivanti dall’implementazione e dalle pratiche comuni sono le seguenti:

Imax e prossimo a 15 e molto inferiore a N

η e di un ordine di grandezza inferiore a n

n e di almeno 3 ordini di grandezza inferiori a N

Posto N il numero di pixel in un frame. la tabella precedente puo essere allora cosı riscritta:

Metodo Caso medio Caso peggiore

ICP TICP (N) = N + Imax ·[2n+O

(n · log

(N))

+ n3]

TICP (N) = N + Imax ·[2n+O

(n · N

)+ n3

]Inverse ICP TINV ICP (N) = 3 · N + Imax ·

[2n+O

(n · log

(N))

+ n3]

TINV ICP (N) = 3 · N + Imax ·[2n+O

(n · N

)+ n3

]ICP Frame Based TFB(N) = N + Imax ·

[2n+O (n · log (ω)) + n3

]TFB(N) = N + Imax ·

[2n+O (n · ω) + n3

]Color Fusion TCF (N ,G,W, l) = N · [(l + 1) ·G+ 1] + η + η

[W + 256 + η + η2

]TCF (N ,G,W, l) = N · [(l + 1) ·G+ 1] + η + η

[W + 256 + η + η2

]Tabella 6.5: Confronto di T(x) sul numero di operazioni richieste

Dalle valutazioni precedenti e possibile stabilire quali metodi sono meno onerosi in termini di numero dioperazioni richieste.

Tra i metodi basati su ICP, il piu oneroso e l’approccio a profondita inversa: la necessita di disporre deipunti in profondita inversa richiede una pre-elaborazione che aumenta la quantita di operazioni richieste.

Il metodo Color Fusion risulta senza dubbio il piu dispendioso. A SIFT infatti e attribuita la maggiorparte delle operazioni, che richiedono piu di un filtraggio e la valutazione degli orientamenti di una grandequantita di punti. Si e voluto ridurre la complessita dei minimi quadrati applicando una soglia al numero dipunti filtrati η.

L’applicazione di Frame Based risulta infine la piu efficiente dal punto di vista della complessita nelnumero di operazioni: risulta infatti molto vantaggioso ricercare i vicini in una regione limitata dello spazio(all’interno di una finestra di ω elementi) piuttosto che su tutti gli N punti di un frame.

Conclusioni

Il confronto tra i metodi sviluppati e quelli gia esistenti ha dimostrato la validita dei nuovi approcci, sia dalpunto di vista della qualita della stima sia dell’onerosita computazionale.

Il primo buon risultato raggiunto e il miglioramento dell’errore di allineamento rispetto ad ICP, cherisulta ancora il metodo piu diffuso. Si e mostrato come anche il solo cambio del sistema di coordinate(da euclideo a profondita inversa) migliora notevolmente la correttezza della stima: a scapito di un piccoloonere computazionale dovuto alla conversione in coordinate in profondita inversa si ottiene un apprezzabileabbattimento dell’errore. Questa opzione potrebbe essere valutata nei sistemi dove gia viene utilizzato ICP(di cui e garantita la convergenza anche a livello teorico) per ottenere un compromesso tra l’utilizzo di unmetodo stabile e diffuso ed un migliore allineamento.

L’allocazione della quasi medesima quantita di memoria da parte di tutti i metodi e un altro risultatodegno di attenzione: cio significa che in tutti gli ambienti in cui viene gia utilizzato ICP, puo essere utilizzatoanche uno degli altri approcci senza dover richiedere piu memoria fisica.

Il metodo sviluppato in [12] risulta senza dubbio il piu efficace e, stando all’articolo, riesce ad allinearealmeno 30 frame al secondo, raggiungendo l’obiettivo della stima in tempo reale. Si e dimostrato soloteoricamente che gli altri metodi non richiedono una computazione largamente piu onerosa: uno sviluppointeressante richiederebbe la riscrittura dei metodi in un linguaggio piu performante (come il C, come e statofatto nell’articolo citato) ed il confronto dei tempi di esecuzione.

Per confermare ulteriormente la bonta dei nuovi metodi sono necessarie altre misure piu accurate sullacausa degli errori di allineamento: e necessario valutare se sono dovuti a particolari tipi di rototraslazione(come sembra nel caso di ICP eseguito sullo spazio di profondita inversa) per eventualmente integrare unmeccanismo correttivo.

Un’ulteriore proposta riguarda l’esecuzione su GPU. I metodi che richiedono diverse iterazioni potrebberoessere eseguiti in piu istanze contemporanee, elaborando le stime ottenute scegliendo quella con errore mediominore oppure combinandole tra loro. Il metodo Color Fusion invece trarrebbe ancora piu vantaggio daun’esecuzione parallela, poiche il frame a colori potrebbe essere suddiviso in sotto-frame su cui eseguireindipendentemente SIFT, abbattendo i tempi di esecuzione.

La proposta forse piu interessante, motivo che ha dato origine a questo progetto, e il pilotaggio di un brac-cio meccanico imponendo in tempo reale le rototraslazioni stimate. Questo rende necessaria una riscritturadel metodo scelto per la stima, adattando il nuovo codice anche al particolare ambiente in cui verra eseguito.

61

Appendice A - ComplessitaComputazionale

La complessita computazionale di un algoritmo e la quantita di risorse fisiche (come la memoria) e temporalinecessarie alla sua esecuzione.

Per quanto riguarda il tempo di esecuzione, si vuole fornire una stima asintotica basata sul metodopiuttosto che sulla particolare tecnologia (linguaggio di programmazione, ambiente di esecuzione) con cui estato sviluppato. Le considerazioni temporali non faranno quindi riferimento a misure espresse in secondi mapiuttosto all’ordine di operazioni “elementari” (somme e prodotti) utilizzate nella procedura.

Anche in merito alla quantita di risorse impiegate si e scelto di non entrare nei dettagli implementatividella rappresentazione degli oggetti (come i tipi elementari, le matrici, gli alberi, le mappe e le liste), evitandostime basate sulla quantita di byte da allocare in memoria. Si vuole invece descrivere l’ammontare dello spaziorichiesto in proporzione al numero di punti utilizzati. L’unita minima di allocazione e quindi il singolo punto(inteso come vettore di elementi), esulando dal numero di byte necessari alla sua rappresentazione.

Per un’ultima puntualizzazione riguardo alla quantita di memoria richiesta si specifica che non vieneconsiderato lo spazio “dinamico” allocato dalle operazioni nello stack di una RAM.

Funzione di complessita

Ad ogni algoritmo e possibile associare un funzionale che ne descrive la complessita al variare della dimensionedell’ingresso.

Dato un algoritmo A e l’unita minima di allocazione u, il funzionale di complessita e una funzione

TA(u) = f(u)

La funzione f e monotona non decrescente ed il suo valore rimane costante o cresce proporzionalmentead u.

Questo valore rappresenta la quantita di memoria o il numero di passi richiesti dall’algoritmo A specificatoun ingresso u.

In merito al “numero di passi” richiesti da un metodo, e consuetudine ignorare le operazioni di somma esottrazione ed associare una complessita di 1 unita alle operazioni di prodotto e divisione.

Un esempio di calcolo di questo funzionale e fornito attraverso lo pseudocodice seguente, che sottrae adogni elemento del vettore v la sua media:

1 FUNCTION DIFF(v)2

3 mean = 0;4

5 for i=1 to length(v)6 mean = mean + v(i);7 end8

9 mean = mean/length(v);10

11 for i=1 to length(v)

63


12 v(i) = v(i)-mean;13 end14

15 return v;16

17 END FUNCTION

La complessita di questo metodo nell’allocazione di memoria e data dalla quantita di spazio richiesto perallocare l’input, ovvero il numero nV di elementi contenuti nel vettore. La funzione di complessita e quindibanalmente lineare:

TDIFF (nV ) = nv

La complessita in merito alle risorse temporali e data dal numero di passi richiesti dal metodo: nV peril primo ciclo, un ulteriore passo per la divisione ed infine nV per l’ultimo ciclo. La complessita e dunquelineare in nV :

TDIFF (nV ) = 2 · nV + 1

Notazioni di Bachmann–Landau

Per descrivere la complessita di un metodo si utilizzano tre particolari notazioni, introdotte dai matematiciEdmund Landau e Paul Bachmann: “o grande”, “theta” e “omega”. Queste descrivono il comportamentoasintotico di un algoritmo, in funzione della dimensione dei dati ricevuti in ingresso.

Attraverso la notazione “o grande” si indica un limite superiore al crescere della dimensione dell’input.Formalmente, data la funzione f(n) rappresentante l’algoritmo applicato ad un ingresso di dimensione n e lafunzione g(n) che rappresenta un ordine di complessita (generalmente n, nx, log(n), nlog(n), xno n!), si diceche f appartiene alla famiglia O (g (n))se

∃k > 0,∃n0 : ∀n > n0 f (n) < k · g (n)

Figura 6.17: Andamento di una funzione f ∈ O (g (n))

La notazione “theta” indica un asintoto mediamente seguito dalla funzione. Formalmente, date f(n) eg(n) si dice che f appartiene alla famiglia Θ (g (n))se

∃k1, k2 > 0, k1 < k2, ∃n0 : ∀n > n0 k1 · g (n) < f (n) < k2 · g (n)

65

Figura 6.18: Andamento di una funzione f ∈ Θ (g (n))

La notazione “omega” indica un limite inferiore asintoticamente mai raggiunto dalla funzione. Formal-mente, date f(n) e g(n) si dice che f appartiene alla famiglia Ω (g (n))se

∃k > 0,∃n0 : ∀n > n0 f (n) > k · g (n)

Figura 6.19: Andamento di una funzione f ∈ Ω (g (n))

La notazione utilizzata piu frequentemente e la “o grande”, poiche fornisce la garanzia che anche nel casopeggiore l’algoritmo non richiedera un ordine di risorse superiore. Ovviamente questo vantaggio perde il suosignificato nel momento in cui la funzione g (n) sia di ordine esponenziale o fattoriale.

Non e sempre possibile stabilire l’ordine di complessita a prescindere dai dati in ingresso: solitamente sicalcola la complessita computazionale distinguendo fra il caso “medio” ed il caso “peggiore”.

Appendice B - Codici sorgente

Inverse ICP

’UseInvIcp.m’

1 %% Auto-acquirement2 addpath('Mex')3 addpath('..\..\icp');4

5 global spacePressed;6

7 nPoints = 640*480;8

9 %Read setting from user10 calibMatrix = single(dlmread(input('Enter calibration file name: ','s')));11

12 percSampling = input('Enter a percentage (0-100) of point that will be randomly sampled for...ICP ([]=0.3): ');

13 if isempty(percSampling)14 percSampling = 0.3;15 end16 percSampling = percSampling/100;17

18 nAcquisitions = input('Enter number of acquisitions (≥2,[]=2): ');19 if(isempty(nAcquisitions) | | nAcquisitions<2)20 nAcquisitions=2;21 end22

23 neighWindowSize = input('Enter size of the neighbors window [h k] ([]=[3 3]): ');24 if isempty(neighWindowSize)25 neighWindowSize = [3 3];26 end27

28 matchType = input('Select match method ([]=1):\n[1] brute force\n[2] kDtree\n');29 if isempty(matchType) | | matchType == 130 matchtype = 1;31 matchString = 'bruteForce';32 else33 matchString = 'kDtree';34 end35

36 minType = input('Select minimization metric ([]=2):\n[1] point to point\n[2] point to plane...\n');

37 if isempty(minType) | | minType == 238 minType=2;39 minString = 'plane';40 else41 minString = 'point';42 end43

44 weightType = input('Select weighting: ([]=1):\n[1] auto\n[2] none\n');45 if isempty(weightType) | | weightType == 146 weightType=1;

67


47 weightString = 'auto';48 else49 weightString = 'none';50 end51

52 nIcpIter = input('Enter number of ICP iterations ([]=10): ');53 if isempty(nIcpIter)54 nIcpIter = 10;55 end56

57 optMemory = input('Optimize memory usage? ([]=no, other=yes): ','s');58 optMemory = ¬isempty(optMemory);59

60 disp('Creating lookup tables...');61 %Create neighbors lookup matrix62 neighbors = findNeighbors(neighWindowSize,[480 640]);63 %Create (L'L)\L' lookup matrix and XY pairs for data acquistion64 [XY, lLookup] = compute invXY1 lLookup(neighbors, calibMatrix, [480 640]);65 dataMatrixes = zeros(nAcquisitions,480*640,'single');66 %Preallocate indexes of subsampled points67 sampledIdx(nAcquisitions,floor(percSampling*nPoints))=uint32(0);68 neighOutlier(nAcquisitions,480*640)=uint32(0);69

70

71 % Acquisition72 % Create context for 640x480 color and shift (disparity) acquisitions73 KinectHandles = mxNiCreateContextC320S640();74 f1=figure('KeyPressFcn',@spacePress);75 % Disparity data acquisition76 disparityMatrix=mxNiDepthAcquisition(KinectHandles);77

78 % Add color and display frames79 rgbMatrix = mxNiColorAcquisition(KinectHandles);80 rgbMatrixP=permute(rgbMatrix,[3 2 1]);81 subplot(1,2,1),h1=imshow(rgbMatrixP);82 subplot(1,2,2),h2=imshow(disparityMatrix',[0 2047]);83 colormap('jet');84

85 tic86 for i=1:nAcquisitions87

88 spacePressed=0;89 while (spacePressed6=1)90 disparityMatrixes=mxNiDepthAcquisition(KinectHandles);91 set(h2,'CDATA',disparityMatrix');92 rgbMatrix=mxNiColorAcquisition(KinectHandles); rgbMatrixP=permute(rgbMatrix,[3 2 ...

1]);93 set(h1,'CDATA',rgbMatrixP);94 drawnow;95

96 end97 %Converts [640 480] images into nAcquisitions x 640*480 matrix where each98 %row contains inverse depth transform of corresponding disparity image.99

100 %[dataMatrixes(i,:), neighOutlier(i,:), sampledIdx(i,:)]=inverseDepthZ sampling nonan(...single(disparityMatrix(:,:,i)'),nPoints,calibMatrix,minType,neighWindowSize,...percSampling);

101

102 t=toc;103 disp([num2str(toc-t) ' seconds for inverse depth conversion and sampling']);104 end105

106 close(f1);107 mxNiDeleteContext(KinectHandles);108

109 % Remove for memory optimization110 if optMemory111 clear rgb;

69

112 clear disparityMatrix;113 end114

115 %% Run inverse depth ICP116 Ricp = zeros(3,3,nIcpIter+1,nAcquisitions-1,'single');117 Ticp = zeros(3,1,nIcpIter+1,nAcquisitions-1,'single');118 ER = zeros(nIcpIter+1,nAcquisitions-1,'single');119 times = zeros(nIcpIter+1,nAcquisitions-1,'single');120 %iterDurations = zeros(nIcpIter,nAcquisitions);121 disp('Running icp...');122 startMatrix = [XY([1 2],:); dataMatrixes(1,:);XY(3,:)];123 for i=1:(nAcquisitions-1)124 targetMatrix = [XY([1 2],:); dataMatrixes(i+1,:); XY(3,:)];125

126 [Ricp(:,:,:,i), Ticp(:,:,:,i), ER(:,i), times(:,i)] = inv icp(targetMatrix,startMatrix...(:,sampledIdx(i,sampledIdx(i,:)>0)),neighbors,neighOutlier(i+1,neighOutlier(i+1,:)...>0),lLookup,nIcpIter,'Minimize',minString,'Matching',matchString,'Weighting',...weightString);

127 %[¬, minErIdx] = min(ER(:,i));128 % Transform data-matrix using ICP result129 Dicp = Ricp(:,:,end,i)*startMatrix([1 2 3],:) + repmat(Ticp(:,:,end,i),1,nPoints);130

131 %Show results: Rotation and Traslation matrix and ICP's iterations times132 disp([num2str(i) '-th iteration results:']);133 Ricp(:,:,end,i)134 Ticp(:,:,end,i)135

136 %Compute times between each ICP iteration137 %iterDurations(:,i) = times([2:end],i)-times([1:(end-1)],i);138

139 % Plot model points blue and transformed points red140 f = figure;141 fig1 = subplot(2,2,1);142 plot3(targetMatrix(1,:),targetMatrix(2,:),targetMatrix(3,:),'b.',startMatrix(1,:),...

startMatrix(2,:),startMatrix(3,:),'r.');143 xlabel('x'); ylabel('y'); zlabel('z');144 title('Acquisitions: blue=targhet image');145

146 % Plot the results147 fig2 = subplot(2,2,2);148 plot3(targetMatrix(1,:),targetMatrix(2,:),targetMatrix(3,:),'b.',Dicp(1,:),Dicp(2,:),...

Dicp(3,:),'g.');149 %axis equal;150 xlabel('x'); ylabel('y'); zlabel('z');151 title('ICP result: blue=targhet image');152

153 % Plot SoSq curve154 subplot(2,2,[3 4]);155 plot(1:size(ER,1),ER(:,i),'--x');156 xlabel('number of iteration');157 ylabel('SoSq');158 title(['Total elapsed time: ' num2str(times(end,i),2) ' s' '; SoSq: ' num2str(ER(end,i...

),8)]);159

160 startMatrix = targetMatrix;161 %N.B. Actually unused time between each ICP iterations, time stamp162 %at each ICP step and changes of rototranslation matrix over the ICP steps163 % Maybe it can be useful...164 end

”findNeighbors.m”

1 function neighbors = findNeighbors(windowSize,imgSize)2 nPoints = imgSize(1)*imgSize(2);3 neigh = zeros(windowSize(1)*windowSize(2)-1,nPoints,'uint32');4

5 wHalfHeight = floor(windowSize(1)/2);


6 wHalfWeight = floor(windowSize(2)/2);7

8 for n=1:nPoints9 %get (x,y) from position n

10 x = mod(n-1,imgSize(1))+1;11 y = floor((n-1)/imgSize(1))+1;12

13 %Raw index in neigh14 neighRawIdx = 1;15

16 %Loop on neighbors window17 for i=(-wHalfHeight):wHalfHeight18 for j=(-wHalfWeight):wHalfWeight19 %Check for range validation and central point exclusion20 if(i6=0 | | j6=0) && (y+j>0) && (y+j≤imgSize(2)) && ...21 (x+i>0) && (x+i≤imgSize(1))22 neigh(neighRawIdx,n) = imgSize(1)*(y-1+j) + x+i;23 neighRawIdx = neighRawIdx+ 1;24 end25 end26 end27 end28

29 neighbors = [1:nPoints;neigh];30 end

”buildInvPointCloud.m”

1 %Build inverse depth point cloud from disparity image. Depending on2 %outlierRule:3 % 1: maintains outliers into point cloud4 % 2: exclude outliers from point cloud5 % 3: set to [nan nan nan] point corresponding with outlier6 %7 %It also provide a list of index that subsample the point cloud and a list8 %of index representing outlier's neighbours.9

10 function [invPointCloud, sampledIdx, neighOutIdx ] = buildInvPointCloud(disparityFrame, UV1..., outlierRule, calibMatrix, percSampling, minType, neighWindowSize)

11

12 nPoints = size(disparityFrame,1)*size(disparityFrame,2);13

14 neighOutIdx = [];15

16 %Get k1 and k217 k1 = calibMatrix(1,3);18 k2 = calibMatrix(2,3);19

20 %For point to plane and 'yesOut' or 'nan', dilate image and get neighOutIdx21 if (minType==2)&&(outlierRule == 1 | | outlierRule == 3)22 dilatedMatrix = disparityFrame;23

24 %dilation for neighbors of border and outlier25 structElement = strel('rectangle',neighWindowSize);26 dilatedMatrix(1,:)=2047;27 dilatedMatrix(:,1)=2047;28 dilatedMatrix(size(dilatedMatrix,1),:)=2047;29 dilatedMatrix(:,size(dilatedMatrix,2))=2047;30

31 dilatedMatrix=imdilate(dilatedMatrix,structElement);32

33 outliers = find(dilatedMatrix == 2047);34

35 neighOutIdx = uint32(outliers(¬ismember(outliers,find(disparityFrame == 2047)))');36 end37

38 %Set outliers to NaN if required

71

39 if outlierRule == 340 disparityFrame(disparityFrame == 2047) = nan;41 end42

43 %Transform into inverse depth44 invDepthZ= k1*disparityFrame(:) + k2;45 invDepthZ=invDepthZ';46

47 if outlierRule == 248 %Build point cloud without outliers if required49 goodIndexes = (invDepthZ>0);50 invPointCloud = [UV1(1,goodIndexes);UV1(2,goodIndexes);invDepthZ(goodIndexes)];51 else52 %Maintains all points on their position but set to NaN if required53 if outlierRule == 354 nanIdx = isnan(invDepthZ);55 UV1(1:2,nanIdx)=[nan;nan];56 end57

58 invPointCloud = [UV1(1,:);UV1(2,:);invDepthZ];59 end60

61 %Subsample data62 sampledIdx = unique(randi(size(invPointCloud,2),1,floor(percSampling*nPoints),'uint32')...

);63

64 %Take only no-nan or no-outlier indexes for sampling, if required65 if outlierRule == 366 sampledIdx = sampledIdx(¬isnan(invDepthZ(sampledIdx)));67

68 %Take out outlier's neighbors for point to plane minimization69 if(minType == 2)70 sampledIdx = sampledIdx(¬ismember(sampledIdx,neighOutIdx));71 end72 else73 if outlierRule == 174 sampledIdx = sampledIdx(invDepthZ(sampledIdx)>0);75

76 %Take out outlier's neighbors for point to plane minimization77 if(minType == 2)78 sampledIdx = sampledIdx(¬ismember(sampledIdx,neighOutIdx));79 end80 end81 end82 end

”computeInvXY1LLookup.m”

1 function [XY, Ltot] = compute invXY1 lLookup(neighbors, calibMatrix, imgSize)2 nPoints = imgSize(1)*imgSize(2);3

4 %Build [1 2 ... 480 1 2 ... 480 ... 1 2 ... 4805 % 1 1 ... 1 2 2 ... 2 ... 640 640 ... 6406 % 1 1 ... 1 1 1 ... 1 ... 1 1 ... 1 ]7

8 [Y,X] = meshgrid(single(1:imgSize(2)),single(1:imgSize(1)));9 Y=Y(:)';

10 X=X(:)';11

12

13 %Transform in inverse depth14 X = (X-calibMatrix(1,1))/calibMatrix(1,2); % X(i) = (i-ci)/fi15 Y = (Y-calibMatrix(2,1))/calibMatrix(2,2); % Y(j) = (j-cj)/fj16

17

18 XY=[X;Y;ones(1,size(X,2),'single')];19


20 Ltot = zeros(3,size(neighbors,1),nPoints,'single');21 for i=1:nPoints22 neigIdx = neighbors(:,i); % Like [i j k l m 0 0 0 ... 0]'23 neigIdx = neigIdx(neigIdx>0); %exclude zero values from neigbors indexes24 L= XY(:,neigIdx)';25 Ltot(:,1:size(L,1),i)=inv(L'*L)*L';26 end27

28 %Takes 5 seconds for 480x640 image29 end

”inverseDepthZSamplingNonan.m”

1 function [invDepthPointCloud, samplIdx] = inverseDepthZ sampling nonan(disparityMatrix, XY,...calibMatrix, percSampling)

2

3 % 1. Find outlier in disparityMatrix and remove them4 % 2. Transform in inverse depth and reshape5 % 3. Subsample points6 % 4. Fill matrix with zeros7

8 k1 = calibMatrix(1,3);9 k2 = calibMatrix(2,3);

10

11 invDepthZ= k1*disparityMatrix(:) + k2;12 invDepthZ=invDepthZ';13 goodIndexes = (invDepthZ>0);14 invDepthPointCloud = [XY(1,goodIndexes);XY(2,goodIndexes);invDepthZ(goodIndexes)];15

16 % On 640*480 numbers, seems that (percSampling/2)% of randomized points17 % are the same (e.g. subsampling 10% of points, 5% of these ones have18 % the same value): unique removes them from subsampled points.19 samplIdx = unique(randi(sum(goodIndexes),1,floor(percSampling*size(XY,2)),'uint32'));20

21 invDepthPointCloud(3,size(XY,2))=single(0);22

23 end

”invIcp.m”

1 %Implsementation of ICP algorithm following shema in2 % 'Martin Kjer and Jakob Wilm, Technical University of Denmark, 2012'3 % and applying inverse depth model as in 'Robust Egomotion Estimation...'4 % article.5

6 %p: inverse depth image at time t7 %q: inverse depth image at time t+18 %neighIdx: for each column i, neighIdx(j,i) contains the index in q9 % of the j-th neighbor of the i-th ponint.

10 %neighOutIdx: indexes of outlier's neighbours11 %lLookup: lookup table for normal estimation12 %nIter: number of ICP iterations13 %14 %N.B. Each point are assumed follower of this format: [u v q].15

16 function [TR, TT, ER, t] = inv icp(q,p,neighIdx,neighOutIdx,lLookup,nIter,varargin)17 % Start timer18 tic;19 inp = inputParser;20

21 validMatching = 'bruteForce', 'kDtree';22 inp.addParamValue('Matching', 'bruteForce', @(x)any(strcmpi(x,validMatching)));23

24 validMinimize = 'point','plane';25 inp.addParamValue('Minimize', 'point', @(x)any(strcmpi(x,validMinimize)));

73

26

27 validWeighting = 'auto','none';28 inp.addParamValue('Weighting', 'auto', @(x)any(strcmpi(x,validWeighting)));29

30 inp.parse(varargin:);31 arg = inp.Results;32 clear('inp');33

34 % Allocate timer vector35 t = zeros(nIter+1,1,'single');36

37 % Number of points to match38 Np = size(p,2);39

40 % Transformed data point cloud41 pt = p;42

43 % Allocate vector for SoSq of errors in every iteration.44 ER = zeros(nIter+1,1,'single');45

46 % Initialize temporary translation and rotations matrixes47 T = zeros(3,1,'single');48 R = eye(3,3,'single');49

50 % Initialize total transform vector(s) and rotation matrix(es).51 TT = zeros(3,1, nIter+1,'single');52 TR = repmat(eye(3,3), [1,1, nIter+1]);53

54 % If Minimize == 'plane', normals are needed55 if (strcmp(arg.Minimize, 'plane'))56 healedq=q;57 %healedq(:,neighOutIdx)= NaN;58 normals = normest(q(3,:),neighIdx,lLookup);59 end60

61 % If Matching == 'kDtree', a kD tree should be built62 if strcmp(arg.Matching, 'kDtree')63 %Exclude outlier's neighbors for kdtree builder64 if (strcmp(arg.Minimize, 'plane'))65 kdOBJ = KDTreeSearcher(healedq');66 else67 kdOBJ = KDTreeSearcher(q');68 end69 end70

71 %standard deviation of last match of points72 stdev=zeros(1,nIter+1,'single');73

74 t(1) = toc;75

76 % Go into main iteration loop77 for k=1:nIter78

79 % Do matching80 switch arg.Matching81 case 'bruteForce'82 if(strcmp(arg.Minimize, 'plane'))83 [match, dist] = match bruteForce(healedq(1:3,:),pt(1:3,:));84 else85 [match, dist] = match bruteForce(q(1:3,:),pt(1:3,:));86 end87 case 'kDtree'88 [match, dist] = match kDtree(q,pt,kdOBJ);89 end90

91 if k == 192 switch arg.Minimize93 case 'point'


94 ER(k) = sum(dist.ˆ2);95 case 'plane'96 ER(k)= sum((sum((p(1:3,:)-q(1:3,match)).*normals(:,match))).ˆ2);97 end98 end99

100 switch arg.Minimize101 % Determine weight vector (if requiredt) with previous rototranslation102 % matrix and previous stdev and evaluate rototranslation103 case 'point'104 switch arg.Weighting105 case 'auto'106 w = weights point point(q(1:3,match),pt,[TR(:,:,k) TT(:,:,k)],stdev...

(k));107 case 'none'108 w=[];109 end110 [R,T] = eq point(q(1:3,match),pt(1:3,:), w);111 case 'plane'112 switch arg.Weighting113 case 'auto'114 w = weights point plane(q(1:3,match),pt,[TR(:,:,k) TT(:,:,k)],stdev...

(k),normals(:,match));115 case 'none'116 w=[];117 end118 [R,T] = eq plane(q(1:3,match),pt(1:3,:),normals(:,match), w);119 end120

121 % Add to the total transformation122 TR(:,:,k+1) = R*TR(:,:,k);123 TT(:,:,k+1) = R*TT(:,:,k)+T;124

125 % Apply last transformation126 pt = TR(:,:,k+1) * p(1:3,:) + repmat(TT(:,:,k+1), 1, Np);127

128 switch arg.Minimize129 % Determine error and stdev between new rototranslation and130 % destination image131 case 'point'132 [ER(k+1),stdev(k+1)]=error point point(pt(1:3,:),q(1:3,match));133 case 'plane'134 [ER(k+1),stdev(k+1)]=error point plane(pt(1:3,:),q(1:3,match),normals(:,...

match));135 end136 t(k+1) = toc;137 end138

139 %Enable to return only last computation's results140 %TR = TR(:,:,end);141 %TT = TT(:,:,end);142

143 end144

145 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%146 % Compute the surface normal of each plane that fits each point in p and its147 % neighbors148 % v: Vector of inverse depth data on wich estimate the normals149 % neighMatrixIdx: Matrix containing, at column i, the indexes (in v) of the150 % neighbour of the point v(:,i)151 % lLookup: lookup table for normals computation152 %153 % n: 3xnPoints matrix containing, at column i, the normal vector of the154 % plane fitting point at v(i) and its neighbor155 function n = normest(v, neighMatrixIdx, lLookup )156

157 nPoints = size(v,2);158

75

159 %Reshape neighbors matrix bulding a 3D matrix where each "layer" i160 %contains indexes of neighbors of point i161 neighResh = reshape(neighMatrixIdx,size(neighMatrixIdx,1),1,nPoints);162

163 %Modify zero indexes allowing addressing (these value will not be used)164 neighResh(neighResh == 0) = 1;165

166 %Compute normals estimation167 Q = v(neighResh);168 n = mmat(lLookup,Q);169

170 %reshape normals avoiding future reshapeing171 n=reshape(n,3,nPoints);172 %Try normalising normals173 n = n./repmat(sqrt(sum(n.ˆ2)),3,1);174

175 end176

177 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%178

179 %Match using 'brute force' criteria in inverse depth space using euclidean180 %metric181 function [match, dist] = match bruteForce(q, p)182

183 nPoints = size(p,2);184 match = zeros(1,nPoints,'uint32');185 dist = zeros(1,nPoints,'single');186

187 for ki=1:nPoints188 %Compute euclidean distance d between point p(:,ki) and all points189 %in q.190 d = sum((q-repmat(p(:,ki),1,size(q,2))).ˆ2);191 [d, m] = min(d);192 dist(ki) = single(d);193 match(ki) = uint32(m);194 end195

196 dist = sqrt(dist);197

198 end199

200 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%201

202 function [match, mindist] = match kDtree(¬, p, kdOBJ)203 [match, mindist] = knnsearch(kdOBJ,p');204 match = uint32(match');205 mindist = single(mindist);206 end207

208 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%209 %Compute weights for each point pair from p to qm210 % qm: qm(:,j) contains point in q that match p(:,j)211 % p: matrix containing points to be weighted212 % previousTR: rototranslation matrix obtained by last iteration of ICP213 % previousSTD: standard deviation of last iteration of ICP214 %215 % W([3i-2, 3i-1, 3i]) vector contains weights of the association between a216 % point in p and matched point in qm: each component (p(1,i),p(2,i),p(3,i))217 % require its own weight.218 function W = weights point point(qm,p,previousTR,previousSTD)219 nPoints = size(p,2);220 %At first iteration, returns all weights as 1.221 if (previousSTD==0)222 W=ones(1,3*nPoints,'single');223 return;224 end225

226 %Compute vector of residual using the last rototranslation (TR) matrix


227 %residual(point i):=[ TR(1,:)*point i - qm(1,i);228 % TR(2,:)*point i - qm(2,i);229 % TR(3,:)*point i - qm(3,i);230 % TR(4,:)*point i - qm(4,i); ] -> ignored, always 0231 %232 %residuals:= [ residual(point 1) residual(point 2) ...233 % residual(point nPoints) ]234 residuals = previousTR*[p;ones(1,nPoints)] - qm;235 W = 1./(previousSTD + residuals(:).ˆ2);236 W=W';237 end238

239 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%240

241 % As previous, but use normal components to compute the residual242 % normals: 3xnPoints matrix containing normals243 %244 % W(i) vector containst weight associated to a single point-plane match and245 % not a triplet for each component as in previous method246 function W = weights point plane(qm,p,previousTR,previousSTD,normals)247 nPoints = size(p,2);248 %At first iteration, returns all weights as 1.249 if (previousSTD==0)250 W=ones(1,nPoints,'single');251 return;252 end253

254 %Compute vector of residual using the last rototranslation (TR) matrix255 %residual(point i):= (TR(1,:)*point i - qm(1,i))*normals(1,i) +256 % (TR(2,:)*point i - qm(2,i))*normals(2,i) +257 % (TR(3,:)*point i - qm(3,i))*normals(3,i) +258 % (TR(4,:)*point i - qm(4,i))*normals(4,i) --> ignored, always 0259 % --> normals(4,:)260 % does not exist261 %residuals:= [ residual(point 1) ; residual(point 2) ; ... ;262 % residual(point nPoints) ]263 residuals = previousTR*[p;ones(1,nPoints)] - qm;264 residuals = sum(residuals.*normals);265 W = 1./(previousSTD + residuals.ˆ2);266 end267

268 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%269

270 %Compute least square solution for (J'WJ)(ˆ-1)J'WY271 function [R, T] = eq point(qm,p,W)272

273 nPoints = size(p,2);274 %%275 % Jacobian matrix276 J=zeros(3*nPoints,6,'single');277

278 for i=1:nPoints279 J([3*i-2 3*i-1 3*i],:)= [p(3,i) , 0, 0;280 0, p(3,i), 0;281 -p(1,i)*p(3,i), -p(2,i)*p(3,i), -(p(3,i)ˆ2);282 -p(1,i)*p(2,i), -1-(p(2,i)ˆ2), -p(3,i)*p(2,i);283 1+p(1,i)ˆ2, p(1,i)*p(2,i), p(3,i)*p(1,i);284 -p(2,i), p(1,i), 0 ]';285 end286 %287 %288 % Alternative method without for loop:289 resP = reshape(p,[3,1,nPoints]);290

291 %Reshapeing and transpseing matrix Jp will change order of elements: Jp292 %must be modified with respect order modification293 Jp = [resP(3,1,:), -resP(1,1,:).*resP(2,1,:), zeros(1,1,nPoints), ...

-1-(resP(2,1,:).ˆ2), zeros(1,1,nPoints), -resP(3,1,:).*resP(2,1,:)

77

294 zeros(1,1,nPoints), 1+resP(1,1,:).ˆ2, resP(3,1,:), ...resP(1,1,:).*resP(2,1,:), zeros(1,1,nPoints), resP(3,1,:).*...

resP(1,1,:)295 -resP(1,1,:).*resP(3,1,:),-resP(2,1,:), -resP(2,1,:).*resP(3,1,:), ...

resP(1,1,:), -(resP(3,1,:).ˆ2), zeros(1,1,nPoints) ...];

296

297 %298 %Original Jp without misordinating effect299 Jp = [resP(3,1,:) , zeros(1,1,nPoints), -resP(1,1,:).*p(3,1,:), -resP(1,1,:).*resP...

(2,1,:), 1+resP(1,1,:).ˆ2, -resP(2,1,:);300 zeros(1,1,nPoints), resP(3,1,:), -resP(2,1,:).*p(3,1,:), -1-(resP(2,1,:)...

.ˆ2), resP(1,1,:).*resP(2,1,:), resP(1,1,:);301 zeros(1,1,nPoints), zeros(1,1,nPoints), -(resP(3,1,:).ˆ2), -resP(3,1,:).*resP...

(2,1,:), resP(3,1,:).*resP(1,1,:), zeros(1,1,nPoints)];302 %303 J = reshape(Jp,6,3*nPoints)';304 %305

306 %Build Y as:307 % [p(1,1)-q(1,1);308 % p(2,1)-q(2,1);309 % p(3,1)-q(3,1);310 % p(1,2)-q(1,2);311 % ... ;312 % p(3,nPoint)-q(3,nPoint)]313 Y=qm - p;314

315 %B is the least squares solution, composed by316 % B =[tx, ty, tz, thx, thy, thz]'317

318 if isempty(W)319 B = inv(J'*J)*J'*Y(:);320 else321 B = inv((J'.*repmat(W,6,1))*J)*(J'.*repmat(W,6,1))*Y(:);322 end323

324

325 T = B(1:3);326

327 R = [1, -B(6), B(5);328 B(6), 1, -B(4);329 -B(5), B(4), 1];330

331 end332

333 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%334

335 %Compute least square solution for (K'WK)(ˆ-1)K'WY336 function [R, T] = eq plane(qm,p,normals,W)337

338 nPoints = size(p,2);339

340 K =[repmat(p(3,:),3,1);p(2,:);repmat(p(1,:),2,1)]'...341 .*[normals;normals(3,:);-normals(3,:);normals(2,:)]'...342 +[zeros(3,nPoints,'single');-normals(2,:);normals(1,:);-p(2,:).*normals(1,:)]';343

344 %for i=1:nPoints345 %K(i,:)=[p(3,i)*normals(1,i) p(3,i)*normals(2,i) p(3,i)*normals(3,i) p(2,i)*...

normals(3,i)-normals(2,i) normals(1,i)-p(1,i)*normals(3,i) p(1,i)*normals(2,i)-...p(2,i)*normals(1,i)];

346 %end347

348 Y = -sum((p-qm).*normals)';349

350 if isempty(W)351 B = inv(K'*K)*K'*Y;352 else


353 B = inv((K'.*repmat(W,6,1))*K)*(K'.*repmat(W,6,1))*Y;354 end355

356 T = B(1:3);357

358 R = [1, -B(6), B(5);359 B(6), 1, -B(4);360 -B(5), B(4), 1];361 end362

363 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%364

365 %Compute error evaluation and standard deviation for point to point366 %minimization367 %qm: targhet set of points368 %p: original set of points transformed with rototranslation matrix369 %370 %ER: Least square error evaluation371 %stdev: Standard deviation of the error between matched points (residuals)372 function [ER, stdev] = error point point(qm,p)373 residuals = p-qm;374 stdev = std(residuals(:));375 ER=sum(residuals(:).ˆ2);376 end377

378 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%379

380 %Compute error evaluation and standard deviation for point to plane381 %minimization382 %qm: targhet set of points383 %p: original set of points transformed with rototranslation matrix384 %normals: 3xnPoints matrix containing normals385 %386 %ER: Least square error evaluation387 %stdev: Standard deviation of the error between matched points (residuals)388 function [ER, stdev] = error point plane(qm,p,normals)389 residuals = sum((p-qm).*normals);390 stdev = std(residuals);391 ER=sum(residuals.ˆ2);392 end

Frame Based ICP

”frameBasedIcpOnFly.m”

1 %Acquire continuouslty disparity frame and run inverse icp for each couple2

3 addpath('Mex');4 addpath('c130 v1');5

6

7 global spacePressed;8

9 nPoints = 640*480;10 settings mode = input('Select settings ([]= 1):\n[1] Read from point to point general ...

settings\n[2] Read from point to plane general settings\n[3] Manual settings\n');11 if isempty(settings mode) | | settings mode == 112 load('pt2pt icpOnFly settings.mat')13 else14 if settings mode == 215 load('pt2pl icpOnFly settings.mat')16 else17 %Read setting from user18 calibMatrix = dlmread(input('Enter calibration file name: ','s'));

79

19

20 percSampling = input('Enter a percentage (0-100) of point that will be randomly ...sampled for ICP ([]=0.3): ');

21 if isempty(percSampling)22 percSampling = 0.3;23 end24 percSampling = percSampling/100;25

26 neighWindowSize = input('Enter size of the neighbors window [h k] ([]=[3 3]): ');27 if isempty(neighWindowSize)28 neighWindowSize = [3 3];29 end30

31 matchType = input('Select match method ([]=1):\n[1] brute force\n[2] kDtree\n');32 if isempty(matchType) | | matchType == 133 matchtype = 1;34 matchString = 'bruteForce';35 else36 matchString = 'kDtree';37 end38

39 minType = input('Select minimization metric ([]=2):\n[1] point to point\n[2] point ...to plane\n');

40 if isempty(minType) | | minType == 241 minType=2;42 minString = 'plane';43 else44 minString = 'point';45 end46

47 weightType = input('Select weighting: ([]=1):\n[1] auto\n[2] none\n');48 if isempty(weightType) | | weightType == 149 weightType=1;50 weightString = 'auto';51 else52 weightString = 'none';53 end54

55 nIcpIter = input('Enter number of ICP iterations ([]=10): ');56 if isempty(nIcpIter)57 nIcpIter = 10;58 end59

60 srcOutType = input('What about outlier in source point cloud ([]=1):\n[1] Maintain ...values\n[2] Exclude from the point cloud\n[3] Set to NaN\n');

61 if isempty(srcOutType)62 srcOutType = 1;63 end64

65 trgOutType = input('What about outlier in target point cloud ([]=1):\n[1] Maintain ...values\n[2] Exclude from the point cloud\n[3] Set to NaN\n');

66 if isempty(trgOutType)67 trgOutType = 1;68 end69 end70 end71

72 disp('Creating lookup tables...');73 %Create neighbors lookup matrix74 neighbors = findNeighbors(neighWindowSize,[480 640]);75 %Create (L'L)\L' lookup matrix and U-V-ONES sets for data acquistion76 [UV1, lLookup] = compute invXY1 lLookup(neighbors, calibMatrix, [480 640]);77

78 %For airplane dislpay79 centerOfPlane = [0 0 0];80 old pitch deg = 0; %degree81 old roll deg = 0; %degree82 old yaw deg= 0; %degree


83

84 % Acquisition85 % Create context for 640x480 color and shift (disparity) acquisitions86 KinectHandles = mxNiCreateContextC320S640();87 f1=figure('KeyPressFcn',@spacePress);88

89 % Disparity data acquisition90 source disparityFrame=mxNiDepthAcquisition(KinectHandles);91

92 % Build source inverse depth point cloud, get indexes of sampled point and93 % indexes of outlier's neighbors94 [source invDepth pointCloud, source sampling] = buildInvPointCloud(single(...

source disparityFrame'), UV1, srcOutType, calibMatrix, percSampling, minType, ...neighWindowSize);%'noOut','nan'

95

96 % Add color and display frames97 rgbFrame = mxNiColorAcquisition(KinectHandles);98 rgbFrameP=permute(rgbFrame,[3 2 1]);99 subplot(1,3,1),h1=imshow(rgbFrameP);

100 subplot(1,3,2),h2=imshow(source disparityFrame',[0 2047]);101 subplot(1,3,3), h3 = c130(centerOfPlane,'wing','c','fin','r','fuselage','b','tailwing','r',...

'scale',42.5);102 axis([-2000 2000 -2000 2000 -2000 2000]);103

104 %%subplot(1,1,1),h2=imshow(source disparityFrame',[0 2047]);105 colormap('jet');106

107 RTicp = []; %it will contains final rototranslation matrixes108 %Acquisition loop109 spacePressed = 0;110

111 while(spacePressed6=1)112 %Get target disparity frame and show disparity and rgb frames113 target disparityFrame=mxNiDepthAcquisition(KinectHandles);114 set(h2,'CDATA',target disparityFrame');115 rgbFrame=mxNiColorAcquisition(KinectHandles); rgbFrameP=permute(rgbFrame,[3 2 1]);116 set(h1,'CDATA',rgbFrameP);117 drawnow;118

119 % Build source inverse depth point cloud (target sampling required only120 % to copy it to next source sampling, but it useful only with 'yesOut'121 % point cloud builder122

123 [target invDepth pointCloud, target sampling , target neighOut] = buildInvPointCloud(...single(target disparityFrame'), UV1, trgOutType, calibMatrix, percSampling, minType..., neighWindowSize);

124

125 %Run inverse depth icp126 %[Ricp all, Ticp all, ER all, times all] = inv icp(target invDepth pointCloud,...

source invDepth pointCloud(:,source sampling),neighbors,target neighOut,lLookup,...nIcpIter,'Minimize',minString,'Matching',matchString,'Weighting',weightString);

127 [Ricp all, Ticp all, ER all, times all] = icp(target invDepth pointCloud,...source invDepth pointCloud(:,source sampling),nIcpIter,'Matching',matchString,'...Minimize',minString,'ReturnAll',true);

128 %Choose RT that minimize metric error129 [¬, minErIdx] = min(ER all);130 RTicp = cat(3,RTicp,[Ricp all(:,:,minErIdx), Ticp all(:,:,minErIdx)]);131

132 %plane draw133 centerOfPlane = centerOfPlane + RTicp(:,4,end)'*100; %Wrong!134 new roll deg= old roll deg + radtodeg(asin(RTicp(2,1,end))); %arcsin(theta z) ...

to degree135 new pitch deg = old pitch deg + radtodeg(asin(RTicp(1,3,end)));%arcsin(theta y)...

to degree136 new yaw deg = old yaw deg + radtodeg(asin(RTicp(3,2,end)));%arcsin(theta x) to ...

degree137

138

81

139 h3=c130(centerOfPlane,'roll',new roll deg,'pitch',new pitch deg,'yaw',...new yaw deg,'wing','c','fin','r','fuselage','b','tailwing','r','scale'...,42.5);

140 axis([-2000 2000 -2000 2000 -2000 2000]);141

142 old roll deg = new roll deg;143 old pitch deg = new pitch deg;144 old yaw deg = new yaw deg;145 xlabel(['roll: ' num2str(new roll deg) ' pitch: ' num2str(new pitch deg) ' yaw:...

' num2str(new yaw deg) ' T: [ ' num2str(centerOfPlane) ' ]']);146

147 % Copy target image to source image. Additional buildPointCloud call is148 % requred if source and target pointcloud work with different rules149 % about outliers150 if(srcOutType == trgOutType)151 source invDepth pointCloud = target invDepth pointCloud;152 source sampling = target sampling;153 else154 [source invDepth pointCloud, source sampling] = buildPointCloud(...

target disparityFrame', UV1, srcOutType);155 end156 end157

158 mxNiDeleteContext(KinectHandles);159 close(f1);

”frameBasedBuildInvFrame.m”

1 %Build inverse depth frame from disparity image, appending matrix with2 %linearized indexes. Depending on satRule:3 %4 % 1: maintains saturated values into point cloud5 %6 % 2: set to [nan nan nan] point corresponding with saturated value7 %8 %It also provide a list of index that subsample the frame9

10 function [invFrame, sampledIdx ] = frameBased buildInvFrame(disparityFrame, UV, satRule, ...calibMatrix, percSampling)

11

12 nRows = size(disparityFrame,1);13 nCols = size(disparityFrame,2);14 nPoints = nRows*nCols;15 %Get k1 and k216 k1 = calibMatrix(1,3);17 k2 = calibMatrix(2,3);18

19 %Transform into inverse depth20 invDepthZ= k1*single(disparityFrame) + k2;21

22 invFrame = cat(3,UV,invDepthZ);23 linIdxMatrix = reshape(1:nPoints,size(disparityFrame));24 invFrame = cat(3,invFrame,linIdxMatrix);25

26 %Maintains all points on their position but set to NaN if required27 if satRule == 228 invDepthZ(invDepthZ < 0) = nan; %set to NaN in original inverse depth frame29 invDepthZ = abs(invDepthZ)./invDepthZ; %build only ones (valid positions) or NaN (...

invalid positions)30 invFrame = invFrame.*repmat(invDepthZ,[1 1 size(invFrame,3)]); %set valid or ...

invalid positions to invFrame31 end32

33 %Subsample data34 if(percSampling == 1)35 sampledIdx = 1:nPoints;36 return;


37 end38 sampledIdx = unique(randi(nPoints,1,floor(percSampling*nPoints),'uint32'));39

40 %Take only no-nan or no-saturated indexes for sampling, if required41 if satRule == 242 sampledIdx = sampledIdx(¬isnan(invDepthZ(sampledIdx)));43 else44 sampledIdx = sampledIdx(invDepthZ(sampledIdx)>0);45 end46

47 %Exclude points near borders, with more than 25 pixel from these48 leftBorder = linIdxMatrix(1:nRows,1:25);49 rightBorder = linIdxMatrix(1:nRows,(nCols-25):nCols);50 topBorder = linIdxMatrix(1:25,1:nCols);51 downBorder = linIdxMatrix((nRows-25):nRows,1:nCols);52

53 sampledIdx = sampledIdx(¬ismember(sampledIdx,unique([leftBorder(:); rightBorder(:); ...topBorder(:); downBorder(:)])));

54

55 end

”frameBasedBuildInvUV.m”

1 function [UV] = frameBased build invUV(calibMatrix, imgSize)2

3 %Build frame of inverse depth coordinates from pixel addresses4

5 [X,Y] = meshgrid(single(1:imgSize(1)),single(1:imgSize(2)));6 X = X';7 Y=Y';8

9

10 %Transform in inverse depth11 U = (X-calibMatrix(1,1))/calibMatrix(1,2); % X(i) = (i-ci)/fi12 V = (Y-calibMatrix(2,1))/calibMatrix(2,2); % Y(j) = (j-cj)/fj13

14 UV=cat(3,U,V);15

16 end

”frameBasedInvIcp.m”

1 %Implsementation of ICP algorithm following frame based approach.2

3 %pCloud: inverse depth point cloud at time t. pCloud([1 2 3],:) represents4 %inverse depth value at coordinate ind2sub(pCloud(4,:));5 %qFrame: inverse depth frame at time t+16 %nIter: number of ICP iterations7 %neighWindowSize: dimension of window on wich find nearest neighbours8

9 function [TR, TT, ER, t] = frameBased inv icp(qFrame,pCloud,nIter,neighWindowSize,...calibMatrix,varargin)

10 % Start timer11 tic;12 inp = inputParser;13

14 validMinimize = 'point','plane';15 inp.addParamValue('Minimize', 'point', @(x)any(strcmpi(x,validMinimize)));16

17 validMinApproach = 'leastsquare','standard';18 inp.addParamValue('MinApproach','standard',@(x)any(strcmpi(x,validMinApproach)));19

20 validNormBasin = 'nearestPoints','flattestWindow';21 inp.addParamValue('NormBasin', 'nearestPoints', @(x)any(strcmpi(x,validNormBasin)));22

83

23 validNormEstimation = 'PCA','leastSquare';24 inp.addParamValue('NormEstimation', 'PCA', @(x)any(strcmpi(x,validNormEstimation)));25

26 validWeighting = 'auto','none';27 inp.addParamValue('Weighting', 'auto', @(x)any(strcmpi(x,validWeighting)));28

29 inp.addParamValue('Rejection', 0, @(x)isscalar(x) && x > 0 && x < 1);30

31 inp.parse(varargin:);32 arg = inp.Results;33 clear('inp','validMinimize','validMinApproach','validNormBasin','validNormEstimation','...

validWeighting','varargin');34

35 nPoints = size(qFrame,1)*size(qFrame,2);36 % Allocate timer vector37 t = zeros(nIter+1,1,'single');38

39 % Transformed data point cloud40 ptCloud = pCloud;41

42 % Allocate vector for SoSq of errors43 ER = zeros(nIter+1,1,'single');44

45 % Initialize temporary translation and rotation matrixes46 T = zeros(3,1,'single');47 R = eye(3,3,'single');48

49 % Initialize total transform vector(s) and rotation matrix(es).50 TT = zeros(3,1, nIter+1,'single');51 TR = repmat(eye(3,3), [1,1, nIter+1]);52

53 %standard deviation of last match of points54 stdev=zeros(1,nIter+1,'single');55

56 %normals vector57 normals = [];58

59 %Define minimization approach algorithm60 switch arg.MinApproach61 case 'leastsquare'62 eq point = @lsq eq point;63 eq plane = @lsq eq plane;64 case 'standard'65 eq point = @std eq point;66 eq plane = @std eq plane;67 end68

69 badIndexes = []; %Define out of bound indexes vector70 t(1) = toc;71

72 % Go into main iteration loop73 for k=1:nIter74

75 % Do matching76 [match, dist] = match nearestPoint(qFrame,ptCloud,neighWindowSize);77

78 %Discard match with only NaN region79 badMatch = [];80 if(sum(match==0)>0)81 badMatch = match==0;82 match(badMatch) = [];83 dist(badMatch)=[];84 ptCloud(:,badMatch)=[];85 end86

87 %Reject a percentage of bad corrispondeces88 if arg.Rejection89 rejIdx = dist>arg.Rejection;


90 match(rejIdx)=[];91 ptCloud(:,rejIdx)=[];92 dist(rejIdx)=[];93 end94

95 %Update matched point deleting bad corrispondences96 qMatchedCloud = qFrame([match; match+nPoints; match+(2*nPoints)]);97

98 % If Minimize == 'plane', normals are needed99 if (strcmp(arg.Minimize, 'plane'))

100 switch arg.NormEstimation101 case 'PCA'102 if(k==1)103 normals = pcaNormEst(qFrame,unique(match),neighWindowSize,arg....

NormBasin);104 else105 %Evaluate normals for new qFrame's points106 normals = [normals, pcaNormEst(qFrame,unique(match(¬ismember(match,...

normals(4,:)))),neighWindowSize,arg.NormBasin)];107 end108 case 'leastSquare'109 if(k==1)110 normals = lsqNormEst(qFrame,unique(match),neighWindowSize,arg....

NormBasin);111 else112 %Evaluate normals for new qFrame's points113 normals = [normals, lsqNormEst(qFrame,unique(match(¬ismember(match,...

normals(4,:)))),neighWindowSize,arg.NormBasin)];114 end115 end116

117 %Build normals vector with only normal used in iteration k118 normalsUseful = zeros(3,size(match,2),'single');119 for i=1:size(match,2)120 normalsUseful(:,i) = normals(1:3,normals(4,:)==match(i));121 end122 %123 %Alternative method124 normalsUseful = [];125 for m=match126 normalsUseful = [normalsUseful, normals(1:3,normals(4,:)==m)];127 end128 %129 end130

131 if k == 1132 switch arg.Minimize133 case 'point'134 ER(k) = sum(dist.ˆ2);135 case 'plane'136 ER(k)= sum((sum((ptCloud(1:3,:)-qMatchedCloud).*normalsUseful)).ˆ2);137 end138 end139

140 switch arg.Minimize141 % Determine weight vector (if required) with previous rototranslation142 % matrix and previous stdev and evaluate rototranslation143 case 'point'144 switch arg.Weighting145 case 'auto'146 w = weights point point(qMatchedCloud,ptCloud(1:3,:),[TR(:,:,k) TT...

(:,:,k)],stdev(k));147 case 'none'148 w=[];149 end150

151 %Evaluate rototranslation matrix152 try

85

153 [R,T] = eq point(qMatchedCloud,ptCloud(1:3,:), w);154 catch err155 disp([2]);156 end157

158 case 'plane'159 switch arg.Weighting160 case 'auto'161 w = weights point plane(qMatchedCloud,ptCloud(1:3,:),[TR(:,:,k) TT...

(:,:,k)],stdev(k),normalsUseful);162 case 'none'163 w=[];164 end165

166 %Evaluate rototranslation matrix167 [R,T] = eq plane(qMatchedCloud,ptCloud(1:3,:),normalsUseful, w);168 end169

170 % Add to the total transformation171 TR(:,:,k+1) = R*TR(:,:,k);172 TT(:,:,k+1) = R*TT(:,:,k)+T;173

174 % Build point cloud for alignment175 pointsForAlignment = pCloud;176

177 %Discard points out of qFrame's bound178 if(¬isempty(badIndexes))179 pointsForAlignment(:,badIndexes)=[];180 end181

182 %Discard points surrounded bya full NaN region183 if(¬isempty(badMatch))184 pointsForAlignment(:,badMatch)=[];185 end186

187 %Align internal points building point cloud for error estimation188 pointsForErrorMetric = TR(:,:,k+1) * pointsForAlignment(1:3,:) + repmat(TT(:,:,k+1)...

, 1, size(pointsForAlignment,2));189

190 %Assemble point cloud to evaluate error metric with matched and not191 %rejected points192 if(arg.Rejection)193 pointsForErrorMetric(:,rejIdx)=[];194 end195

196 switch arg.Minimize197 % Determine error and stdev between new rototranslation and198 % destination image199 case 'point'200 [ER(k+1),stdev(k+1)]=error point point(pointsForErrorMetric(1:3,:),...

qMatchedCloud);201 case 'plane'202 [ER(k+1),stdev(k+1)]=error point plane(pointsForErrorMetric(1:3,:),...

qMatchedCloud,normalsUseful);203 end204

205 ptCloud = TR(:,:,k+1) * pCloud(1:3,:) + repmat(TT(:,:,k+1), 1, size(pCloud,2));206 %Add linearized indexes and exclude external points.207 [idxRow, badIndexes] = linearizedIndexes(ptCloud(1:2,:),calibMatrix,[size(qFrame,1)...

, size(qFrame,2)]);208 ptCloud(:,badIndexes)=[];209 ptCloud=[ptCloud;idxRow];210

211 t(k+1) = toc;212 end213

214 %Enable to return only last computation's results215 %TR = TR(:,:,end);


216 %TT = TT(:,:,end);217

218 end219

220 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%221 % Compute the surface normal of each plane that fits matched points in222 % frame and their neighbors using PCA.223 %224 % H. Hoppe, T. DeRose, T. Duchamp, J. McDonald, and W. Stuetzle.225 % Surface reconstruction from unorganized points.226 % In Proceedings of ACM Siggraph, pages 71:78, 1992.227 %228 % frame: 3D matrix containing inverse depth points229 % matchedPointsIdx: linearized indexes of point in frame on wich estimate the normal230 % neighWindowSize: dimension of windows where search for neighbors231 % normBasin: describe kind of search into the windows: search only nearest232 % points or search for flattest plane surrounding point.233 %234 % n: 4xnPoints matrix following this model:235 % n(1:3,:) contains normal's component236 % n(4,:) contains linearized indexes of related point into frame's237 % coordinates238 function n = pcaNormEst(frame,matchedPointsIdx,neighWindowSize,normBasin)239

240 n = [];241 %Quit if there's no points on wich estimate normal242 if isempty(matchedPointsIdx)243 return;244 end245

246 switch normBasin247 case 'nearestPoints'248 getNeighPointCloud = @nearestPointsSelection;249 case 'flattestWindow'250 getNeighPointCloud = @flattestWindowSelection;251 end252

253 m = size(matchedPointsIdx,2);254 n = zeros(4,m);255

256 for i = 1:m257 [coorX,coorY] = ind2sub(size(frame),matchedPointsIdx(i));258 neighPointCloud = getNeighPointCloud(frame,squeeze(frame(coorX,coorY,:)),...

neighWindowSize);259

260 p bar = 1/size(neighPointCloud,2) * sum(neighPointCloud,2);261

262 P = (neighPointCloud - repmat(p bar,1,size(neighPointCloud,2))) * transpose(...neighPointCloud - repmat(p bar,1,size(neighPointCloud,2))); %spd matrix P

263 %P = 2*cov(x);264

265 [V,D] = eig(P);266

267 [¬, idx] = min(diag(D)); % choses the smallest eigenvalue268

269 n(1:3,i) = V(:,idx); % returns the corresponding eigenvector270 n(4,i) = matchedPointsIdx(i);271 end272

273 %Normalize normals274 %n = n./repmat(sqrt(sum(n.ˆ2)),3,1);275 end276

277 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%278 % Compute the surface normal of each plane that fits matched points in279 % frame and their neighbors using Least Square method.280 %281 % frame: 3D matrix containing inverse depth point

87

282 % matchedPointIdx: linearized indexes of point in frame on wich estimate the normal283 % neighWindowSize: dimension of windows where search for neighbors284 % normBasin: describe kind of search into the windows: search only nearest285 % points or search for flattest plane surrounding point.286 %287 % n: 4xnPoints matrix following this model:288 % n(1:3,:) contains normal's component289 % n(4,:) contains linearized indexes of related point into frame's290 % coordinates291 function n = lsqNormEst(frame,matchedPointsIdx,neighWindowSize,normBasin)292

293 n=[];294

295 if isempty(matchedPointsIdx)296 return;297 end298

299 switch normBasin300 case 'nearestPoints'301 getNeighPointCloud = @nearestPointsSelection;302 case 'flattestWindow'303 getNeighPointCloud = @flattestWindowSelection;304 end305

306 m = size(matchedPointsIdx,2);307 n = zeros(4,m);308 for i=1:m309 [coorX,coorY] = ind2sub(size(frame),matchedPointsIdx(i));310 neighPointCloud = getNeighPointCloud(frame,squeeze(frame(coorX,coorY,:)),...

neighWindowSize);311 L = [neighPointCloud(1:2,:)', ones(size(neighPointCloud,2),1)];312 Q = neighPointCloud(3,:)';313

314 n(1:3,i) = inv(L'*L)*L'*Q;315 n(4,i) = matchedPointsIdx(i);316 end317

318 %Normalize normals319 n(1:3,:) = n(1:3,:)./repmat(sqrt(sum(n(1:3,:).ˆ2)),3,1);320 end321

322 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%323

324 %With size(windowSize)≥ [6 6], found nearest 8 neighbors of point at pivot325 % into a window of windowSize dimension326 function pointCloud = nearestPointsSelection(frame,srcPoint,windowSize)327

328 pointCloud = getNeighPointCloud(frame,srcPoint,windowSize,9,false);329

330 end331

332 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%333

334 %Found flattest 3x3 window containing point at pivot position335 function pointCloud = flattestWindowSelection(frame, srcPoint, ¬)336

337 %Get 5x5 window, composed by 9 3x3 window containing point at pivot338 searchField = getSurroundingWindow(frame, [5 5], srcPoint(4));339

340 %Get 9 3x3 matrixes containing point at pivot and evaluate they're341 %condition number342 windows = [];343 condDescriptor = [];344

345 nVerticalMatrix = min(size(searchField,1)-2,3);346 nHorizontalMatrix = min(size(searchField,2)-2,3);347 for c=1:nHorizontalMatrix348 for r=1:nVerticalMatrix


349 windows = cat(4,windows,searchField(r:r+2,c:c+2,:));350 windows(isnan(windows))=0;351 condDescriptor = [condDescriptor, cond(windows(:,:,3,end))];352 end353 end354

355 %Get higher condition number and related matrix356 [¬, flattestWindowIdx] = max(condDescriptor);357 flattestWindow = windows(:,:,:,flattestWindowIdx);358

359 %Build point cloud from flattest matrix360 pointCloud = flattestWindow([1:9;10:18;19:27]);361

362 end363

364 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%365

366 % Return linearized indexes and euclidean distances of points in qFrame367 % with best neighborhood to points into ptCloud368 function [match, dist] = match nearestPoint(qFrame,ptCloud,neighWindowSize)369 match = zeros(1,size(ptCloud,2),'uint32');370 dist = zeros(1,size(ptCloud,2),'single');371

372 for i=1:size(ptCloud,2)373 nearestPoint = getNeighPointCloud(qFrame,ptCloud(:,i),neighWindowSize,1,true);374 match(i) =nearestPoint(4);375 dist(i) = sqrt(sum((nearestPoint(1:3) - ptCloud(1:3,i)).ˆ2));376 end377 end378

379 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%380

381 %Returns a point cloud containing closest nNeighRes points into frame from382 %srcPoint, searching into a window of size neighWindowSize and adding a383 %fourth row containing linearized indexes if required.384 %If srcPoint is member of frame, it will be the first point into385 %pointCloud.386 function pointCloud = getNeighPointCloud(frame, srcPoint, neighWindowSize, nNeighRes, ...

addLinIndex)387

388 %Build pivot's surrounding window with respect to frame's dimension389 [window] = getSurroundingWindow(frame, neighWindowSize, srcPoint(4));390 nNeighbors = size(window,1)*size(window,2);391 windowIdxs = 1:nNeighbors;392 pointCloud = window([windowIdxs; windowIdxs+nNeighbors; windowIdxs+(2*nNeighbors)]);393

394 %If addLinIndex, add another row containing linearized indexes of each395 %point396 if(addLinIndex)397 pointCloud = [pointCloud;window(windowIdxs+(3*nNeighbors))];398 end399

400 %Evaluate distance to each point in window401 srcDistancesSqr = sum((pointCloud(1:3,:) - repmat(srcPoint(1:3),1,nNeighbors)).ˆ2);402

403 %Find nearest point from srcPoint and return it as best match404 [¬,order] = sort(srcDistancesSqr);405 pointCloud = pointCloud(:,order);406 pointCloud = pointCloud(:,1:nNeighRes);407

408 end409

410 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%411

412 function [surrWindow, top, down, left, right] = getSurroundingWindow(frame, windowSize, ...positionIdx)

413 [x, y] = ind2sub(size(frame),positionIdx);414

89

415 %Get upper and lower bound for neighbor window416 top = max(1,x-floor(windowSize(1)/2));417 down = min(size(frame,1),x+floor(windowSize(1)/2));418 left = max(1,y-floor(windowSize(2)/2));419 right = min(size(frame,2),y+floor(windowSize(2)/2));420

421 %Build pivot's surrounding window with respect to frame's dimension422 surrWindow = frame(top:down, left:right,:);423

424 end425

426 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%427

428 % Build a set of linearized indexes from pairs of inverse depth coordinates429 % Return such vector and another vector of "bad indexes", representing430 % point outside the frame's bound.431 function [linIdx, badIdx] = linearizedIndexes(oldCloudCoordinates,calibMatrix,frameSize)432 %Convert inverse depth coordinates (1/m) to frame coordinates (pixel)433 X = oldCloudCoordinates(1,:).*calibMatrix(1,2) + repmat(calibMatrix(1,1),1,size(...

oldCloudCoordinates,2));434 Y = oldCloudCoordinates(2,:).*calibMatrix(2,2) + repmat(calibMatrix(2,1),1,size(...

oldCloudCoordinates,2));435

436 %Logical indexes of points exceeding frame's bound437 badIdx= logical((X<1) + (X>frameSize(1)) + (Y<1) + (Y>frameSize(2)));438

439 %Exclude external pixel coordinates440 X(badIdx) = [];441 Y(badIdx) = [];442

443 linIdx = sub2ind(frameSize,round(X),round(Y));444 end445

446 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%447 %Compute weights for each point pair from p to qm448 % qm: qm(:,j) contains point that match p(:,j)449 % p: matrix containing points to be weighted450 % previousTR: rototranslation matrix obtained by last iteration of ICP451 % previousSTD: standard deviation of last iteration of ICP452 %453 % W([3i-2, 3i-1, 3i]) vector contains weights of the association between a454 % point p(:,i) and matched point in qm: each component (p(1,i),p(2,i),p(3,i))455 % require its own weight.456 function W = weights point point(qm,p,previousTR,previousSTD)457 nPoints = size(p,2);458 %At first iteration, returns all weights as 1.459 if (previousSTD==0)460 W=ones(1,3*nPoints,'single');461 return;462 end463

464 %Compute vector of residual using the last rototranslation (TR) matrix465 %residual(point i):=[ TR(1,:)*point i - qm(1,i);466 % TR(2,:)*point i - qm(2,i);467 % TR(3,:)*point i - qm(3,i);468 % TR(4,:)*point i - qm(4,i); ] -> ignored, always 0469 %470 %residuals:= [ residual(point 1) residual(point 2) ...471 % residual(point nPoints) ]472 residuals = previousTR*[p;ones(1,nPoints)] - qm;473 W = 1./(previousSTD + (residuals(:).ˆ2));474 W=W';475 end476

477 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%478

479 % As previous, but use normal components to compute the residual480 % normals: 3xnPoints matrix containing normals


481 %482 % W(i) vector containst weight associated to a single point-plane match and483 % not a triplet for each component as in previous method484 function W = weights point plane(qm,p,previousTR,previousSTD,normals)485 nPoints = size(p,2);486 %At first iteration, returns all weights as 1.487 if (previousSTD==0)488 W=ones(1,nPoints,'single');489 return;490 end491

492 %Compute vector of residual using the last rototranslation (TR) matrix493 %residual(point i):= (TR(1,:)*point i - qm(1,i))*normals(1,i) +494 % (TR(2,:)*point i - qm(2,i))*normals(2,i) +495 % (TR(3,:)*point i - qm(3,i))*normals(3,i) +496 % (TR(4,:)*point i - qm(4,i))*normals(4,i) --> ignored, always 0497 % --> normals(4,:)498 % does not exist499 %residuals:= [ residual(point 1) ; residual(point 2) ; ... ;500 % residual(point nPoints) ]501 residuals = previousTR*[p;ones(1,nPoints)] - qm;502 residuals = sum(residuals.*normals);503 residuals(isnan(residuals)) = 0;504 W = 1./(previousSTD + (residuals.ˆ2));505 end506

507 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%508

509 %Compute least square (weighted) solution (J'WJ)(ˆ-1)J'WY510 function [R, T] = lsq eq point(qm,p,W)511

512 nPoints = size(p,2);513 %%514 % Jacobian matrix515 J=zeros(3*nPoints,6,'single');516

517 for i=1:nPoints518 J([3*i-2 3*i-1 3*i],:)= [p(3,i) , 0, 0;519 0, p(3,i), 0;520 -p(1,i)*p(3,i), -p(2,i)*p(3,i), -(p(3,i)ˆ2);521 -p(1,i)*p(2,i), -1-(p(2,i)ˆ2), -p(3,i)*p(2,i);522 1+p(1,i)ˆ2, p(1,i)*p(2,i), p(3,i)*p(1,i);523 -p(2,i), p(1,i), 0 ]';524 end525 %526 %527 % Alternative method without for loop:528 resP = reshape(p,[3,1,nPoints]);529

530 %Reshapeing and transpseing matrix Jp will change order of elements: Jp531 %must be modified with respect order modification532 Jp = [resP(3,1,:), -resP(1,1,:).*resP(2,1,:), zeros(1,1,nPoints), ...

-1-(resP(2,1,:).ˆ2), zeros(1,1,nPoints), -resP(3,1,:).*resP(2,1,:)533 zeros(1,1,nPoints), 1+resP(1,1,:).ˆ2, resP(3,1,:), ...

resP(1,1,:).*resP(2,1,:), zeros(1,1,nPoints), resP(3,1,:).*...resP(1,1,:)

534 -resP(1,1,:).*resP(3,1,:),-resP(2,1,:), -resP(2,1,:).*resP(3,1,:), ...resP(1,1,:), -(resP(3,1,:).ˆ2), zeros(1,1,nPoints) ...

];535

536 %537 %Original Jp without misordinating effect538 Jp = [resP(3,1,:) , zeros(1,1,nPoints), -resP(1,1,:).*p(3,1,:), -resP(1,1,:).*resP...

(2,1,:), 1+resP(1,1,:).ˆ2, -resP(2,1,:);539 zeros(1,1,nPoints), resP(3,1,:), -resP(2,1,:).*p(3,1,:), -1-(resP(2,1,:)...

.ˆ2), resP(1,1,:).*resP(2,1,:), resP(1,1,:);540 zeros(1,1,nPoints), zeros(1,1,nPoints), -(resP(3,1,:).ˆ2), -resP(3,1,:).*resP...

(2,1,:), resP(3,1,:).*resP(1,1,:), zeros(1,1,nPoints)];

91

541 %542 J = reshape(Jp,6,3*nPoints)';543 %544

545 %Build Y as:546 % [p(1,1)-q(1,1);547 % p(2,1)-q(2,1);548 % p(3,1)-q(3,1);549 % p(1,2)-q(1,2);550 % ... ;551 % p(3,nPoint)-q(3,nPoint)]552 Y=qm - p;553

554 %B is the least squares solution, composed by555 % B =[tx, ty, tz, thx, thy, thz]'556

557 if isempty(W)558 B = inv(J'*J)*J'*Y(:);559 else560 B = inv((J'.*repmat(W,6,1))*J)*(J'.*repmat(W,6,1))*Y(:);561 end562

563

564 T = B(1:3);565

566 R = [1, -B(6), B(5);567 B(6), 1, -B(4);568 -B(5), B(4), 1];569

570 end571

572 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%573

574 %Compute least square solution for (K'WK)(ˆ-1)K'WY575 function [R, T] = lsq eq plane(qm,p,normals,W)576

577 nPoints = size(p,2);578

579 %Discard ponts with NaN normals580 normals(isnan(normals))=0;581

582 K =[repmat(p(3,:),3,1);p(2,:);repmat(p(1,:),2,1)]'...583 .*[normals;normals(3,:);-normals(3,:);normals(2,:)]'...584 +[zeros(3,nPoints,'single');-normals(2,:);normals(1,:);-p(2,:).*normals(1,:)]';585

586 %for i=1:nPoints587 %K(i,:)=[p(3,i)*normals(1,i) p(3,i)*normals(2,i) p(3,i)*normals(3,i) p(2,i)*...

normals(3,i)-normals(2,i) normals(1,i)-p(1,i)*normals(3,i) p(1,i)*normals(2,i)-...p(2,i)*normals(1,i)];

588 %end589

590 Y = -sum((p-qm).*normals)';591

592 if isempty(W)593 B = inv(K'*K)*K'*Y;594 else595 B = inv((K'.*repmat(W,6,1))*K)*(K'.*repmat(W,6,1))*Y;596 end597

598 T = B(1:3);599

600 R = [1, -B(6), B(5);601 B(6), 1, -B(4);602 -B(5), B(4), 1];603 end604

605 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%606


607 %Evaluate rotation and translation matrixes from q and p point clouds608 %using Singular Value Decomposition (standard approach from icp.m)609 function [R,T] = std eq point(q,p,weights)610

611 m = size(p,2);612 n = size(q,2);613

614 if(isempty(weights))615 weights = ones(1,3*size(q,2),'single');616 end617

618 % normalize weights619 weights = weights ./ sum(weights);620

621 % find data centroid and deviations from centroid622 q bar = sum(q .* reshape(weights,[3, size(q,2)]),2);623 q mark = q - repmat(q bar, 1, n);624 % Apply weights625 q mark = q mark .* reshape(weights,[3, size(q,2)]);626

627 % find data centroid and deviations from centroid628 p bar = sum(p .* reshape(weights,[3, size(p,2)]),2);629 p mark = p - repmat(p bar, 1, m);630 % Apply weights631 %p mark = p mark .* repmat(weights, 3, 1);632

633 N = p mark*transpose(q mark); % taking points of q in matched order634

635 [U,¬,V] = svd(N); % singular value decomposition636

637 R = V*diag([1 1 det(U*V')])*transpose(U);638

639 T = q bar - R*p bar;640 end641 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%642

643 %Evaluate rotation and translation matrixes from q and p point clouds644 %using linear system solution (standard approach from icp.m)645 function [R,T] = std eq plane(q,p,n,weights)646

647 if(isempty(weights))648 weights = ones(1,size(n,2),'single');649 end650

651 n = n .* repmat(weights,3,1);652

653 %Discard NaN normals654 n(isnan(n))=0;655

656 c = cross(p,n);657

658 cn = vertcat(c,n);659

660 C = cn*transpose(cn);661

662 b = - [sum(sum((p-q).*repmat(cn(1,:),3,1).*n));663 sum(sum((p-q).*repmat(cn(2,:),3,1).*n));664 sum(sum((p-q).*repmat(cn(3,:),3,1).*n));665 sum(sum((p-q).*repmat(cn(4,:),3,1).*n));666 sum(sum((p-q).*repmat(cn(5,:),3,1).*n));667 sum(sum((p-q).*repmat(cn(6,:),3,1).*n))];668

669 X = C\b;670

671 cx = cos(X(1)); cy = cos(X(2)); cz = cos(X(3));672 sx = sin(X(1)); sy = sin(X(2)); sz = sin(X(3));673

674 R = [cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz;

93

675 cy*sz cx*cz+sx*sy*sz cx*sy*sz-cz*sx;676 -sy cy*sx cx*cy];677

678 T = X(4:6);679 end680 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%681

682 %Compute error evaluation and standard deviation for point to point683 %minimization684 %qm: targhet set of points685 %p: original set of points transformed with rototranslation matrix686 %687 %ER: Least square error evaluation688 %stdev: Standard deviation of the error between matched points (residuals)689 function [ER, stdev] = error point point(qm,p)690 residuals = p-qm;691 stdev = std(residuals(:));692 ER=sum(residuals(:).ˆ2);693 end694

695 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%696

697 %Compute error evaluation and standard deviation for point to plane698 %minimization699 %qm: targhet set of points700 %p: original set of points transformed with rototranslation matrix701 %normals: 3xnPoints matrix containing normals702 %703 %ER: Least square error evaluation704 %stdev: Standard deviation of the error between matched points (residuals)705 function [ER, stdev] = error point plane(qm,p,normals)706 normals(isnan(normals))=0;707 residuals = sum((p-qm).*normals);708 stdev = std(residuals);709 ER=sum(residuals.ˆ2);710 end

Color Fusion

”colorFusionStatic.m”

1 %% Environement configuration2 %Load rgb and infrared camera's calibration parameters3 calib workspace = load('calibStereoMatrixes.mat');4 %Load acquisitions5 load('rgbMatrixesStore.mat');6 load('depthMatrixesStore.mat');7 %Add vl-feat path8 addpath(genpath('vlfeat-0.9.20'));9 %Select approach

10 approach = 'leastSquareNonLinear';11 %Set twice as true if use 342x320 images with 640x480 calibration12 twice = true;13

14 nFrames = size(depthMatrixes,3);15

16 %% Error estimation or show frames17 estimateError = true;18 showFrames = ¬estimateError;19

20

21 %% Configure visualizer for color and depth22 srcDepthFrame=depthMatrixes(:,:,1);23 srcColorFrame = rgbMatrixes(:,:,:,1);


24

25 if(showFrames)26 f1=figure('KeyPressFcn',@spacePress);27 subplot(1,2,1),h1=imshow(srcColorFrame);28 subplot(1,2,2),h2=imshow(srcDepthFrame,[]);29 end30

31 if(estimateError)32 meanErrors = zeros(1,nFrames-1);33 end34

35 time = zeros(1,nFrames-1);36 tic();37

38

39 %% Static trajectory estimator40 history R = [];41 history T = [];42

43 for(i=2:nFrames)44 tgtDepthFrame=depthMatrixes(:,:,i);45 if(showFrames)46 set(h2,'CDATA',tgtDepthFrame);47 end48

49 tgtColorFrame=rgbMatrixes(:,:,:,i);50 if(showFrames)51 set(h1,'CDATA',tgtColorFrame);52 end53

54 t = toc();55 [R, T] = colorFusionRototranslationEstimation(srcColorFrame,srcDepthFrame,tgtColorFrame...

,tgtDepthFrame, approach, twice, calib workspace);56 t = toc() - t;57

58 if(showFrames)59 drawnow;60 end61

62 if(estimateError)63 meanErrors(i-1) = estimateMeanError(srcDepthFrame, tgtDepthFrame, R, T, ...

calib workspace.K ir, twice);64 end65

66 time(i) = t;67 disp(['Aligned n. ' num2str(i)]);68

69 srcDepthFrame = tgtDepthFrame;70 srcColorFrame = tgtColorFrame;71

72 history R = cat(3,history R,R);73 history T = cat(3,history T,T);74 end75

76 %% Show error results77 if(estimateError)78 plot(1:(nFrames-1),meanErrors);79 end80

81 disp(['Effective mean time: ' num2str(mean(time))]);

”ColorFusionRototranslationEstimation.m”

1 %% Estimate rotation matrix and translation vector using SIFT for point matching2 % Use 'twice' as true for 240x320 acquisition, false for 480x6403 function [R, T] = colorFusionRototranslationEstimation(srcColorFrame, srcDepthFrame, ...

tgtColorFrame, tgtDepthFrame, approach, twice, calibWs)

95

4 [color2DMatches, scores] = getSiftMatches(srcColorFrame, tgtColorFrame);5

6 color2DMatches = getConsistentMatches(color2DMatches, scores, size(srcColorFrame));7

8 if(size(color2DMatches,2)==0)9 R=eye(3);

10 T=zeros(3,1);11 return;12 end13

14 depth2DMatches = mapColorToIrCamera(color2DMatches, srcDepthFrame, tgtDepthFrame, twice..., calibWs);

15

16 if(isempty(depth2DMatches))17 R=eye(3);18 T=zeros(3,1);19 return;20 end21

22 %src3DIrPoints = get3DPoints(depth2DMatches(1,:), srcDepthFrame, calibWs.K ir);23 %tgt3DIrPoints = get3DPoints(depth2DMatches(2,:), tgtDepthFrame, calibWs.K ir);24

25 %[R, T] = RTestimation(src3DIrPoints, tgt3DIrPoints, approach, ones(3,size(...src3DIrPoints,2)) );

26

27 srcInvDepthIrPoints = getInvDepthPoints(depth2DMatches(1,:), srcDepthFrame, calibWs....K ir);

28 tgtInvDepthIrPoints = getInvDepthPoints(depth2DMatches(2,:), tgtDepthFrame, calibWs....K ir);

29

30 badIndexes = isnan(srcInvDepthIrPoints(3,:)) + isnan(tgtInvDepthIrPoints(3,:)) + isinf(...srcInvDepthIrPoints(3,:)) + isinf(tgtInvDepthIrPoints(3,:));

31 srcInvDepthIrPoints = srcInvDepthIrPoints(:,¬badIndexes);32 tgtInvDepthIrPoints = tgtInvDepthIrPoints(:,¬badIndexes);33 [R, T] = RTestimation(srcInvDepthIrPoints, tgtInvDepthIrPoints, approach, ones(3,size(...

srcInvDepthIrPoints,2)) );34 end35

36 %Returns linearized indexes of hotspots in srcFrame and their match in37 %tgtFrame38 function [matches, scores] = getSiftMatches(srcFrame, tgtFrame)39 srcFrameGray = single(rgb2gray(srcFrame));40 tgtFrameGray = single(rgb2gray(tgtFrame));41

42 %"frame" is a technical word in SIFT to define an hotspot43 [framesSrc, descriptorsSrc] = vl sift(srcFrameGray) ;44 [framesTgt, descriptorsTgt] = vl sift(tgtFrameGray) ;45 [matches, scores] = vl ubcmatch(descriptorsSrc, descriptorsTgt);46

47 hotspotSrcU = round(framesSrc(2,matches(1,:)));48 hotspotSrcV = round(framesSrc(1,matches(1,:)));49 hotspotTgtU = round(framesTgt(2,matches(2,:)));50 hotspotTgtV = round(framesTgt(1,matches(2,:)));51

52 linearizedHotspotSrc = sub2ind(size(srcFrame),hotspotSrcU,hotspotSrcV);53 linearizedHotspotTgt = sub2ind(size(tgtFrame),hotspotTgtU,hotspotTgtV);54

55 matches = [linearizedHotspotSrc; linearizedHotspotTgt];56 end57

58 %Returns only "better" matches based on scores and pixel distance.59 function [filteredMatches] = getConsistentMatches(matches, scores, frameSize)60 filteredMatches = [];61 pixelDistanceTresh = 10; % maximum permitted distance62 scoreTresh = 50000; % maximum permitted descriptor's distance63

64 for (hotspotPair = [matches;scores])65 if(hotspotPair(3)>scoreTresh)


66 continue;67 end68

69 [srcU, srcV] = ind2sub(frameSize, hotspotPair(1));70 [tgtU, tgtV] = ind2sub(frameSize, hotspotPair(2));71

72 if(norm([srcU;srcV] - [tgtU;tgtV]) ≤ pixelDistanceTresh)73 filteredMatches = [filteredMatches, [hotspotPair(1);hotspotPair(2)]];74 end75 end76 end77

78 function [irMatches] = mapColorToIrCamera(color2DMatches, srcIrFrame, tgtIrFrame, twice, ...calibWs)

79 irMatches = [];80

81 for(colorPointPair=color2DMatches)82 [rgbSrcU, rgbSrcV] = ind2sub(size(srcIrFrame),colorPointPair(1));83 [rgbTgtU, rgbTgtV] = ind2sub(size(srcIrFrame),colorPointPair(2));84

85 [irSrcU, irSrcV] = convertToIr([rgbSrcU; rgbSrcV], srcIrFrame, twice, calibWs);86 [irTgtU, irTgtV] = convertToIr([rgbTgtU; rgbTgtV], tgtIrFrame, twice, calibWs);87

88 if(¬isnan(irSrcU*irTgtU))89 irMatches = [irMatches, [sub2ind(size(srcIrFrame), irSrcU, irSrcV); sub2ind(...

size(tgtIrFrame), irTgtU, irTgtV)]];90 end91 end92 end93

94 function [irU, irV] = convertToIr(rgbPixel, irImage, twice, calibWs)95 windowSearchSize = [11 11];96 nearTresh = 5; %Maximum distance for valid mapping97

98 [minU, maxU, minV, maxV] = searchBounds(rgbPixel, size(irImage), windowSearchSize);99

100 Ugrid = repmat((minU:maxU)',1,maxV-minV+1);101 Vgrid = repmat((minV:maxV), maxU-minU+1,1);102 Zgrid = single(irImage(minU:maxU, minV:maxV));103 Zgrid(Zgrid==0)=nan;104

105 %%106 % Without for loop107

108 irXYZ = [Vgrid(:)'.*Zgrid(:)'; Ugrid(:)'.*Zgrid(:)'; Zgrid(:)'];109 if twice110 irXYZ = [2*irXYZ(1,:); 2*irXYZ(2,:); irXYZ(3,:)];111 end112

113 rgbXYZ = calibWs.K rgb*(calibWs.R*inv(calibWs.K ir)*irXYZ + repmat(calibWs.T,1,size(...irXYZ,2)));

114 rgbVU = rgbXYZ./repmat(rgbXYZ(3,:),3,1);115 if twice116 rgbVU = rgbVU/2;117 end118 rgbVU = round(rgbVU);119 rgbUV = [rgbVU(2,:); rgbVU(1,:)];120

121 distances = sqrt(sum((rgbUV - repmat(rgbPixel,1,size(irXYZ,2))).ˆ2));122

123 [minValue, minIdx] = min(distances);124 %If there's no valid match or rgb mapped point is too far from original rgb point, ...

return nan125 if(isnan(minValue) | | minValue > nearTresh)126 irU = nan;127 irV = nan;128 return;129 end

97

130

131 irU = rgbUV(1,minIdx);132 irV = rgbUV(2,minIdx);133 %134

135 %136 % With for loop137 irCandidatePixels = [];138 distances = [];139

140 for(u=minU:maxU)141 for(v=minV:maxV)142 %Calibration uses [v;u] instead of [u;v]143 z = single(irImage(u,v));144 %Exclude saturated depth pixels145 if(z==0)146 continue;147 end148

149 irXYZ = [v*z; u*z; z];150 rgbXYZ = calib KK right*(calib R*inv(calib KK left)*irXYZ + calib T);151 rgbVU = round(rgbXYZ./rgbXYZ(3));152 rgbUV = [rgbVU(2);rgbVU(1)];153

154 irCandidatePixels = [irCandidatePixels, [u,v]];155 distances = [distances, norm(rgbPixel - rgbUV)];156 end157 end158

159 if(isempty(irCandidatePixels))160 irU = nan;161 irV = nan;162 return;163 end164

165 [minValue, minIdx] = min(distances);166 %If rgb mapped point is too far from original rgb point, return nan167 if(minValue > nearTresh)168 irU = nan;169 irV = nan;170 return;171 end172

173 irU = irCandidatePixels(1,minIdx);174 irV = irCandidatePixels(2,minIdx);175 %176 end177

178 function [minU, maxU, minV, maxV] = searchBounds(rgbPixel, imageSize, windowSearchSize)179 halfHeigh = (windowSearchSize(1)-1)/2;180 halfWidth = (windowSearchSize(2)-1)/2;181

182 minU = max(1,rgbPixel(1)-halfHeigh);183 maxU = min(imageSize(1), rgbPixel(1)+halfHeigh);184 minV = max(1,rgbPixel(2)-halfWidth);185 maxV = min(imageSize(2),rgbPixel(2)+halfWidth);186 end187

188 function XYZ = get3DPoints(linearizedCoordinates, depthImage, K ir)189 z = single(depthImage(linearizedCoordinates));190 [U, V] = ind2sub(size(depthImage),linearizedCoordinates);191 zU = U.*z;192 zV = V.*z;193 XYZ= inv(K ir)*[zU; zV; z];194 end195

196 function uvq = getInvDepthPoints(linearizedCoordinates, depthImage, K ir)197 xyz = get3DPoints(linearizedCoordinates, depthImage, K ir);


198 uvq = [xyz(1,:)./xyz(3,:); xyz(2,:)./xyz(3,:); ones(1,size(xyz,2))./xyz(3,:)];199 end

”RTestimation.m”

1 function [R, T] = RTestimation(srcPoints,targetPoints,approach, weights)2

3 switch approach4 case 'leastSquareSmall'5 [R, T] = leastSquareSmallEstimation(srcPoints, targetPoints);6 case 'leastSquareNonLinear'7 [R, T] = leastSquareNonLinearEstimation(srcPoints, targetPoints);8 case 'SVD'9 [R, T] = svdEstimation(srcPoints, targetPoints, weights);

10 end11 end12

13 %Assuming small rototraslation between point clouds, solving Ax=b14 %15 % that is, denoting points as (x,y,z), 'source' as 's', 'target' as 't',16 % theta angles as 'th', traslation as 'T':17 %18 % xs - thz*ys + thy*zs + Tx = xt19 % thz*xs + ys - thx*zs + Ty = yt20 % -thy*xs + thx*ys + zs + Tz = zt21 %22 % that is:23 %24 % | 0 zs -ys 1 0 0 | | thx | | xt - xs |25 % | -zs 0 xs 0 1 0 | * | thy | = | yt - ys |26 % | ys -xs 0 0 0 1 | | thz | | zt - zs |27 % | Tx |28 % | Ty |29 % | Tz |30 %31 % related to a generic source-target point pair32 function [R, T] = leastSquareSmallEstimation(srcPoints, targetPoints)33 n = size(srcPoints,2);34

35 reshepableConstants = [zeros(1,n); %036 srcPoints(3,:); %zs37 -srcPoints(2,:); %-ys38 ones(1,n); %139 zeros(2,n); %0; 040 -srcPoints(3,:); %-zs41 zeros(1,n); %042 srcPoints(1,:); %xs43 zeros(1,n); %044 ones(1,n); %145 zeros(1,n); %046 srcPoints(2,:); %ys47 -srcPoints(1,:); %-xs48 zeros(3,n); %0; 0; 0;49 ones(1,n)]; %150

51 A = reshape(reshepableConstants,6,3*n)';52

53 b = targetPoints(:) - srcPoints(:);54

55 x = pinv(A)*b;56

57 R = [1 -x(3) x(2);58 x(3) 1 -x(1);59 -x(2) x(1) 1];60

61 T = x(4:6);62 end

99

63

64 %For a generic rototranslation between point cloud, solve Ax=b, starting65 %from generic rotation and traslation:66 %67 % | cos(thx)*cos(thy) cos(thz)*sin(thy)*sin(thx)-sin(thz)*cos(thx) cos(thz)*sin(thy)*cos(...

thx)+sin(thz)*sin(thx) | | xs | | Tx | | xt - xs |68 % | sin(thz)*cos(thy) sin(thz)*sin(thy)*sin(thx)+cos(thz)*cos(thx) sin(thz)*sin(thy)*cos(...

thx)-cos(thz)*sin(thx) | * | ys | + | Ty | = | yt - ys |69 % | -sin(thy) cos(thy)*sin(thx) cos(thy)*cos(thx) ...

| | zs | | Tz | | zt - zs |70 %71 % that is, denoting points as (x,y,z), 'source' as 's', 'target' as 't',72 % traslation as 'T' and other unknow parameters with letters A,..,I:73 %74 % A*xs + B*ys + C*zs + Tx = xt75 % D*xs + E*ys + F*zs + Ty = yt76 % G*xs + H*ys + I*zs + Tz = zt77 %78 % that is:79 %80 % | xs ys zs 0 0 0 0 0 0 1 0 0 | | A | | xt |81 % | 0 0 0 xs ys zs 0 0 0 0 1 0 | * | B | = | yt |82 % | 0 0 0 0 0 0 xs ys zs 0 0 1 | | C | | zt |83 % | D |84 % | E |85 % | F |86 % | G |87 % | H |88 % | I |89 % | Tx |90 % | Ty |91 % | Tz |92 function [R, T] = leastSquareNonLinearEstimation(srcPoints, targetPoints)93 n = size(srcPoints,2);94

95 reshepableConstants= [srcPoints; %xs; ys; zs;96 zeros(6,n); %0; 0; 0; 0; 0; 0;97 ones(1,n); %198 zeros(5,n); %0; 0; 0; 0; 0;99 srcPoints; %xs; ys; zs;

100 zeros(4,n); %0; 0; 0; 0;101 ones(1,n); %1102 zeros(7,n); %0; 0; 0; 0; 0; 0; 0;103 srcPoints; %xs; ys; zs;104 zeros(2,n); %0; 0;105 ones(1,n) ]; %1106

107 A = reshape(reshepableConstants,12, 3*n)';108

109 b = targetPoints(:);110

111 x = pinv(A)*b;112

113 R = [x(1) x(2) x(3);114 x(4) x(5) x(6);115 x(7) x(8) x(9) ];116

117 T = x(10:12);118 end119

120 function [R, T] = svdEstimation(srcPoints, targetPoints, weights)121 nSrcPoints = size(srcPoints,2);122 nTgtPoints = size(targetPoints,2);123

124 % normalize weights125 weights = weights ./ repmat(sum(weights,2),1,size(weights,2));126

127 % find data centroid and deviations from centroid


128 targetPoints bar = sum(targetPoints .* weights,2);129 targetPoints mark = targetPoints - repmat(targetPoints bar, 1, nTgtPoints);130 % Apply weights131 targetPoints mark = targetPoints mark .* weights;132

133 % find data centroid and deviations from centroid134 srcPoints bar = sum(srcPoints .* weights,2);135 srcPoints mark = srcPoints - repmat(srcPoints bar, 1, nSrcPoints);136

137

138 N = srcPoints mark*transpose(targetPoints mark);139

140 [U,¬,V] = svd(N); % singular value decomposition141

142 R = V*diag([1 1 det(U*V')])*transpose(U);143

144 T = targetPoints bar - R*srcPoints bar;145 end

”estimateMeanError.m”

1 function meanError = etsimateMeanError(srcDepthFrame, tgtDepthFrame, R, T, K ir, twice)2

3 nRows = size(srcDepthFrame,1);4 nCols = size(srcDepthFrame,2);5

6 %If required, convert 240x320 to 480x6407 if(twice)8 c = 2;9 else

10 c = 1;11 end12

13 U = repmat(c*(1:nRows),1,nCols);14 V = reshape(repmat(c*(1:nCols),nRows,1),1,nRows*nCols);15

16 zSrc = single(srcDepthFrame(:)');17 zTgt = single(tgtDepthFrame(:)');18

19 %Calibration switch U and V pixels20 srcPointCloud = inv(K ir)*[V.*zSrc; U.*zSrc; zSrc];21 srcPointCloud = [srcPointCloud(2,:); srcPointCloud(1,:); srcPointCloud(3,:)];22 tgtPointCloud = inv(K ir)*[V.*zTgt; U.*zTgt; zTgt];23 tgtPointCloud = [tgtPointCloud(2,:); tgtPointCloud(1,:); tgtPointCloud(3,:)];24

25 %Remove saturated points26 sanitizedSrcPointCloud = srcPointCloud(:,srcPointCloud(3,:)>0);27 sanitizedTgtPointCloud = tgtPointCloud(:,tgtPointCloud(3,:)>0);28

29 %Align point cloud30 sanitizedAliPointCloud = R * sanitizedSrcPointCloud + repmat(T,1,size(...

sanitizedSrcPointCloud,2));31

32 %For each aligned point, found nearest point in sanitized target point cloud and ...evaluate mean of distances

33 tgtKdObj = KDTreeSearcher(sanitizedTgtPointCloud');34 [¬, distances] = knnsearch(tgtKdObj, sanitizedAliPointCloud');35 meanError = mean(distances);36 end

Ringraziamenti

Mi dicono che nei ringraziamenti si puo dismettere il tono formale ed utilizzare termini meno tecnici e piuumani.

Allora il primo grande e sincero grazie per avermi spinto ed attirato fino a qui va senza dubbio a Lui.La prima persona che ringrazio invece e il mio relatore, prof. Felice Andrea Pellegrino (questa volta non lo

sbaglio il cognome!): mi ha offerto l’opportunita di sperimentarmi in una tesi particolare, attraente e curiosaper un informatico un po’ “border line” (chissa se nel ma.le.lab. mi danno ancora del controllista...). Oltrealla proposta, e stato un esempio di cura e dedizione verso il proprio lavoro, sia come ricercatore sia comeinsegnante (cosa che ho molto apprezzato di trovare all’universita). Soprattutto, mi ha responsabilizzatomolto, dandomi allo stesso tempo l’aiuto necessario e la fiducia di chi crede in te. Quando insegnero anch’ioavro un altro bel ricordo da cui attingere.

Non posso poi non ringraziare i miei compagni di ingegneria, triennali e specialistici, con cui ho condivisoanni di esami, provette, cinema, D&D, colazioni, munchkin, eestec, kebab, ecc.. e che ultimamente si sonosentiti spessissimo ripetere “eh non posso, devo scrivere la tesi..”.

Un grazie alla mia famiglia che non mi ha mai fatto mancare nulla in questo periodo e che mi ha vistomolto teso e di corsa...ed ora mi potra vedere molto soddisfatto!

Il grazie va poi esteso agli altri miei fratelli, quelli “dell’oratorio”, che probabilmente si aspettavano checon questa tesi costruissi Gundam o Iron Man...ci stiamo lavorando.

E come dimenticare le importantissime persone che ho conosciuto grazie all’Azione Cattolica che in questimesi mi hanno sostenuto, supportato (sopportato?) e riportato “all’essenziale”.

E infine, GRAZIE a te.

CAPITOLO 6. RISULTATI SPERIMENTALI E CONFRONTI

Bibliografia

[1] Fusiello, “Visione Computazionale”, Franco Angeli, Milano. 2013

[2] K.Khoshelham, “Accurate Analisys of Kinect Depth Data”, International Archives of thePhotogrammetry, Remote Sensing and Spatial Information Sciences, Volume XXXVIII-5/W12, 2011

[3] X. Zhou, “A Study of Microsoft Kinect Calibration”, 2012

[4] ROS technical documentation, http: // wiki. ros. org/ kinect_ calibration/ technical

[5] Yang Chen, Gerard Medioni, “Object Modeling by Registration of Multiple Range Images”, IEEEInternational Conference on Robotics and Automation, 1991.

[6] Paul J. Besl, “A Method fo Registration of 3-D Shapes”, IEEE Transactions on Pattern Analysis andMachine Intelligence, Vol. XIV n. 2, 1992.

[7] Szymon Rusickiewicz, Marc Levoy, “Efficient Variants of the ICP Algorithm”, Proceedings of the 3rdInternational Conference on 3D Digital Imaging and Modeling, pp. 145-152, 2001.

[8] Pulli K., “Multiview Registration for Large Data Sets”, Proceedings of the 3rd International Conferenceon 3D Digital Imaging and Modelling, 1999

[9] J. L. Bentley, “Multidimensional binary search trees used for associative searching”, Communications ofthe ACM, Vol. XVIII pp. 509-517, 1975

[10] J. Montiel, J. Crivera, A.J. Davison, “Unified inverse depth parametrization for monocular SLAM”,Ropotics Science and Systems Conference, Philadelphia, 2006

[11] J. Crivera, A.J. Davison, J. Montiel, “Inverse depth to depth conversion for monocular SLAM”, IEEEInternational Conference on Robotics and Automation, pp. 2278-2783, 2007

[12] W.L.D. Lui, T.J.J. Tang, Tom Drummond, Wai Ho Li, “Robust Egomotion Estimation in Inverse DepthCoordinate”, IEEE International Conference on Robotics and Automation, 2012

[13] D.G. Lowe, “Object recognition from local scale-invariant features”, Proceedings of the InternationalConference on Computer Vision, Vol. II pp. 1150–1157, 1999

http://wiki.ros.org/kinect_calibration/technical

Sviluppo e confronto di tecniche di stima della traiettoria di sensori 3D

Engineering

Transcript of Sviluppo e confronto di tecniche di stima della traiettoria di sensori 3D