Beyond Points: Evaluating Recent 3D Scan-Matching

Home

Algorithms

Methodology

Results

References

Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms Algorithms

Methodology

1

1

0.8

0.8 Probability

Probability

Results 0.6 0.4 Apartment Stairs ETH Gazebo Wood Plain

0.2

0.6 0.4 71% 54% 57% 17% 14% 6%

0.2

0

0 0

0.2 0.4 0.6 0.8 Error (m) P2D-NDT

1

0

0.2 0.4 0.6 0.8

1

Error (m) MUMC DC-ON

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

ICP: iterative closest point Outline: For each point in one scan, find closest neighbour in other scan. 2 Minimise distance metric: 1

point-to-point distance [Besl and McKay 1992], or point-to-plane distance [Chen and Medioni 1992]. 3

Repeat.

Point-to-plane typically has smaller median error for these data sets. Using libpointmatcher implementation.

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

NDT: normal-distributions transform Represent point cloud as voxel grid with Gaussians. Applications: Scan matching [Biber and Straßer 2003; Magnusson et al. 2007; Stoyanov et al. 2012] Mapping [Saarinen et al. 2013a] Precise and efficient localisation [Saarinen et al. 2013b; Valencia et al. 2014] Fast loop detection [Magnusson et al. 2009]

Using perception_oru implementation. Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

NDT for one cell

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

NDT for one cell

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

NDT for one cell i=n

1X pi n i=1   M = p1 − µ . . . pn − µ , 1 Σ= MMT n−1 µ=

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

NDT for one cell i=n

1X pi n i=1   M = p1 − µ . . . pn − µ , 1 Σ= MMT n−1 µ=

Minimise fp2d (Θ) = −

|M| X j=1

exp

1 2

T(mj , Θ) − µm


T

Σ−1 m

T(mj , Θ) − µm Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms




Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

NDT for one cell i=n

1X pi n i=1   M = p1 − µ . . . pn − µ , 1 Σ= MMT n−1 µ=

Minimise fp2d (Θ) = −

|M| X j=1

exp

1 2

T(mj , Θ) − µm


T

Σ−1 m

T(mj , Θ) − µm Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms




Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

Biber and Straßer 2003

Compact representation

∼300 000

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

∼4 000

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

Modelling outliers 0.4

5 p(x) p (x) –

0.35

4

0.3 0.25

3

0.2 2

0.15 0.1

1

0.05 0

-log(p(x)) – -log(p (x))

0 -4

-2

0 x

2

4

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

-4

-2

0 x

2

4

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT scan registration Outline: Create NDT map of one scan. Compute likelihood of having recorded other scan at pose T, given map (sum up log-likelihood for all points). 3 Find T that maximises log-likelihood (Newton search). 1 2

Issues: Discontinuities at cell edges. Cell size trade-off.

Extensions: Multi-resolution matching. Interpolated NDTs. Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

D2D-NDT scan registration

Outline: 1 2 3

Create NDT map of both scans. Compute likelihood of one, given the other. Find (local) max, using Newton and analytical derivative expressions

Fast, and good convergence in structured environments.

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Point-to-point ICP error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Point-to-point ICP error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Point-to-point ICP error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Point-to-point ICP error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Point-to-point ICP error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Point-to-point ICP error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Point-to-point ICP error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

P2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

P2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

P2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

P2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

P2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

P2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

P2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

D2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

D2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

D2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

D2D-NDT error function

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Mapping Kjula

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Saarinen et al. 2013a

NDT-OM

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

NDT-MCL NDT-MCL uses NDT-OM for measurement update.

Figure courtesy of Rafael Valencia

Particle weights are updated using L2 -likelihood for overlapping cells.

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

[Saarinen et al. 2013b; Valencia et al. 2014]

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Saarinen et al. 2013b

NDT-MCL efficiency and accuracy

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

DT-NDT-MCL

