19 I Vol.51, No.5, 309/318 2015 (CIF) Robust Global Scan Matching Method Using Congruence Transformation Invariant Feature Descriptors and a Geometric Constraint between Keypoints Takayuki Nakamura and Shohei Wakita This paper proposes a new global scan matching algorithm using the CIF descriptors and a geometric constraint between keypoints. The CIF descriptor was proposed in our previous work. It is a feature decriptor that is invariant against a congruence transformation. In our previous work, our method was able to perform robust local scan matching using CIF decriptors, but was apt to fail global scan mathching where a large map is used as the reference scan. In this paper, in order to resolve this problem, we propose to use a geometric constraint between keypoints in addtion to the CIF decriptors for the global scan mathching task. Our method can perform global scan matching in a cluttered environment without using an initial alignment. Through experiment in real environment, we confirm the validity of our method by comparing the performance of our method and that of our previous method. Key Words: CIF descriptor, geometric constraint, global scan matching, map updating, mobile robot 1. 1) 2) 3) 930 Faculty of Systems Engineering, Wakayama University, 930 Sakaedani, Wakayama Received June 20, 2014 Revised December 26, 2014 CIF (Congruence transformation Invariant Feature) 4), 5) CIF 2 4), 5) CIF 2. 1), 2), 6) 7) 10) TR 0005/15/5105 0309 c 2014 SICE
310 T. SICE Vol.51 No.5 May 2015 Konolige 7) Correlationbased Markov Localization Olson 8) Konolige Dellaert 9) Monte Carlo Localization (MCL) 10) 2 2 2 2 FFT CIF (Congruence transformation Invariant Feature) CIF 3. Fig. 1 t Scan(t) Map ( ) Δ x, Δỹ, Δ θ (1) (3) Map 1 Fig. 1 Overview of our global scanmatching method 3. 1 1 1 2 3 1 CIF 4 (2) CIF
51 5 2015 5 311 2 CIF 11) Fig. 1 (1) 1 δp max 2 δp 3 (Fig. 2 ) 11) ( ) yi+α y i η i =arctan x i+α x i η i ϑ i = η i+1 η i thresh CIF thresh Fig. 3 Definition of a keypoint Fig. 2 Rearranging scan data 3. 2 2 Fig. 1 (2) Fig. 3(a) p i(i =1, 2, N) p i,p i+α η i 4 η i 2 δp max =0.5m 3 δp =0.1m 0.1 0.2m 4 δp =0.1m α =1 3. 3 CIF (Congruence transformation Invariant Feature) CIF 3. 2 p CIF Fig. 1 (3) CIF Fig. 3(b) p Σ p p i 1,p i+1 p 2 2 η p p Σ p p p p Σ p 2 1 SP p p
312 T. SICE Vol.51 No.5 May 2015 h =(H SP [1]...H SP [8]H SD [1]...H SD [8]) Fig. 4 Definition of segment SP and its histogram p M 2M CIF 32 CIF p M CIF h M 2M CIF h 2M h 2M h M 2 h 2M 1/2 h M h 2M 32 CIF h h = (h M[1], )..., h M[16], h2m [1],..., h2m [16] 2 2 Fig. 5 Definition of segment SD and its histogram p Fig. 4(a) SP 1 SD n 1 p Fig. 5(a) SD p d M neighbor() neighbor() index neighbor() SP λ SP i η Σ p θ SP i p θ SP i π π 8 H SP Fig. 4(b) H SP H SP H SP [k], (k =1, 2, 8) neighbor() SD λ SD i η Σ p θ SD i p θ SD i π π 8 H SD Fig. 5(b) H SD H SD H SD [k], (k =1, 2, 8) p H SP H SD 16 16 16 3. 4 CIF p, (i =1 N KC) q j, (j =1 N KR) (Fig. 1 (4) ) 3 3 (p s, p t, p u) CIF 3 r L 3 3 CIF 3 3 d min 3 d max r L d min d max 3 5 5 r L =8.0m,d min =3.0m,d max =10.0m
s, p o) <r L, d(p t, p o) <r L, d(p u, p o) <r L d(p d min <d(p s, p t) <d max d min <d(p t, p u) <d max d min <d(p u, p s) <d max d(p 1, p 2) 2 p 1, p 2 p o 3 p, (i =1 N KC) N α L C L C : { (p s1, p t1, p u1), (p s2, p t2, p u2), (p snα, p tnα, p unα )} q j, (j = 1 N KR) 3 3 (q l, q m, q n) 3 3 3 6 7 d(p s, p t) δd < d(q l, q m) <d(p s, p t)+δd d(p t, p u) δd < d(q m, q n) <d(p t, p u)+δd d(p u, p s) δd < d(q n, q l ) <d(p u, p s)+δd 3 q j, (j =1 N KR) N β L R L R : { (q l1, q m1, q n1), (q l2, q m2, q n2), (q lnβ, q mnβ, q nnβ ) } 3 3 CIF 96 3 3 L C α 3 CIF 3 96 H α L R β 3 CIF 3 96 H β H α = ( h s [1],..., h s [32], h t [1],..., h t [32], h u [1],..., h u [32] ) 6 3 7 δd =0.1m 51 5 2015 5 313 ( ) H β = h l [1],..., h l [32], h m [1],..., h m [32], h n [1],..., h n [32] 2 1 96 0 1 H α H α H α[k] = Hα[k] 96 H α[k] k=1 H α H β 3 (p sα, p tα, p uα) 3 (q lβ, q mβ, q nβ ) S (α, β) S (α, β) = 96 k=1 H α[k] H β [k] 3 (p sα, p tα, p uα) (q lβ, q mβ, q nβ ) α =1 N α,β =1 N β { } (α,β ) = arg max α arg max S(α, β) β (p sα, p tα, p uα ) (q lβ, q mβ, q nβ ) CIF CIF 3. 5 2 1 (Fig. 1 (5) ): 12), 13) 3 12), 13) 2 (Fig. 1 (6) ):
314 T. SICE Vol.51 No.5 May 2015 14), 15) ICP o ICP [i,j (i )] w j (i) O o = w j (i) O Fig. 6 Map of environment I (reference scan) w j (i) w j () =1 / C j (), w j (i) = w j (i) 3 w j (i) j (i)=1 C j C j = 32 k=1 (h k hj k )2 ICP ICP ICP 4. SICK LMS100 270 0.25 Mobilerobots (P3-DX) A 1 5 PC(2.70 GHz Intel Corei7-2620M 8GB of RAM) 55 m 55 m (MRPT) 16) Fig. 6 A 1 ( I) 8 ID 26 8 21 3630 δp =0.1m 4. 1 CIF CIF thresh =40deg M =20 Fig. 7 CIF 9 4 10 CIF 1 CIF Fig. 7 Example result of finding corresponding points based on only CIF descriptors Fig. 8 CIF 11 9 66 ms 10 338 43 555 56 1.39 ms 1.11 ms 11 22651 ms
51 5 2015 5 315 Table 1 Success /failure of finding corresponding points by our method at point [4] in case of changing M and thresh thresh =20 thresh =30 thresh =40 M =20 M =30 M =40 Fig. 8 Example result of finding corresponding points by our method 4 3 3 3 3 26 CIF 26 7 ( 27%) CIF 26 19 ( 73%) CIF CIF thresh M Fig. 8 Fig. 6 4 43 Table 1 thresh M 20 40 ( ) M (CIF ) thresh thresh = 30 deg M =20 Fig. 9 12 Fig. 6 5 307 43 18 Fig. 9 Table 2 Example result of finding corresponding points by our method Success /failure of finding corresponding points by our method at point [5] in case of changing M and thresh thresh =20 thresh =30 thresh =40 M =20 M =30 M =40 Table 2 thresh M 20 40 M thresh thresh =40deg M =20 thresh =40deg 35 12 174643 ms
316 T. SICE Vol.51 No.5 May 2015 thresh, M MCL Fig. 6 26 (MRPT) 16) 5000 26 19 MCL MCL 13 4. 2 ( ) Σ W Δ x, Δỹ, Δ θ (5), (6) Fig. 10 (6) 14 (MRPT) 16) ICP O =50 3 Table 3 Fig. 10 error ( ) Δ x, Δỹ, Δ θ Table 3 Result of global self-localization by our method in Fig. 9 Δx [m] Δy [m] Δθ [deg] ground truth 10.62 2.74 137.5 our method 10.54 2.71 137.4 error 0.08 0.03 0.1 4. 3 Fig. 11 A 5 ( II) 15 Fig. 12 16 Fig. 10 Result of precise matching by our method 13 MCL MCL 14 ( ICP) 21872 ms Fig. 11 Map of environment II (reference scan) 15 33 2395 δp =0.1m 16 1887 ms
51 5 2015 5 317 Fig. 12 Result of finding corresponding points in environment II by our method ( ) 17 Fig. 13 Fig. 12 18 MCL ( ) Fig. 11 MCL MCL ( ) (MRPT) 16) 5000 Fig. 14 MCL 19 MCL Fig. 14 The result of precise alignment of reference and input (dark gray dots) scans using MCL method in the environment II Fig. 13 The result of precise alignment of reference (gray dots) and input (dark gray dots) scans based on pairs of the corresponding points in the environment II Table 4 Results of global self-localization by our method in Fig.12andMCLmethodinFig.13 Δx [m] Δy [m] Δθ [deg] ground truth 0.46 0.64 49.4 our method 0.51 0.73 49.8 error 0.05 0.09 0.4 MCL 18.01 32.11 215.2 error 17.55 31.47 165.8 17 361 261 57 1.09 ms 0.90 ms 18 ( ICP) 1996 ms Table 4 II MCL 19 MCL 384088 ms
318 T. SICE Vol.51 No.5 May 2015 error MCL ( ) ( ) 5. CIF CIF ICP 3 ICP ( (C) No. 26450366) 1 J.S. Gutmann, T. Weigel and B. Nebel: Fast, Accurate, and Robust Self-Localization in Polygonal Environments, Proc. IROS 99, 1412/1419 (1999) 2 P. Jensfelt and S. Kristensen: Active Global Localization for a Mobile Robot Using Multiple Hypothesis Tracking, IEEE Trans. Robotics and Automation, 17-5, 748/760 (2001) 3 25-3, 66/77 (2007) 4 T. Nakamura and Y. Tashita: Congruence Transformation Invariant Feature Descriptor for Robust 2D Scan Matching, Proc. 2013 IEEE International Conference on Systems, Man, and Cybernetics (SMC), SYS-10 (2013) 5 2D (CIF) 19 6C3, 592/598 (2014) 6 M. Tomono: A scan matching method using Euclidean invariant signature for global localization and map building, Proc. ICRA 04, 866/871 (2004) 7 K. Konolige and K. Chou: Markov Localization Using Correlation, Proc. IJCAI 1999, 1154/1159 (1999) 8 E. Olson: Real-time Correlative Scan Matching, Proc. ICRA 2009, 4387/4393 (2009) 9 F. Dellaert, D. Fox, W. Burgard and S. Thrun: Monte Carlo Localization for Mobile Robots, Proc. ICRA 1999, 1322/1328 (1999) 10 S. Bando, Y. Hara and T. Tsubouchi: Global Localization of a Mobile Robot in Indoor Environment Using Spatial Frequency Analysis of 2D Range Data, Proc. ICMA 2013, 488/493 (2013) 11 G.A. Borges and M.J. Aldon: Line extraction in 2D range images for mobile robotics, Journal of Intelligent and Robotic Systems, 40-3, 267/297 (2004) 12 K. Lingemann, H. Surmann, A. Nuchter and J. Hertzberg: Indoor and outdoor localization for fast mobile robots, Proc. IROS 04, 2185/2190 (2004) 13 2 28-5, 648/657 (2010) 14 P.J. Besl and N.D. McKay: A Method for Registration of 3-D Shapes, IEEE Trans. Pattern Analysis and Machine Intelligence, 14-2, 239/256 (1992) 15 F. Lu and E. Milios: Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans, Journal of Intelligent and Robotic Systems, 18-3, 249/275 (1997) 16 The Mobile Robot Programming Toolkit (MRPT): http://www.mrpt.com 1996 97 2002 2013 96 1 2007 1 2013