Introduction Scan registration Experimental environment Experiments and results Conclusions

3D Mapping the Kvarntorp Mine— A Field Experiment for Evaluation of 3D

Scan Matching Algorithms

Martin Magnusson1, Andreas Nüchter2,Christopher Lörken2, Achim J. Lilienthal1, and

Joachim Hertzberg2

1AASS, Örebro University2KBS, Osnabrück University

September 26, 2008

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Outline

1 IntroductionOutlineBenchmarking

2 Scan registrationICPNDTExperimental setup

3 Experimental environmentThe Kvarntorp mine

4 Experiments and resultsValley of convergenceOutlier countCumulative errorExecution time

5 Conclusions

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Benchmarking

Unbiased comparisons

We are all biased.

Novel algorithms are often compared to previous workwithout the experience and thoroughness of the originalauthors.

We have attempted an unbiased comparison.

Two groups ran their implementations using their hands-onknowledge about parameter selection, using the samehardware and the same input data.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Benchmarking

Repeatable and evaluatable comparisons

Repeatable test:publicly available data,specify all relevant parameters,show some cases where the method doesn’t perform well.

Performance metrics:quantitative error measures,execution time.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

What is scan registration?

The process of aligning overlapping scans (point clouds).

First step of building a metric map.

Find the correct pose of the current scan by comparingsurface shapes with the previous scan.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

ICP

ICP: the iterative corresponding point algorithm

Find closest point in previous scan for each point in currentscan.

Minimise point-to-point distances.

Iterate until convergence.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

ICP

ICP specifics

Approximate kd trees

Cached kd treesAndreas Nüchter, Kai Lingemann, Joachim Hertzberg, andHartmut Surmann, 6D SLAM — 3D mapping outdoorenvironments, J. Field Robotics 24 (2007), no. 8–9,699–722.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

ICP

ICP specifics

Approximate kd trees

Cached kd treesAndreas Nüchter, Kai Lingemann, Joachim Hertzberg, andHartmut Surmann, 6D SLAM — 3D mapping outdoorenvironments, J. Field Robotics 24 (2007), no. 8–9,699–722.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

NDT

NDT: the normal distributions transform

Probabilistic approach to scanregistration.

Represent the reference surfacewith probability density functions(normal distributions).

Registration = Finding thefunction optimum whenevaluating the PDFs at the pointsin the current scan = Just anordinary numerical optimisationproblem.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

NDT

NDT variants

Discretising space into aregular grid gives a fewproblems:

finding the best cell size,limited field of influence,discontinuities at cellboundaries.

Solutions:

iterative NDT,linked cells,trilinear interpolation.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

NDT

NDT variants

Discretising space into aregular grid gives a fewproblems:

finding the best cell size,limited field of influence,discontinuities at cellboundaries.

Solutions:iterative NDT,

linked cells,trilinear interpolation.

Martin Magnusson, Achim J. Lilienthal, and Tom Duckett,Scan registration for autonomous mining vehicles using3D-NDT, J. Field Robotics 24 (2007), no. 10, 803–827.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

NDT

NDT variants

Discretising space into aregular grid gives a fewproblems:

finding the best cell size,limited field of influence,discontinuities at cellboundaries.

Solutions:iterative NDT,

linked cells,trilinear interpolation.

Martin Magnusson, Achim J. Lilienthal, and Tom Duckett,Scan registration for autonomous mining vehicles using3D-NDT, J. Field Robotics 24 (2007), no. 10, 803–827.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

NDT

NDT variants

Discretising space into aregular grid gives a fewproblems:

finding the best cell size,limited field of influence,discontinuities at cellboundaries.

Solutions:iterative NDT,

linked cells,trilinear interpolation.

Martin Magnusson, Achim J. Lilienthal, and Tom Duckett,Scan registration for autonomous mining vehicles using3D-NDT, J. Field Robotics 24 (2007), no. 10, 803–827.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

NDT

NDT variants

Discretising space into aregular grid gives a fewproblems:

finding the best cell size,limited field of influence,discontinuities at cellboundaries.

Solutions:iterative NDT,linked cells,

trilinear interpolation.

Martin Magnusson, Achim J. Lilienthal, and Tom Duckett,Scan registration for autonomous mining vehicles using3D-NDT, J. Field Robotics 24 (2007), no. 10, 803–827.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

NDT

NDT variants

Discretising space into aregular grid gives a fewproblems:

finding the best cell size,limited field of influence,discontinuities at cellboundaries.

Solutions:iterative NDT,linked cells,trilinear interpolation.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

NDT

NDT variants

Discretising space into aregular grid gives a fewproblems:

finding the best cell size,limited field of influence,discontinuities at cellboundaries.

Solutions:iterative NDT,linked cells,trilinear interpolation.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Experimental setup

Parameters

