Vizuális helymeghatározás

Vizuális helymeghatározás Relatív helymeghatározás Lukovszki Csaba 1 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ó Bevezetés Követelmények Megvalósítási lehetőségek Szenzorok modellezése Gyorsulásmérő Giroszkóp Kamera Inerciális, szűrő alapú relatív helymeghatározás Propagáció Update MSCKF Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 2 / 28

Bevezetés Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 3 / 28

Relatív helymeghatározás képi alapon Követelmények Quality: pontosság Consistency: az elért pontosság és a valós helyzet mennyire függ össze Scalability: hosszú távú használat (időben és térben) Accesibility: megvalósíthatóság, alacsony költségű, széles körű megvalósíthatóság Efficiency: hatékonyság, valós idejű felhasználás kiegészítő eszközök használata nélkül Robustness: különféle környezetekben, kültér, beltér, környezeti változások, mozgó objektumok Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 4 / 28

Relatív helymeghatározás képi alapon Modell Mobil eszköz 6DoF Szenzorok Kamera Nagy számítási kapacitás Nagy tárolási kapacitás Képi elemekben gazdag környezet Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 5 / 28

Relatív helymeghatározás képi alapon Megvalósítási lehetőségek Tématerületek SLAM (Simultaneous Localization and Mapping) VO (Visual Odometry) Megoldási lehetőségek Geometriai megoldások 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 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 Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 6 / 28

Szenzorok modellezése Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 7 / 28

Mobil szenzorok modellezése Szenzorok Gyorsulásmérő Giroszkóp Kamera Modellezés 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" Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 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 Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 9 / 28

Vonatkoztatási rendszerek z y y C I x x z y z G x Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 10 / 28

Vonatkoztatási rendszerek Jelölési és transzformációs konvenciók Pozíció: p G : pozíció a globális koordináta rendszerben pc,g : a C koordináta rendszer a globális koordináta rendszerben Orientáció: q I, R I : az IMU koordináta rendszerében értelmezett orientáció, rotáció q GC, R GC : 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ó: p G = R GC p C + p C,G : a kamera koordináta rendszerben értelmezett helyvektor transzformációja a globális koordináta rendszerbe Az egyértelmű indexeket elhagyjuk Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 11 / 28

Gyorsulásmérő modellezése A gyorsulásmérő az IMU-hoz rögzített koordináta rendszerben az eszköz aktuális gyorsulását méri (a, m ): s 2 Az eszköz relatív helyváltoztatásának mérésében van szerepe A mért érték hibákkal terhelt: Zaj (Noise): modellezése fehér zajjal (an ), a n N (0, σ an ) Eltolódás (Bias): normális eloszlás szerinti véletlen bolyongás (ab ), a w N (0, σ aw ), ahol ȧ b = a w Rossz helyzet és skála hiba (Missalignment and scale errors): modellezése átskálázással (T a ), T a = I + ɛ a m,i = T a R IG (a G g G ) + a b,i + a n,i (1) Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 12 / 28

Giroszkóp modellezése A giroszkóp szögsebességet mér (ω, rad sec ) az eszközhöz rendelt saját koordináta rendszerében: Az orientáció meghatározásában van szerepe A mért érték hibákkal terhelt: 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 ), T s = ɛ ω m,i = T ω ω I + T s a I + ω n,i + ω b,i Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 13 / 28

Mobil kamera modellezése Egyszerű vetítési modell (az Eukidészi térben) A kamera vetítés modellje Pont a C térben: X C = [X, Y, Z] T R 3 A vetített pont koordinátái: x = [x, y] T R 2 Fókusztávolság: f Pixel mértékegységben Fókusz: f u = f d x, f v = f d y Középpont: (ou, o v ) Vetítés u = f u X Z + o u v = f v Y Z + o v 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 focal point x = f X Z f Z I x y Figure 5.2. 3D to 2D projection y = f Y Z ábra: Egyszerű vetítés 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 add the pixel coordinate of the center v point, o. Note that the camera sensor is oft square, meaning dx and dy will be different values. x = C X fx Z + Z o ox y = Y fy Z + oy For simplicity, we define fx = f X f and fy =. The units of o, fx, and fy are all pixe dx dy H now have the basic pinhole model Y camera projection function. 0 1 X apple apple apple h @ Y A ox fx 0 X W = + Z oy 0 Y fy Z Z Figure 5.3. The camera and pixel coordinate frames. ábra: Kamera középpont 5.1.2. Image Distortion 32 Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 14 / 28 X Y

Mobil kamera modellezése Radiális és tangenciális torzítások A torzított projekció ([ ]) [ ] uv XY [ ] = h = fu 0 ( [ X ] ) [ ] Z 0 f d Z r v Y + d t + ou o v Z Radiális torzítás (k 1, k 2, k 3 ) d r = (1 + k 1 r + k 2 r 2 + k 3 r 3 ) Tangenciális torzítás (t 1, t 2 ) [ 2 X Y Z d t = 5. Computer Vision Basics 2 X Z ] Z t 1 + (r + 2 X 2 Z )t2 Y Z t 2 + (r + 2 Y 2 Z )t1 r = X Z 5. Computer Vision Basics Figure 5.4. Examples of distortion. Left: no distortion, middle: ra distortion. Here G p C is the position of the camera in the global frame. forming a 3D point in the global frame, G x, to pixel coordinate 2 + Y Z 2 apple u v = h CG R G x G p C Figure 5.4. Examples of distortion. Left: no distortion, middle: radial distortion, right: tang distortion. Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 15 / 28