Semi-static obstacles are troublesome. Idea: add another timescale. One static and one short-term map. Use both timescales in measurement update. ( X L2 (czi , cms ) if L2 (czi , cms ) >  static map if it fits k i i L2 = L2 (czi , cmti ) otherwise else, short-term map i

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

MUMC works in planar patches cloud domain cascades uncertainty of patches extraction to registration uncertainty robust to dynamics in the scene very good rotation estimates reliable failure detection can be used without odometry

[Pathak et al. 2010a] Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Plane extraction from 3D point clouds

[Pathak et al. 2010b] [Vaskevicius et al. 2010] Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

Input to MUMC

`

ˆi · ` p = ` di n "

Cnˆnˆ Cnˆd C= CTnˆd Cdd k

ˆi , i dk , k Ci i, i = 1 . . . Nk }. P , { kPih kn

Two planar patches clouds with covariances Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

#

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

MUMC Pathak et al. 2010a outline

1

Find all consistent correspondence pairs Prune space using consistency tests Collect nonparallel rotationally 2-consistent pairs Collect parallel translationally 2-consistent pairs

2

Find minimally uncertain maximum consensus Investigate correspondence pairs and build consensus set For each consensus set, estimate transformation with uncertainties using decoupled formulation Take the consensus set with lowest uncertainty volume

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Consistency tests

Patch 1-Consistency

Invariants for patch-pairs from scan to scan

size consistency color consistency Pathak et al. 2012 etc...

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

Angle between normals of non-parallel planes - Rotation 2 consistent Distance between planes for parallel planes - Translation 2 consistent

Home

Algorithms

Methodology

Results

References

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Minimally Uncertain Max Consensus Given 2 scans, two non-parallel correspondence pairs are enough to determine rotation uniquely. Given 2 scans, three non-parallel correspondences are enough to determine translation uniquely. Random Sample Consensus (RANSAC) like consensus-sets can be built, and for each set, the Planar Procrustes Problem is solved to get the least-squares rotation and translation and their covariances. We found that instead of using the consensus size as the quality of the consensus, it is more robust to take its Planar Procrustes Problem uncertainty volume as a quality metric: α = | `r Ctt | × | `r Cqˇqˇ |+ Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

Home

Algorithms

Methodology

Results

References

ICP

Diversity constraint and failure detection

Plane-diversity is needed for reliable translation estimation DC-ON: not enough diversity declare failure DC-OFF: fill-in translation component in unreliable direction with zero, odometry or another estimate

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

Example of planar map - Bremen downtown

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Bremen flythrough

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Underwater example

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

ICP

NDT

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

ICP

NDT

Dynamic and less structured environments

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

P2D-NDT

D2D-NDT

NDT examples

MUMC

DC

MUMC examples

Home

Algorithms

Methodology

Results

References

Data sets

Protocol

Pomerleau et al.

Data sets

ETH

Apartment

Stairs

Gazebo

Wood

Plain

“Challenging data sets for point cloud registration algorithms” [Pomerleau et al. 2012] “Comparing ICP Variants on Real-World Data Sets” [Pomerleau et al. 2013] Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

Home

Algorithms

Methodology

Results

References

Data sets

Protocol

Pose offsets

“Easy” σ = 10 cm, 10◦ Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

“Hard” σ = 100 cm, 45◦

Home

Algorithms

Methodology

Results

References

Data sets

Protocol

Overlap ratios

“Large” >75% Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

“Small” 75%

0.6 0.4 0.2

0 0.2 0.4 0.6 0.8 Error (m) P2Plane ICP

30%–75%

1

0.4 Apartment Stairs ETH Gazebo Wood Plain

0.2

0 0

0.6

0 0

0.2 0.4 0.6 0.8

1

0

0.2 0.4 0.6 0.8

Error (m) D2D-NDT

Error (m) P2D-NDT

10%–80%

45%–75%

Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

1

Home

Algorithms

Methodology

Results

References

Local matching

Global matching

Complete table

Timing

1

1

0.8

0.8

0.8

0.6 0.4 0.2

Probability

1

Probability

Probability

Translation errors: Small overlap only 75%)

0.2 0.4 0.6 0.8 Error (m) P2D-NDT

1

0.4 100% 93% 100% 83% 69% 23%

0.2

0 0

0.6

0 0

0.2 0.4 0.6 0.8 Error (m) MUMC DC-ON

1

0

0.2 0.4 0.6 0.8 Error (m) MUMC DC-OFF

NDT: 2 495 data points, with odo; MUMC: 13 data points, without odo. Magnusson et al. Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms

1

Home

Algorithms

Methodology

Results

References

Local matching

Global matching

Complete table

Timing

1

1

0.8

0.8

0.8

0.6 0.4 Apartment Stairs ETH Gazebo Wood Plain

0.2

0.6 0.4 50% 40% 10% 0% 0% 0%

0.2

0

Probability

1

Probability

Probability

Translation errors: Small overlap only (