The Efficient Extension of Globally Consistent Scan Matching to 6 DoF

Transcription

1 The Efficient Extension of Globally Consistent Scan Matching to 6 DoF Dorit Borrmann, Jan Elseberg, Kai Lingemann, Andreas Nüchter, Joachim Hertzberg 1 / 20

2 Outline 1 Introduction 2 Algorithm 3 Performance 4 Results 2 / 20

3 Scan Matching 3 / 20

4 Sequential vs. Globally Consistent 4 / 20

5 Sequential vs. Globally Consistent Globally Consistent Range Scan Alignment for Environment Mapping, F. Lu and E. Milios (1997) Previously only 3-d poses - here extended to 6-d 4 / 20

6 Problem Definition Given: Wanted: A sequence of 3D scans Initial 6-d pose estimates with Gaussian noise New pose estimates Globally consistent 3D map 5 / 20

7 Algorithm 1 Sequential scan matching with loop-closing 1 Sequential scan matching repeat Register scan with predecessor (ICP) until pose differences small 2 Create graph Edge between consecutive scans Edge between scans with corresponding points 6 / 20

8 Algorithm 2 Estimation algorithm according to Lu and Milios 1 Determine point correspondences 2 For each edge in the graph determine pose differences and covariances 3 Create equation system from pose differences and covariance matrices W = (i,j) (D i,j D i,j ) T C 1 i,j (D i,j D i,j ) 4 Linearize and solve equation system 5 Revise poses and covariances 7 / 20

9 8 / 20

10 8 / 20

11 8 / 20

12 8 / 20

13 8 / 20

14 8 / 20

15 8 / 20

16 8 / 20

17 Improving Performance Fast search of corresponding points Fast construction of linear system Fast matrix inversion 9 / 20

18 Searching corresponding point pairs n points per scan Unoptimized closest point search takes O(n 2 ) 10 / 20

19 Searching corresponding point pairs n points per scan Unoptimized closest point search takes O(n 2 ) Storing points in k-d tree reduces cost to O(n log n) 10 / 20

20 Searching corresponding point pairs Problem: Solution: Each global optimization step transforms all scan poses Recomputing k-d tree each step is inefficient Calculate k-d tree only once Transform query point into local coordinate system 11 / 20

21 Constructing the linear system Needed: C 1 i,j = (M T M)/s 2 D i,j = (M T M) 1 M T Z 12 / 20

22 Constructing the linear system Needed: C 1 i,j = (M T M)/s 2 D i,j = (M T M) 1 M T Z M R 3n 6 Z R 3n 1 12 / 20

23 Constructing the linear system Needed: C 1 i,j = (M T M)/s 2 D i,j = (M T M) 1 M T Z M R 3n 6 Z R 3n 1 M T M = 0 nx k= y k z k z k x k y k 0 x k 0 z k y k yk 2 + z2 k x k z k x k y k y k x k 0 x k z k yk 2 + x2 k y k z k z k 0 x k x k y k y k z k xk 2 + z2 k 1 C A M T Z = 0 nx B x k y k z k z k y k + y k z k y k x k + x k y k z k x k x k z k 1 C A 12 / 20

24 Matrix Inversion Solving the linear equation system by matrix inversion (O(n 3 )) Matrix is positive definite use Cholesky decomposition Matrix is sparse number of entries number of overlapping scans sparse Cholesky decomposition (O(number of entries)) 13 / 20

25 Computing times Simple matrix inversion Cholesky decomposition Sparse Chol. decomposition Standard k-d tree search Improved k-d tree search Absolute time in ms Relative time in % Absolute time in ms Relative time in % University building Bridge 14 / 20

26 Campus of the Leibniz University of Hannover Laser scans by courtesy of O. Wulf und B. Wagner (University of Hannover) 15 / 20

27 Complete Map - Leibniz University of Hannover 16 / 20

28 Matrix in iteration / 20

29 Market square in Horn (Austria) 13 scans points per scan error correction from cm to < 9cm Laser scans by courtesy of N. Studnicka (RIEGL Laser Measurement Systems GmbH) 18 / 20

30 19 / 20

31 Conclusions Sequential scan matching leads to summation of errors Presented approach leads to globally consistent scan matching Functionality in all 6 degrees of freedom Some modifications allow handling of large amounts of data in reasonable time 20 / 20