Mobil kamera modellezése Idealizált koordináták A gyakorlatban a hasonló háromszögek arányát használjuk, melyet idealizált koordinátáknak nevezünk Koordinátánként u = X Z = 1 f u (u o u ) v = Y Z = 1 f v (v o v ) Mátrix alakban [ u v ] = [ 1 f u 0 0 1 f v ] ([uv ] [ ou o v ]) Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 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 Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 18 / 28

Inerciális relatív helymeghatározás EKF segítségével A vezérlő jelek IMU mérésekből Mintha "zajos vezérlés" lenne Mérés Képi feldolgozás segítségével "Feature" pontok vetített képéből Megvalósítás Inertial EKF Térbeli jellemző pontok az EKF állapotvektorban MSCKF (Multi-State Constrained Kalman Filter) Kamera hely és helyzet az EKF állapotvektorban Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 19 / 28

Állapottér valós nom. hiba összefüggés mért érték zaj teljes áll. x t x δx... pozíció p t p δp... sebesség v t v δv... kvaternió q t q δq q t = q δq rotáció m. R t R δr R t = RδR rotáció v. δθ δq [1, δθ/2] T R I + [δθ x ] x acc. bias a bt a b δa b... a w gyro. bias ω bt ω b δω... ω w gyorsulás a t a m a n szöggy. ω t ω m ω n Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 20 / 28

Kinematika leírása Nominális állapot (ld. Sola) q = 1 2 q (ω m ω b ) ṗ = v v = R GI (a m a b ) + q ȧ b = 0 ω b = 0 Hiba állapot (ld. Sola) δ θ = [ω m ω b ] x δθ δω b ω n δṗ = δv δ v = R[a m a b ] x δθ Rδa b Ra n δȧ b = a w δ ω b = ω w Az A mátrix a fenti összefüggésekből írható fel. Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 21 / 28

Propagáció Nominális állapot változása a bt ω bt ẋ t = f (x t, u, w) A propagáció elemei állapot vektor vezérlés zaj q t p t [ ] x t = v t u = am a n ω m ω n w = Hiba állapot változása Diszkrét időben δx = Aδx + Bũ + Cw δx n+1 = F x δx n + F u ũ n + F w w n F x = Φ = e A t, F u = B t, F w = C [ ] aw ω w Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 22 / 28

Állapotok propagálása A propagálás a mért adatok alapján t idővel hogy változik meg az állapottér A nominális állapotok a kinematikai egyenletek alapján változnak q q q{(ω m ω b ) t} p p + v t + 1 2 (R(a m a b ) + g) t 2 v v + (R(a m a b ) + g) t a b a b ω b ω b A hiba növekszik A P kovariancia mátrix elemei folyamatosan növekednek P F xpf T x + Q Q = t 2 BU c B T + tcw c C T Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 23 / 28

Update Valós mérésekből származó információk IMU + GPS IMU + monokuláris képi információk IMU + sztereó képi információk IMU + (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 A hiba nullázása K = PH T (HPH T + R) 1 δx K(z h( x)) P (I KH)P ˆ δx = 0 Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 24 / 28

MSCKF megvalósítás Áttekintés Az állapottér kiegészül a kamera hely és helyzet értékekkel M hosszú kamera hely és helyzet puffer x MSCKF = [x, {q GCi, p Ci,G}], i [1...M] A működés Új kép esetében "Feature track" menedzment Egy "feature track" egy térbeli ponthoz tartozik Egy "feature track" akkor ér véget, ha a "feature" már nem látható a "feature track" hossza nagyobb, mint M "Measurement update" minden befejezett "feature track"-re Kamera állapot kiterjesztés "Feature point" trianguláció Mérési modell alkalmazása Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 25 / 28

MSCKF megvalósítás Mérési modell Alap összefüggés, ahol, a hol r = H x + n (j) r a reziduál, minden j "feature point"-ra, r i = z (j) i, az i-dik képen a "feature" koordinátái a vetítési síkon z (j) i, 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 H (j) x i (j) H f i ẑ (j) i r (j) i = H (j) x i x + H (j) f i p fj,g + n (j) i ẑ (j) i, ahol a mérés Jakobi mátrixa az állapot hibájára vonatkozóan a mérés Jakobi mátrixa a "feature" térbeli helyére vonatkozóan Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 26 / 28

MSCKF megvalósítás Mérési modell Egyszerűsítés A a H f mátrix jobboldali nulltere r (j) 0 = A T (z (j) ẑ (j) ) A T H (j) x x + A T n (j) = H (j) 0 x(j) + n (j) 0 Megoldás r 0 = H x x + n 0 H x felbontása [ ] H x = [Q 1 Q 2 ] TH 0 r n = Q T 1 r 0 = T H x + Q T 1 n 0 R n = Q T 1 R 0Q 1 A Kálmán nyereség K = PT T H (T HPT T H + R n) 1 Az állapot korrekció x = Kr n Kovariancia mátrix változás P (I KT H )P(I KT H ) 1 + KR n K T Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 27 / 28

Felhasznált irodalom A. I. Mourikis,..., "A Multi-State Contraint Kalman Filter for Vision-aided Inertial Navigation", In Proc. 2007 IEEE International Conference on Robotics and Automation Michael Andrew Shelley, "Monocular Visual Odometry on a Mobile Device", M.Sc. Theses, University of München, 2014 Joan Sola, "Quaternion kinematics for the error-state KF", March 11, 2015 Etc... Lukovszki Csaba (BME TMIT) Navigációs szolgáltatások és alkalmazások 2015 28 / 28