TARGET: Estimate camera pose and trajectory from 3D images acquired by 3D structured light sensor. Development of new algorithm for egomotion estimation (Frame Based and Color Fusion) and comparison with Iterative Closest Point approaches. Usage of inverse depth space for 3D data modelization.

1. Laureando
Andrea Bidinost
Relatore
Prof. Felice Andrea Pellegrino
Tesi di Laurea Magistrale in Ingegneria Informatica
Università degli Studi di Trieste

2.  Stimare la traiettoria imposta ad un sensore
3D
Matrici di rototraslazione
r0,0 r0,1 r0,2 tx
r1,0 r1,1 r1,2 ty
r2,0 r2,1 r2,2 tz
r0,0 r0,1 r0,2 tx
r1,0 r1,1 r1,2 ty
r2,0 r2,1 r2,2 tz
r0,0 r0,1 r0,2 tx
r1,0 r1,1 r1,2 ty
r2,0 r2,1 r2,2 tz
Algoritmo di
allineamento
Sequenza di
Traiettoria imposta

3.  Robotica mobile (odometria)
 Robotica industriale (pilotaggio di un braccio
meccanico)
 Ricostruzione virtuale

4.  Campo di ricerca (robotica): Simultaneous
Localization And Mapping (SLAM)
 Soluzione più diffusa: stereovisione
 Approccio recente: acquisizione 3D (luce
strutturata, Time of Flight)
 Nel progetto: Iterative Closest Point (ICP),
InverseDepth-ICP

5.  Due nuovi approcci per l’allineamento di una
coppia di immagini: «Frame Based» e «Color
Fusion»
 Ambiente Matlab

7. Algoritmi
Immagine
Infrarossi
Immagine
di
disparità
Immagine
di
profondità
Nuvola di
punti
Input
(coppie)

8.  Algoritmo di riferimento
 Input: coppia di nuvole di punti (origine e destinazione)
 Iterazione di 5 fasi:
1. Selezione
2. Accoppiamento
3. Pesatura
4. Rigetto
5. Minimizzazione
 Output: matrice di rototraslazione
Algoritmi
Minimizza un funzionale d’errore che ha minimo assoluto nella
rototraslazione che allinea perfettamente due nuvole di punti.

9. Selezione
Estrazione di punti da nuvola di origine
(casuale, selettivo)
Accoppiamento
Associazione di ogni punto estratto con un
punto della nuvola di destinazione (più vicino,
criteri locali)
Pesatura
Peso del contributo di un accoppiamento alla
stima (costante, distanza, adattativa)
Rigetto
esclusione di coppie (percentuale, soglia sul
peso)
Minimizzazione
Minimizzazione del funzionale d’errore
attraverso minimi quadrati (punto-punto,
punto-piano)
Arresto
Numero massimo di iterazioni o diminuzione
dell’errore al di sotto di una certa tolleranza
Algoritmi

10.  Dati due punti accoppiati
𝑃 = [𝑥 𝑝 𝑦 𝑝 𝑧 𝑝] 𝑇 𝑄 = [𝑥 𝑞 𝑦 𝑞 𝑧 𝑞] 𝑇
 Generica rotazione (3 DOF)
 Generica Traslazione (3 DOF)
𝑇 =
𝑡 𝑥
𝑡 𝑦
𝑡 𝑧
 Sistema (non lineare)
𝑅 𝑇
𝑃
1
= 𝑄
Algoritmi

11.  Spazio di profondità inversa:
◦ errore di misura isometrico (altrimenti funzionale alla distanza dal
sensore)
◦ si costruisce dall’immagine di disparità attraverso coefficienti di
calibrazione
◦ 𝐼𝑛𝑣𝑒𝑟𝑠𝑒𝐷𝑒𝑝𝑡ℎ 𝑥 𝑦 𝑧 =
𝑥
𝑧
𝑦
𝑧
1
𝑧
 Approssimazione a piccole rotazioni:
𝑅 𝑇
0 1
=
1 −𝜃 𝑧 𝜃 𝑦
𝜃 𝑧 1 −𝜃 𝑥
−𝜃 𝑦 𝜃 𝑥 1
𝑡 𝑥
𝑡 𝑦
𝑡 𝑧
0 0 0 1
 Stesse fasi e Output di ICP, trasformazione dell’Input in spazio di
profondità inversa
 Sfruttamento della linearità del nuovo sistema da minimizzare
 Implementazione articolo «Robust Egomotion Estimation using ICP in Inverse Depth
Space», D. Luis 2012
Algoritmi

12.  Idea: sfruttare l’informazione sulla posizione
reciproca dei punti presente nell’immagine
acquisita (disparità o profondità)
 Nuovo modello dei dati: 3D-frame
Algoritmi

13.  Ricerca dei punti da accoppiare all’interno
una finestra
 Sfruttare i punti vicini per caratterizzare la
