Fig Measurement data combination. 2 Fig. 2. Ray vector. Fig (12) 1 2 R 1 r t 1 3 p 1,i i 2 3 Fig.2 R 2 t 2 p 2,i [u, v] T (1)(2) r R 1 R 2

Transcription

1 IP / IIS D Environment Modeling from Images Acquired with an Omni-Directional Camera Mounted on a Mobile Robot Atsushi Yamashita, Tomoaki Harada, Ryosuke Kawanishi, Toru Kaneko (Shizuoka University) Abstract Measurement and modeling of a surrounding environment is important for mobile robots to move autonomously. In this paper, we propose a new method for environment measurement using an omni-directional camera on a mobile robot. Our method is based on a structure-from-motion under the assumption that the environment is static. The method measures surrounding environment at the same time as estimating the robot s motion. We show the validity of our method through experiments. 3 (Structure from Motion, Omni-Directional Camera, 3-D Modeling, Single Camera, Estimation of Position and Posture ) 1. (6) GPS 3 (7) SFM Structure from Motion (1) (8) SFM (1) (9) (10) (2) 360 (3) (4) (5) 1 2 R t R 1 t weakly calibrated stereo R t R t 3 1/6

2 Fig Measurement data combination. 2 Fig. 2. Ray vector. Fig (12) 1 2 R 1 r t 1 3 p 1,i i 2 3 Fig.2 R 2 t 2 p 2,i [u, v] T (1)(2) r R 1 R 2 t 1 t su 1 r = sv (1) (su) 2 + (sv) 2 + (sf 2c) 2 sf 2c t i t 1 s = a2 (f a 2 + b 2 + b u 2 + v 2 + f 2 ) (2) a t 2 2 f 2 b 2 (u 2 + v 2 ) a b c f 3 p 1,i p 2,i t i 3 3 E 2 r i = [x i, y i, z i ] T r i = [x i, y i, z i] T E (3) r T i Er i = 0 (3) (3) (4) u T i e = 0, (4) KLT Kanade-Lucas-Tomasi (11) Tracker u i = [x i x i, y i x i, z i x i, x i y i, y i y i, z i y i, x i z i, y i z i, z i z i] T l (5) e = [e 11, e 12, e 13, e 21, e 22, e 23, e 31, e 32, e 33] T (6) e ab E a b l E 8 (7) 3 2 KLT Tracker n 3 n r = [x, y, z] T min (r T i Er i) 2 (7) E T i=1 2/6

3 min E Ue 2 (8) U = [u 1, u 2,..., u n] T 0 ±1 0 e U T Y = (17) U 0 0 det UV E T ±1 0 Z = (18) RANSAC (13) E T t R t 8 E rand r i r i (9) k 3 6 R m t m m r T i E rand r i < q (9) R m t m 3 q E rand k 2 k w k (9) (8) E 3 5 E R t = [t x, t y, t z] T (10) E = RT (10) 0 t z t y T = t z 0 t x (11) t y t x 0 (r T m,ir T mr m,i)(r T m,ir T mt m) (20) t B m,i = (r T m,it m)(r T m,ir mr m,i) 1 (r T m,ir m,i )(r T m,ir T mt m ) (21) t = 1 E t = 1 T (10) T R E = 2 (12) E 2 = E (12) p m,i 2 [u m,i, v m,i ] T E [u m,i, v m,i] T (22) E R T g g (23) E = UΣV (13) g = Σ = diag(r, s, 0) pm,i + p m,i + p m,i + p m,i r s E 1 R r = s = 1 (14) (15)(16) R T E = UYV T VZV T (14) R = UYV T (15) T = VZV T (16) 2 i p m,i (19) p m,i = 1 2 { A m,ir m,i + B m,ir T mr m,i (r T m,i r m,i)(r T m,ir m,i) (r T m,i RT mr m,i) 2 + t m (19) A m,i = (r T m,it m)(r T m,ir m,i) u m,i v m,i u m,i v m,i (22) g < h (23) h } 3/6

