Vizuális helymeghatározás

Vizuális helymeghatározás Relatív helymeghatározás

Lukovszki Csaba1 1 Budapesti

Műszaki és Gazdaságtudományi Egyetem Távközlési és Médiainformatikai Tanszék

2015

Lukovszki Csaba (BME TMIT)

Navigációs szolgáltatások és alkalmazások

2015

1 / 28

Összefoglaló

I

Bevezetés I I

I

Szenzorok modellezése I I I

I

Követelmények Megvalósítási lehetőségek Gyorsulásmérő Giroszkóp Kamera

Inerciális, szűrő alapú relatív helymeghatározás I I I

Propagáció Update MSCKF



2015

2 / 28

Bevezetés



2015

3 / 28

Relatív helymeghatározás képi alapon Követelmények

I

Quality: pontosság

I

Consistency: az elért pontosság és a valós helyzet mennyire függ össze

I

Scalability: hosszú távú használat (időben és térben)

I

Accesibility: megvalósíthatóság, alacsony költségű, széles körű megvalósíthatóság

I

Efficiency: hatékonyság, valós idejű felhasználás kiegészítő eszközök használata nélkül

I

Robustness: különféle környezetekben, kültér, beltér, környezeti változások, mozgó objektumok



2015

4 / 28

Relatív helymeghatározás képi alapon Modell

I

Mobil eszköz I I I I I

I

6DoF Szenzorok Kamera Nagy számítási kapacitás Nagy tárolási kapacitás

Képi elemekben gazdag környezet



2015

5 / 28

Relatív helymeghatározás képi alapon Megvalósítási lehetőségek I

Tématerületek I I

I

SLAM (Simultaneous Localization and Mapping) VO (Visual Odometry)

Megoldási lehetőségek I

Geometriai megoldások I I I

I I

Képpárok között Jellemző detekció, leírás, társítás, nyomon követés Kulcs-kép alapú megoldások (a skálázhatóság és pontosság növelésének érdekében) Epipoláris geometria felhasználása

Szűrő alapú megoldások I

I

Mozgás modellezés a propagáció alatt pl. EKF-SLAM, MonoSLAM Inerciális szenzorok használata propagációhoz pl. Inerciálisan támogatott EKF-SLAM, MSCKF



2015

6 / 28

Szenzorok modellezése



2015

7 / 28

Mobil szenzorok modellezése

I

Szenzorok I I I

I

Gyorsulásmérő Giroszkóp Kamera

Modellezés I I I I

A matematikai modellek csak közelítik a valós működést A pontos modell a működés megértésén alapul Minél pontosabb a modell, annál pontosabb a működés "A mérnök múzsája az empíria"



2015

8 / 28

Inertial Measurement Unit I

Integrált megoldás (gyorsulás érzékelő, giroszkóp)

I

Pontos, jól modellezhető

I

Elterjedt megoldás



2015

9 / 28

Vonatkoztatási rendszerek z

y

y

C

I

x

x z z


G


y x 2015

10 / 28

Vonatkoztatási rendszerek Jelölési és transzformációs konvenciók

I

Pozíció: I I

I

Orientáció: I I

I

qI , RI : az IMU koordináta rendszerében értelmezett orientáció, rotáció qGC , RGC : passzív kvaternió, rotáció a kamera koordináta rendszerből a globális koordináta rendszerbe.

Koordináta rendszerek közötti transzformáció: I

I

pG : pozíció a globális koordináta rendszerben pC,G : a C koordináta rendszer a globális koordináta rendszerben

pG = RGC pC + pC,G : a kamera koordináta rendszerben értelmezett helyvektor transzformációja a globális koordináta rendszerbe

Az egyértelmű indexeket elhagyjuk



2015

11 / 28

Gyorsulásmérő modellezése I

A gyorsulásmérő az IMU-hoz rögzített koordináta rendszerben az eszköz aktuális gyorsulását méri (a, sm2 ): I

I

Az eszköz relatív helyváltoztatásának mérésében van szerepe

A mért érték hibákkal terhelt: I I

I

Zaj (Noise): modellezése fehér zajjal (an ), an ∼ N (0, σan ) Eltolódás (Bias): normális eloszlás szerinti véletlen bolyongás (ab ), aw ∼ N (0, σaw ), ahol a˙b = aw Rossz helyzet és skála hiba (Missalignment and scale errors): modellezése átskálázással (Ta ), Ta = I +

am,I = Ta RIG (aG − gG ) + ab,I + an,I



(1)

