Pose Estimation of Multiple Cameras with Particle Filters Evaluation on Simulation

Transcription

1 ( ) Pose Estmaton of Multple Cameras wth Partcle Flters Evaluaton on Smulaton *R. Ueda, S. Nkolads, P. Kamol A. Hayash and T. Ara (Unv. of Tokyo) Abstract In ths paper we propose a novel algorthm for estmatng postons and poses of cameras. Ths method utlzes partcle flters to estmate relatve poses of pars of cameras. When we consder geometrc constrants of three cameras, the partcle flters can work well wth small numbers of partcles n sx dmensonal space. Key Words: partcle flters, geometrc constrants, mult-camera system. IP [, 2] [3, 4] [3, 4] [5] [6] 3 3 [7] [8, 9] 2. 2 Fg. {Cam =, 2,...,n} Cam Σ Cam Σ z Cam Fg. Assumed Camera System ζ = ( m x m y m z ) T ( m x / m z, m y / m z )

2 2 2 Σ Σ (, ) R p T = () Cam Cam N { T [n] n =, 2,...,N} T [n] w [n] t [n] =( T [n],w [n] ) R p Σ Σ R 3 3 Σ p =( x y z) T ( =, 2,...,n) p = p T (2) R p Cam Cam ζ ζ Cam Cam 3. ζ ζ Fg.2(a) p ob Σ ζ p p( p ζ) p( p ζ) ( p( p x ζ)=p r, y m ) x m y, m p( r m r ), (3) r m r r m r r ( m x / m r, m y / m r ) Fg.2 Samplng based on Obect Measurements from Two Cameras ζ ζ (3) p( p ζ), p( p ζ) (4) (2) = T [n] = [ R [n] 0 ] (5) = (6) 6 (6) Σ Fg.2(b) R [n] Σ tmp Σ z z z tmp = z z tmp =(x tmp y tmp z tmp ) T x x tmp = ( x tmp y tmp 2z tmp) T (7) y y tmp = z tmp x tmp

3 T [n] tmp T [n] tmp = cos θ roll sn θ roll 0 0 x tmp y tmp z tmp x tmp y tmp z tmp 0 sn θ roll cos θ roll (8) θ roll tmp T [n] =[00z ] T ( T tmp) [n] =[00z ] T (9) Σ tmp z z (9) Σ tmp Σ Σ tmp Σ Σ tmp y x T [n] = T [n] tmp T pant tlt, (0) cos θ pan 0 sn θ pan 0 T pan = sn θ pan 0 cosθ pan 0 () T tlt = 0 cosθ tlt sn θ tlt 0 0 sn θ tlt cos θ tlt 0 (2) z < 0cos θ pan cos θ tlt T pan T tlt s 2 pan 0 s pan T pan = s pan 0 s 2 pan T tlt = 0 s 2 tlt s tlt 0 0 s tlt s 2 tlt (3) (4) (5) (0) T pan( T [n] tmp) [ [ = T [n] tmp T pant tlt ] = T tlt T pan[0 0 z ] T = T tlt [ ] ] (5) z s pan = x [n] (6) 0= y [n] s 2 tlt + z [n] s tlt (7) z s 2 pan = y [n] s tlt + z [n] s 2 tlt (8) (6) (7) ( ) x (s pan,s tlt )= z, ± y 4 + z 2 y 2 y 2 + z 2 (9) (8) (s pan,s tlt ) (0) T [n] 3 2 T = T k k T (20) t [n] T [n] = T [n ] k T [n ], and (2) w [n] = ηw [n ] ] k w[n k (22) n n, 2,...,N {t [n] k n =, 2,...,N} {t[n] k n =, 2,...,N} (22) η Fg.3 Samplng of A Partcle T [n] Constrant of Three Cameras from Geometrc

4 3 T [n] Cam k {t [n] n =, 2,..., N} T [n] = t [n] ( ) T [n] (23) = t [n] (24) {t [n] n =, 2,...,N} 3 3 ζ ζ T [n] t [n] t [n] := ηp( ζ ζ, T [n] )P ( T [n] ) = ηp( ζ ζ, T [n] )t [n] (25) ( ) = ηp( ζ ζ, T [n] )t [n] p( ζ ζ, T [n] ) (3) p( p ζ) T [n] Σ p( p ζ) [7] Σ ζ ζ p( p ζ) Σ Σ p( p ζ) σ [0] : ζ σ ( [] σ [4] (3) x p r, y m ) x m y r, m r m r σ [0] σ [5] σ [6] Σ ζ (3) p( r m r ) σ [0] T [n] σ [l] = T [n] σ [l]. (26) Σ σ [l] (l = 0,,...,6) ζ σ [l] ζ (3) σ [l] ζ p( ζ σ [l] ) ζ ζ p( ζ ζ, T [n] ) 4 p( ζ σ [0] )+ 8 6 p( ζ σ [l] ) l= (27) 3 4 Cam Cam Cam Cam Cam Cam Cam k Cam Cam α[%] β[%] ±5[deg] ±0.05[m] 5[deg] 0.05[m] 4. z 4[m] 4[m] [step] 0.5[m] Robot Roomba [step] 2 σ depth [m] r[m]

5 rσ mage [m] [m] σ mage [m] Σ Σ 2[m] z x y z = [0, ] [7] α N = 2000 α = β =[%] σ depth = 0.05[m] σ mage = Σ Fg.4 Fg.5 000[step] 500[m] β Fg.6 000[step] top vew 2000 Cam T [] 2 ( =, 2,...,2000) Cam 2 Cam 2 Cam 3 Cam 3 Cam 4 2 σ depth,σ mage σ depth = 0.2, 0.4, [m] σ mage =0.05, 0., 0.2 Table Fg.4 Poses of Cameras σ depth =[m] σ depth σ mage σ depth =0.4[m] σ mage =0.2 σ mage =0.2 2[m] 400[mm] N = [step] 2.0GHz CPU 4[s] 5. 3

6 Table Estmaton Errors after M Steps (absolute avg. of the three cameras) (a) poston error [mm] σ depth σ mage M = 250 M = 500 M = 750 M = [m] [m] [m] [m] [m] [m] [m] [m] [m] (b) drecton error of z-axs [deg] σ depth σ mage M = 250 M = 500 M = 750 M = [m] [m] [m] [m] [m] [m] [m] [m] [m] Fg.5 Wth/Wthout Samplng wth The Constrant B Fg.6 Coordnate Systems Drawn from Partcles after 000 steps 3 400[mm] 200[mm] 5[deg] [] T. Hasegawa and K. Murakam: Robot Town Proect: Supportng Robots n an Envronment wth Its Structured Informaton, In Proc. of Internatonal Conference on Ubqutous Robots and Ambent Intellgence, pp. 9 23, [2],, :, C, Vol. 73, No. 725, pp , [3], : 3,, 998. [4] :,, 999. [5] K. Matsumoto, et al.: Automatc Parameter Identfcaton for Dstrbutedly Placed Modular Robots, In Proc. of IEEE ICRA, pp , [6] :, 24, 2N4, [7] S. Thrun, et al.: Probablstc ROBOTICS, MIT Press, [8] D. Fox, Monte Carlo Localzaton: Effcent Poston Estmaton for Moble Robots, In Proc.ofAAAI, pp , 999. [9] A. Doucet, et al. : Sequental Monte Carlo Methods n Practce, Sprnger-Verlag, 200. [0].,, Vol. 23, No. 4, pp. 84 9, [] S. Lenser and M. Veloso: Sensor resettng localzaton for poorly modelled robots, In Proc. of IEEE ICRA, pp , 2000.