4 3 Fig. 3. Scale mismatching. (a) Omni-directional camera. (b) Mobile robot. 4 Fig. 4. Experimental system. 1 Fig.3 2 c P m,i = [x m,i, y m,i, z m,i] T P m+1,i = [x m+1,i, y m+1,i, z m+1,i] T P m,i P m+1,i c 5 Fig. 5. Feature points in an omnidirectional image. 2 s (24) 3 n min log(p m,i c) log(s p m+1,i c) (24) pixels s i= L 40cm/s 15fps frame 530mm 3 Fig.5 3 RANSAC l = RANSAC q = h = GHz CPU SONY HDR- 2.0s HC1 SOIOS70-SCOPE 0.6s Fig.4(a) Mobile Robots lnc. w = 5000 Fig.6 Pioneer3 Fig.4(b) 2 (a) Fig.4 (b) (c) 4/6

5 (a) 10 frame. (a) Result 1. (b) Result 2. (b) 30 frame. (c) 70 frame. (c) Result 3. (d) Result 4. 7 Fig. 7. Influence of baseline length. 6 Fig. 6. Outlier and low accuracy points rejection. (d) L (a) (a) Dead reckoning. (b) Proposed method. 8 (b) Fig. 8. Dead reckoning and proposed method. (c) (d) Fig.8 Fig.6 (a) (b) 1 (a) 10frame 30frame (b) 70frame Fig.7 (a) 10frame (b) 2.6% (b) 30frame 3 Fig.9 (a) (b) (c) 70frame 3 (c) 3 5/6

6 (b) Wire frame Fig. 9. (a) Actual image. (c) Result of 3-D modeling. 3 Result of 3-D modeling. 3 Fig. 10. Result of 3-D movement. 1 S. Thrun: Robotic Mapping: A Survey, Technical Report CMU-CS , Carnegie Mellon University IEEE International Conference on Robotics and Automation, pp (2005) 3 8 R. Hartley, R. Gupta and T. Chang: Stereo from Uncalibrated Cameras, Proceedings of the 1992 IEEE 4 Fig.10 (a) 3 Computer Society Conference on Computer Vision and Pattern Recognition, pp (1992) (b) 3 9 J. Gluckman and S. K. Nayar: Ego-Motion and Omnidirectional Cameras, Proceedings of 6th International Conference on Computer Vision, pp (1998) 10 M. Oe, T. Sato and N. Yokoya: Estimating Camera Position and Posture by Using Feature Landmark 5. Database, Proceedings 14th Scandinavian Conference SFM on Image Analysis, pp (2005) 3 11 J. Shi and C. Tomasi: Good Features to Track, Proceedings of the 1994 IEEE Computer Society Confer- ence on Computer Vision and Pattern Recognition, pp (1994) 12 K. Yamazawa, Y. Yagi and M. Yachida: HyperOmni Vision: Visual Navigation with an Omnidirectional Image Sensor, Systems and Computers in Japan, Vol.28, No.4, pp (1997) 3 13 M. A. Fischler and R. C. Bolles: Random Sample Consensus: A Paradigm for Model Fitting with Applications to Image Analysis and Automated Cartogra- phy, Communications of the ACM, Vol. 24, pp (1981) (2002) 2 J. Gaspar, N. Winters and J. Santos-Victor: Vision- Based Navigation and Environmental Representations with an Omnidirectional Camera, IEEE Transactions on Robotics and Automation, Vol.16, No.6, pp (2000) 3 Y. Yagi: Omnidirectional Sensing and Its Applications, IEICE Transactions on Information and Systems, Vol.E82-D, No.3, pp (1999) 4 J. Takiguchi, M. Yoshida, A. Takeya, J. Eino and T. Hashizume: High Precision Range Estimation from an Omnidirectional Stereo System, Proceedings of the 2002 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp (2002) 5 H. Koyasu, J. Miura and Y. Shirai: Mobile Robot Navigation in Dynamic Environments Using Omnidirectional Stereo, Proceedings of the 2003 IEEE International Conference on Robotics and Automation, pp (2003) 6 J. Meguro, J. Takiguchi, Y. Amano and T. Hashizume: Omni-directional Motion Stereo Vision based on Accurate GPS/INS Navigation System, Proceedings of the 2nd Workshop on Integration of Vision and Inertial Sensors (2005) 7 M. Tomono: 3-D Localization and Mapping Using a Single Camera Based on Structure-from-Motion with Automatic Baseline Selection, Proceedings of the /6