2015

12 / 28

Giroszkóp modellezése I

A giroszkóp szögsebességet mér (ω, koordináta rendszerében: I

I

rad sec )

az eszközhöz rendelt saját

Az orientáció meghatározásában van szerepe

A mért érték hibákkal terhelt: I I

I

I

Zaj (Noise): modellezése fehér zajjal (ωn ), ωn ∼ N (0, σωn ) Eltolódás (Bias): normális eloszlás szerinti véletlen bolyongás (ωb ), ωw ∼ N (0, σωw ), ahol ω˙ b = ωw Rossz helyzet és skála hiba (Missalignment and scale errors): modellezése átskálázással (Tω ), Tω = I + Gyorsulás érzékenység (g-sensitivity): transzformáció (Ts ), Ts =

ωm,I = Tω ωI + Ts aI + ωn,I + ωb,I



2015

13 / 28

shortest distance from the focal point to the image plane is known as the focal length find the location of an object on the image plane, we use the geometry of similar tri (see figure 5.2). An object appears on the image plane of a camera with focal len meters f at image coordinates ximg = ( x, y) T while its position in space is C X = ( X, Y

Mobil kamera modellezése

x= f

X Z

y= f

Y Z

Egyszerű vetítési modell (az Eukidészi térben) I

I

A kamera vetítés modellje I

I

I

I

I

f dx

Fókusz: fu = , fv = Középpont: (ou , ov )

Vetítés X + ou Z Y v = fv + ov Z u = fu

focal point

y f

x

Y X

Z

Figure 5.2. 3D to 2D projection

ábra: Egyszerű vetítés

Pixel mértékegységben I

I

Pont a C térben: XC = [X , Y , Z ]T ∈ R3 A vetített pont koordinátái: x = [x, y ]T ∈ R2 Fókusztávolság: f f dy

5.1. Pinhole Camera Mode Although ximg is the location in the image plane, it has the wrong units (e.g. me opposed to pixels). To convert the units to pixels we need the size of a pixel in m Additionally, the top left corner of an image has pixel coordinates (0, 0). To conver image coordinates to pixel coordinates, weudivide by d, the size of a pixel in meter v point, o. Note that the camera sensor is oft add the pixel coordinate of the center square, meaning d x and dy will be different values.

o X Z Y x =Cf x + o x y = f y + oy Z Z f X f For simplicity, we define f x = dx and f y = dy . The units H of o, f x , and f y are all pixe Y now have the basic pinhole model camera projection function. 0 1    X X o xW fx 0 Z + h@ Y A = Y oy 0 fy Z Z Figure 5.3. The camera and pixel coordinate frames.

ábra: Kamera középpont 5.1.2. Image Distortion 32



2015

14 / 28

Mobil kamera modellezése Radiális és tangenciális torzítások

5. Computer Vision Basics

I

A torzított projekció " #! h i h i X h i X f 0 u =h u Z Y = 0f dr Y + dt + oovu v v Z Z

I

Radiális torzítás (k1 , k2 , k3 )

5. Computer Vision Basics 2 3 dr = (1 + k1 r + k2 rFigure + k5.4. ) 3 r Examples of distortion. Left: no distortion, middle: ra distortion.

I

Tangenciális torzítás (t1 , t2 ) " # 2 Here 2 XZ YZ t1 + (r + 2 XZG p)t C is 2 the position of the camera in the global frame. dt = X Y 2 a 3D point in the global frame, G x, to pixel coordinate forming 2 Z Z t2 + (r + 2 YZ )t1  u

⇣

⇣

⌘⌘

= h CG R G x G pC X2 Y2 v r= + Z ZLeft: no distortion, middle: radial distortion, right: tang Figure 5.4. Examples of distortion.

distortion. Lukovszki Csaba (BME TMIT)


2015

15 / 28

Mobil kamera modellezése Idealizált koordináták I

A gyakorlatban a hasonló háromszögek arányát használjuk, melyet idealizált koordinátáknak nevezünk

I

Koordinátánként X 1 = (u − ou ) Z fu 1 Y = (v − ov ) v0 = Z fv u0 =

I

Mátrix alakban 1 h i h i u 0 = fu 0 u ou v − ov v0 0 f1 v



2015

16 / 28

Figure 5.7. The rolling shutter can cause visible distortion when the camera rotates quickly. These examples were taken by the iPhone 4S camera while moving in opposite directions.

Mobil kamera modellezése 5. Computer Vision camera, Basics When an image is captured by a rolling shutter there are two temporal values that need to be known to accurately use the image. The first is the offset td between the image’s timestamp and the time that the center column was captured. This value is usually negative, meaning that the timestamp is late, but in cases where video data and IMU data is completely unsynchronized and only the framerate is known, td could in theory be a positive value.

Rolling shutter

I

The second important calibration constant is the camera’s read time, tr . If this value is negative, it implies the rolling shutter moves from right to left, which is the case for the Mobil CCD kiolvasása iPhoneszenzorok 4S. Our experiments gave values Figure of around 20ms forvisible tr . distortion The method followed 5.7. The rolling shutter can cause when the camera rotates quickly. in These examples were taken by the iPhone 4S camera while moving in opposite directions. this thesis finds both of these temporal calibration factors by estimating them in the EKF, I Soronkénti kiolvasás ábra: RS jelenség as explained in chapter 6. I I

I

Gyakorlatban n · 10ms ideig is eltart t0 torzítást okoz Jelentős

Modellezése I

A kamera koordináta rendszer t0 + t d td transzformációja adott tr t0 + td + t2 időpontra r

time

When an image is captured by a rolling shutter camera, there are two temporal values that need to be known to accurately use the image. The first is the offset td between the image’s timestamp and the time that the center column N was captured. This value is usually N negative, meaning that the timestamp is late, but in 2 cases where video data and IMU 2data is completely unsynchronized and only the framerate is known, td could in theory be a 1 positive value.

t

The second important calibration constant is the camera’s read time, tr . If this value is negative, it implies the rolling shutter moves from right to left, which is the case for the iPhone 4S. Our experiments gave values of around 20ms for tr . The method followed in this thesis finds both of these temporal calibration factors by estimating them in the EKF, as explained in chapter 6.

t1 + t d

t0

N 2

t1

td tr

t1 + t d +

t0 + t d

t0 + t d +

tr 2

ktr N

k

t1 + t d

time

t1 + t d +

N 2

ktr N

k

Figure 5.8. When an image is captured, a timestamp is provided (t0 , t1 , . . .). The middle column of When an image image is captured, is provided middleof column 1 , . . .). The the image was captured at some time t + td . Figure The5.8.entire of Na timestamp columns has (ta0, tread time tr , of ábra: modell the image was captured at some time t + td .RS The entire image of N columns has a read time of tr , ktr column k was captured . that columnNavigációs k was captured at t + tmeaning where k2 [ N2at ,t +N2td]+. ktN where k 2 [ N2 , N2 ]2015 Lukovszki Csabameaning (BME TMIT) szolgáltatások és Nthat alkalmazások 17 / 28 d+ r

Inerciálisan segített képi alapú helymeghatározás hiba-állapot kiterjesztett Kálmán szűrő segítségével



2015

18 / 28

Inerciális relatív helymeghatározás EKF segítségével

I

A vezérlő jelek I I

I

Mérés I I

I

IMU mérésekből Mintha "zajos vezérlés" lenne Képi feldolgozás segítségével "Feature" pontok vetített képéből

Megvalósítás I

Inertial EKF

I

MSCKF (Multi-State Constrained Kalman Filter)

I

I

Térbeli jellemző pontok az EKF állapotvektorban Kamera hely és helyzet az EKF állapotvektorban



2015

19 / 28

Állapottér

teljes áll. pozíció sebesség kvaternió rotáció m.

valós xt pt vt qt Rt

nom. x p v q R

rotáció v. acc. bias gyro. bias gyorsulás szöggy.

hiba δx δp δv δq δR δθ

abt ωbt at ωt


ab ωb

δab δω

összefüggés ... ... ... qt = q ⊗ δq Rt = RδR δq ≈ [1, δθ/2]T R ≈ I + [δθx ]x ... ...


mért érték

zaj

am ωm

aw ωw an ωn

2015

20 / 28

Kinematika leírása Hiba állapot (ld. Sola) Nominális állapot (ld. Sola) 1 q˙ = q ⊗ (ωm − ωb ) 2 p˙ = v v˙ = RGI (am − ab ) + q

δ θ˙ = −[ωm − ωb ]x δθ − δωb − ωn

δ p˙ = δv

δ v˙ = −R[am − ab ]x δθ − Rδab − Ran

δ a˙b = aw

a˙b = 0

δ ω˙ b = ωw

ω˙ b = 0

Az A mátrix a fenti összefüggésekből írható fel.



2015

21 / 28

Propagáció I

Nominális állapot változása x˙t = f (xt , u, w )

I

I

I

A propagáció elemei állapotvektor vezérlés zaj  qt h i h i  pt  am − a n aw w = xt =  v t  u = ω ωw m − ωn abt ωbt Hiba állapot változása ˙ = Aδx + Bu˜ + Cw δx Diszkrét időben δxn+1 = Fx δxn + Fu uñ + Fw wn Fx = Φ = e A∆t , Fu = B∆t, Fw = C



2015

22 / 28

Állapotok propagálása I

A propagálás a mért adatok alapján ∆t idővel hogy változik meg az állapottér I

A nominális állapotok a kinematikai egyenletek alapján változnak q ← q ⊗ q{(ωm − ωb )∆t} 1 p ← p + v ∆t + (R(am − ab ) + g )∆t 2 2 v ← v + (R(am − ab ) + g )∆t

ab ← ab

ωb ← ωb I

A hiba növekszik I

A P kovariancia mátrix elemei folyamatosan növekednek P ← Fx PFTx + Q Q = ∆t 2 BUc BT + ∆tCWc CT



2015

23 / 28

Update I

Valós mérésekből származó információk I I I I

I

I

IMU IMU IMU IMU

+ + + +

GPS monokuláris képi információk sztereó képi információk (bármilyen helymeghatározás)

Hiba állapot megfigyelése a méréseken keresztül z = h(x) + n, n ∼ N (0, R)

A hiba figyelembe vétele, az állapot módosításával K = PHT (HPHT + R)−1 ˜ ← K(z − h(˜ δx x ))

I

A hiba nullázása

P ← (I − KH)P ˆ =0 δx



2015

24 / 28

MSCKF megvalósítás Áttekintés

I

Az állapottér kiegészül a kamera hely és helyzet értékekkel I

I

M hosszú kamera hely és helyzet puffer xMSCKF = [x, {qGCi , pCi ,G }], i ∈ [1...M]

A működés I

Új kép esetében "Feature track" menedzment

I

Egy "feature track" akkor ér véget, ha

I

I I I

Egy "feature track" egy térbeli ponthoz tartozik a "feature" már nem látható a "feature track" hossza nagyobb, mint M

"Measurement update" minden befejezett "feature track"-re I I I

Kamera állapot kiterjesztés "Feature point" trianguláció Mérési modell alkalmazása



2015

25 / 28

MSCKF megvalósítás Mérési modell I

Alap összefüggés r = H˜ x +n

I

, ahol I

(j)

r a reziduál, minden j "feature point"-ra, ri I I

I I I

(j)

(j)

− zî , ahol

zi , az i-dik képen a "feature" koordinátái a vetítési síkon (j) zî , a j-dik triangulált "feature point" visszavetítése

H a mérés Jakobi mátrixa x˜ az állapot hibája n zaj (j)

ri I

(j)

= zi

(j)

(j)

(j)

= Hxi x˜ + Hfi p˜fj ,G + ni

, a hol I I

(j)

Hxi a mérés Jakobi mátrixa az állapot hibájára vonatkozóan (j) Hfi a mérés Jakobi mátrixa a "feature" térbeli helyére vonatkozóan



2015

26 / 28

MSCKF megvalósítás Mérési modell I

Egyszerűsítés I

I

A a Hf mátrix jobboldali nulltere (j) (j) (j) (j) r0 = AT (z (j) − zˆ(j) ) ' AT Hx x˜ + AT n (j) = H0 ˜x(j) + n0

Megoldás

r0 = Hx x˜ + n0 I

Hx felbontása

I

A Kálmán nyereség

h i Hx = [Q1 Q2 ] T0H ˜ + QT rn = QT 1 r0 = TH x 1 n0 T Rn = Q1 R0 Q1 T −1 K = PTT H (TH PTH + Rn )

I

I

Az állapot korrekció ∆x = Krn Kovariancia mátrix változás P ⇐ (I − KTH )P(I − KTH )−1 + KRn KT



2015

27 / 28

Felhasznált irodalom

I

A. I. Mourikis, ..., "A Multi-State Contraint Kalman Filter for Vision-aided Inertial Navigation", In Proc. 2007 IEEE International Conference on Robotics and Automation

I

Michael Andrew Shelley, "Monocular Visual Odometry on a Mobile Device", M.Sc. Theses, University of München, 2014

I

Joan Sola, "Quaternion kinematics for the error-state KF", March 11, 2015 Etc...



2015

28 / 28

Vizuális helymeghatározás

Recommend Documents