regione locale da ricercare nell’immagine di
destinazione per l’accoppiamento
 Input: coppie di frame di disparità (o
profondità)
 Stesse fasi di ICP, ipotesi di piccole rotazioni
 Output: matrice di rototraslazione
Algoritmi

14.  Idea: sfruttare l’informazione sul colore dei
punti data dall’acquisizione a colori per
ottenere un accoppiamentovalido al primo
tentativo
 Input: coppie di immagini di profondità (o
disparità) e relative acquisizioni a colori
 Non è richiesta nessuna iterazione
 3 Fasi:
1. SIFT e accoppiamento
2. Pesatura e Rigetto
3. Stima dei coefficienti rototraslazionali
 Output: matrice di rototraslazione
Algoritmi

15. Algoritmo per la caratterizzazione di punti
notevoli in un’immagine a colori con invarianza
a trasformazioni di scala, rotazione e
luminosità.
 Estrae punti notevoli da immagini a colori
 Associa ad ogni punto notevole un vettore di
descrittori invarianti alle trasformazioni
dell’immagine
Algoritmi

16.  Implementazione utilizzata: VLFeat (Vedaldi,
http://www.vlfeat.org/overview/sift.html)
 Metodo per l’accoppiamento già disponibile
Algoritmi

18.  Coppia consecutiva di acquisizioni (origine,
destinazione) Nuvole di punti
𝑁𝑜 =
𝑥 𝑜
1
… 𝑥 𝑜
𝑛
𝑦𝑜
1 … 𝑦𝑜
𝑛
𝑧 𝑜
1
… 𝑧 𝑜
𝑛
𝑁𝑑 =
𝑥 𝑑
1
… 𝑥 𝑑
𝑚
𝑦 𝑑
1
… 𝑦 𝑑
𝑚
𝑧 𝑑
1
… 𝑧 𝑑
𝑚
 Stima rototraslazione [𝑅 𝑇]
 Allineamento
𝑁𝑎 = 𝑅 𝑇 ∙
𝑁𝑜
1
 Accoppiamento (𝑁𝑎, 𝑁𝑑)
 Per ogni coppia, distanza euclidea
 Errore (mm): distanza media
Risultati

19.  Sequenza comune di acquisizioni
 Valutazione errore di allineamento tra coppie
di acquisizioni consecutive
 Algoritmi confrontati
◦ ICP (spazio euclideo, inverse depth)
◦ Inverse depth ICP
◦ Frame Based (spazio euclideo, inverse depth)
◦ Color Fusion (spazio euclideo, inverse depth)
Risultati

20.  Distribuzione cumulata: numero di
allineamenti affetti da un errore minore di…
 Approccio standard: ICP spazio euclideo
Errore (mm)
%
18
Risultati

21. Errore (mm)5.8
ICP (Inverse Depth)
Risultati
%

22. Fame Based (Inverse Depth)
Errore (mm)5.4 7.3
Risultati
%

23. Color Fusion (Inverse Depth)
Risultati
9.5 Errore (mm)
%

24.  Allocazione di memoria
◦ Unità minima di allocazione: punto 3D
◦ Unità richieste (numero di punti da allocare)
◦ Landau: Θ 𝑁
◦ Metodi non più onerosi di ICP, Color Fusion più
svantaggioso
Risultati

25.  Numero di operazioni (in funzione di)
◦ Numero di pixel delle immagini (𝑁)
◦ Numero di punti sottocampionati (𝑛)
◦ Parametri di configurazione (𝜔, 𝐼 𝑚𝑎𝑥, 𝑙, 𝐺, 𝑊, 𝜂)
Risultati

26.  Considerazioni sul numero di operazioni:
◦ Complessità sostanzialmente paragonabile per
metodi su ICP (minimi quadrati, Θ 𝑛3 asintotico)
◦ Color Fusion metodo più oneroso per SIFT (un
ordine di grandezza, Θ 𝑁 ∙ 𝐺 asintotico)
◦ Frame Based approccio più leggero
Risultati

27. Risultati

28.  Utilizzo dello spazio di profondità inversa
riduce l’errore di allineamento ma richiede
conversione
 Ottimi risultati dall’utilizzo dello spazio di
profondità inversa con ICP
 Confermati risultati dell’articolo
implementato tramite InverseDepth-ICP
Conclusioni

29.  Migliori risultati nell’errore di allineamento
rispetto ad IC
 Complessità computazionale paragonabile ad
ICP, Frame Based metodo più vantaggioso
 Color Fusion estende la possibilità di stima
anche in ambienti sfavorevoli per ICP (piano
ortogonale frontale) purchè in presenza di
colori diversi

30.  Necessità di test per approfondire le cause
degli errori di allineamento
 Implementazione più performante (C, GPU) in
termini di tempo di esecuzione (real time)
 Riscrittura per il calcolo parallelo,
 Imposizione della traiettoria stimata ad un
dispositivo semovente

31. Risultati