NDTIterative discretisation: 2 m, 1 m, 0.5 m.Linked cells.Newton optimisation: max step size |∆~p| = 0.2, convergencethreshold |∆~p| < 10−6.

ICPCached kd trees, 10 points per bucket.Distance threshold 0.5 m.Convergence treshold |∆~p| < 10−6

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

The Kvarntorp mine

Location

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

The Kvarntorp mine

Location

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

The Kvarntorp mine

Location

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

The Kvarntorp mine

Location

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

The Kvarntorp mine

Location

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

The Kvarntorp mine

Location

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

The Kvarntorp mine

Location

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

The Kvarntorp mine

Characteristics

Limestone mine (no longer in production).

Wide tunnels and relatively planar walls and ceiling.

Data collected in a part of the mine used for testing ofautomated loading vehicles.

Smooth artificial walls erected along some tunnels to makethem narrower and more challenging for tunnel followingbehaviours.

Tunnels are about 5 m high and 5 to 10 m wide.

Scans subsampled to 8000 points, around 20 m long.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Valley of convergence

Surveying the valley of convergence

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Valley of convergence

Plot legend

Use 7 × 7 translation offsets from-2 m to +2 m.

For each translation offset, use 9rotation offset from -80◦to +80◦.

Success: position within 1.0 m or0.2 m and orientation within 5◦

of reference.

0o

180o

90o

-90o

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Valley of convergence

NDT vs. ICP (loose threshold)

-2

-1

0

1

2

-2 -1 0 1 2

-2

-1

0

1

2

-2 -1 0 1 2

Success rate: 77% for NDT, 30% for ICP.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Valley of convergence

NDT vs. ICP (strict threshold)

-2

-1

0

1

2

-2 -1 0 1 2

-2

-1

0

1

2

-2 -1 0 1 2

Success rate: 37% for NDT, 13% for ICP.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Valley of convergence

NDT with and without trilinear interpolation(loose threshold)

-2

-1

0

1

2

-2 -1 0 1 2

-2

-1

0

1

2

-2 -1 0 1 2

Success rate: 77% for NDT, 95% with trilinear interpolation.M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Valley of convergence

NDT with and without trilinear interpolation(strict threshold)

-2

-1

0

1

2

-2 -1 0 1 2

-2

-1

0

1

2

-2 -1 0 1 2

Success rate: 37% for NDT, 95% with trilinear interpolation.M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Outlier count

Outlier count

Count how many times the initial pose estimate (fromodometry) had to be manually altered for convergence.Results:

ICP: 1 (scan nr 33)NDT: 1 (scan nr 23)Trilinear NDT : none

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Cumulative error

Cumulative error at loop closure

Measure accumulated errorafter registering 55 scans(160 m trajectory).

Reference: register last scanof loop to first scan.

(Only a rough estimate ofregistration performance.)

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Cumulative error

Trilinear NDT

Accumulated translation error: 3.99 m

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Cumulative error

ICP

Accumulated translation error: 2.97 m

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Cumulative error

NDT

Accumulated translation error: 2.26 m

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Execution time

Execution time

0

5

10

15

20

25T

ime

(s)

ICP 0

5

10

15

20

25

Tim

e (s

)

NDT 0

5

10

15

20

25

Tim

e (s

)

Trilinear NDT

All three algorithms can be performed in “real time”. Vehiclemotion and (even more so) scanning speed are the mainbottlenecks.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Summary of results

ICP NDT Trilinear NDTConvergence (1.0 m) 30% 77% 95%Convergence (0.2 m) 13% 37% 95%Outliers 1/55 1/55 0/55Time 4.8 s 2.4 s 9.5 s

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Summary of results

ICP NDT Trilinear NDTConvergence (1.0 m) 30% 77% 95%Convergence (0.2 m) 13% 37% 95%Outliers 1/55 1/55 0/55Time 4.8 s 2.4 s 9.5 s

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Summary of results

ICP NDT Trilinear NDTConvergence (1.0 m) 30% 77% 95%Convergence (0.2 m) 13% 37% 95%Outliers 1/55 1/55 0/55Time 4.8 s 2.4 s 9.5 s

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Summary of results

ICP NDT Trilinear NDTConvergence (1.0 m) 30% 77% 95%Convergence (0.2 m) 13% 37% 95%Outliers 1/55 1/55 0/55Time 4.8 s 2.4 s 9.5 s

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

Conclusions

Trilinear NDT converges from the largest range of poses.

ICP has stable but narrow valley of convergence.

Standard NDT is the fastest.

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms

Introduction Scan registration Experimental environment Experiments and results Conclusions

x

M. Magnusson, A. Nüchter, C. Lörken, A. J. Lilienthal, and J. Hertzberg

3D Mapping the Kvarntorp Mine — A Field Experiment for Evaluation of 3D Scan Matching Algorithms