What is scan matching Andrea Censi , La Sapienza Universit y of - - PowerPoint PPT Presentation

▶
what is scan matching
SMART_READER_LITE
LIVE PREVIEW

What is scan matching Andrea Censi , La Sapienza Universit y of - - PowerPoint PPT Presentation

Sep 24, 2023 500 likes •882 views

San mathing in a p robabilisti framew o rk Andrea Censi a master student in ontrol engineering at Universit degli Studi di Roma La Sapienza www.dis.uniroma1.it/ acensi andrea.censi@dis.uniroma1.it Andrea Censi ,

slide-1
SLIDE 1 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
1/25 S an mat hing in a p robabilisti framew
  • rk
Andrea Censi a master student in
  • ntrol
engineering at Universit degli Studi di Roma La Sapienza

www.dis.uniroma1.it/∼acensi andrea.censi@dis.uniroma1.it

slide-2
SLIDE 2 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
2/25

What is scan matching

slide-3
SLIDE 3 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
2/25

What is scan matching

Geometric interpretation:

Find a rotation ϕ and a translation t whi h maximize the
  • verlapping
  • f
t w
  • sets
  • f
2D-data.
slide-4
SLIDE 4 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
2/25

What is scan matching

Geometric interpretation:

Find a rotation ϕ and a translation t whi h maximize the
  • verlapping
  • f
t w
  • sets
  • f
2D-data.

Probabilistic interpretation:

Find an app ro ximation to the p robabilit y distribution

p(t, ϕ|xt−1, ut, zt, zt−1) x

: rob
  • t
p
  • se, z
: senso r reading, u :
  • dometry
.
slide-5
SLIDE 5 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
3/25

Contribution of the paper

  • gpm
is a new algo rithm that
  • uses,
soundly , an a rbitra ry evolution mo del; no random sampling required Gaussian assumption: [Minguez&al.’05]; Random sampling: MCL, [Silver&al.’04]
  • ha
ra terizes the un ertaint y analyti ally , also in under onstrained situations Sample error function around the estimate: [Bengtsson&al.’03]; analytic, elegant but

bounded estimate of covariance: [Pfister&al.’02]

  • not
iterative: result do es not dep end
  • n
rst guess W eak p
  • ints
  • f
gpm:
  • The
environment must have some regula rit y to estimate surfa es'
  • rientation.
  • It
is mo re p re ise than i p, id , but not than last generation i p-lik e metho
  • ds. [Minguez&al.’06]
slide-6
SLIDE 6 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
4/25

GPM overview

It is a dual
  • f
Monte Ca rlo Lo alization:
  • In
m l, pa rti les a re dra wn from the evolution mo del and w eighted b y the
  • bservation
mo del.
  • In
gpm, pa rti les a re generated (deterministi ally) from the
  • bservation
mo del and w eighted b y the evolution mo del. Summa ry
  • f
the algo rithm: 1. Extra t
  • rientation
info rmation from the senso r data. 2. Generate a loud
  • f
pa rti les from the
  • bservations.
3. W eight ea h pa rti le a o rding to the evolution mo del. 4. T urn the pa rti les into
  • nstraints
to ha ra terize un ertaint y .
slide-7
SLIDE 7 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
5/25

Extracting the orientation

The input data a re t w
  • sets
  • f
  • riented
p
  • ints {(pi, αi, )}
, where pi is the a rtesian p
  • int
and αi is the dire tion
  • f
the no rmal to the surfa e. Currently using linea r regression; there a re many alternatives.
slide-8
SLIDE 8 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
6/25

Generating the particles

W e reate a set
  • f
hyp
  • theses
(pa rti les) b y
  • nsidering
all p
  • ssible
pairs
  • f
p
  • ints
(no correspondence heuristics ).

pj = Rϕpi + t αj = αi + ϕ

Invert to
  • btain

ˆ ϕ = αj − αi ˆ t = pj − R ˆ

ϕpi

Ea h hyp
  • thesis ( ˆ

ϕ,ˆ t)

is treated as a pa rti le (generated deterministically ; no random sampling here).
slide-9
SLIDE 9 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
7/25

Example (1)

This is ho w the set
  • f
pa rti les lo
  • ks
lik e:

(green is one of the sensor scans; particles are red)

  • 8
  • 6
  • 4
  • 2

2 4 6 8 10

  • 15
  • 10
  • 5

5 10 15 20

W e
  • nsider
  • nly
the pa rti les in a xed ball where the evolution mo del is non-zero.
slide-10
SLIDE 10 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
8/25

Example (2)

P a rti les with |ϕ| ≤ 20◦ , |t| ≤ 20cm .
  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

⇒

it is a pa rti le app ro ximation to p(ϕ, t|xt−1, zt, zt−1) (little a rro ws rep resent ˆ

ϕ

)
slide-11
SLIDE 11 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
9/25

Using the evolution model

W eight b y evolution mo del:

wk = p(ϕk, tk|xt−1, ut)

  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

b efo re w eighting
  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

after w eighting

⇒

no w a pa rti le app ro ximation to p(ϕ, t|xt−1, ut, zt, zt−1) .
slide-12
SLIDE 12 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
10/25

Least squares formulation

T
  • ha
ra terize the un ertaint y
  • f
the pa rti les, w e
  • nsider
the info rmation useful
  • nly
along the dire tion
  • f
the w all. The result is a set
  • f
  • nstraints:
a least squa res p roblem.
slide-13
SLIDE 13 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
11/25

Example (3)

F rom pa rti les . . .
  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

slide-14
SLIDE 14 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
11/25

Example (3)

. . .

to
  • nstraints . . .
  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

slide-15
SLIDE 15 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
11/25

Example (3)

. . .

to
  • va
rian e.
  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

slide-16
SLIDE 16 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
12/25

The need for tuning

Exp erimentally , the estimated
  • va
rian e is signi ant
  • nly
up to a
  • nstant
(go
  • d
shap e, bad a rea).

−10 −8 −6 −4 −2 2 4 6 −6 −4 −2 2 4 6 x (mm) y (mm) Residual error on x,y GPM m=2.57 ||bias|| = 1.5mm sqrt(mse) = 4.2mm GPM sample Σ GPM Σ

T w
  • reasons
fo r this:
  • un ertaint
y should b e mo deled b etter
  • all
pa rti les
  • nsidered
indep endent (instead, the global
  • va
rian e matrix is not diagonal)
slide-17
SLIDE 17 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
13/25

Unconstrained situations

slide-18
SLIDE 18 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
13/25

Unconstrained situations

slide-19
SLIDE 19 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
14/25

Mine example

A rob
  • t
in a mine
  • thanks
to Dirk Haehnel and the CMU group fo r the data les. Senso r data gpm result
slide-20
SLIDE 20 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
15/25

Eigenvalues of estimated covariance

Green and red a re the eigenvalues
  • f
the estimated
  • va
rian e matrix.
  • One
eigenvalue is alw a ys small (left and right w alls).
  • The
  • ther
is big at b eginning
  • f
  • rrido
rs (1,2,. . . ) then de reases.
  • O lusions
a re not fatal and dete ted.
slide-21
SLIDE 21 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
15/25

Eigenvalues of estimated covariance

Green and red a re the eigenvalues
  • f
the estimated
  • va
rian e matrix.
  • One
eigenvalue is alw a ys small (left and right w alls).
  • The
  • ther
is big at b eginning
  • f
  • rrido
rs (1,2,. . . ) then de reases.
  • O lusions
a re not fatal and dete ted.
slide-22
SLIDE 22 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
15/25

Eigenvalues of estimated covariance

Green and red a re the eigenvalues
  • f
the estimated
  • va
rian e matrix.
  • One
eigenvalue is alw a ys small (left and right w alls).
  • The
  • ther
is big at b eginning
  • f
  • rrido
rs (1,2,. . . ) then de reases.
  • O lusions
a re not fatal and dete ted.
slide-23
SLIDE 23 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
15/25

Eigenvalues of estimated covariance

Green and red a re the eigenvalues
  • f
the estimated
  • va
rian e matrix.
  • One
eigenvalue is alw a ys small (left and right w alls).
  • The
  • ther
is big at b eginning
  • f
  • rrido
rs (1,2,. . . ) then de reases.
  • O lusions
a re not fatal and dete ted.
slide-24
SLIDE 24 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
15/25

Eigenvalues of estimated covariance

Green and red a re the eigenvalues
  • f
the estimated
  • va
rian e matrix.
  • One
eigenvalue is alw a ys small (left and right w alls).
  • The
  • ther
is big at b eginning
  • f
  • rrido
rs (1,2,. . . ) then de reases.
  • O lusions
a re not fatal and dete ted.
slide-25
SLIDE 25 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
15/25

Eigenvalues of estimated covariance

Green and red a re the eigenvalues
  • f
the estimated
  • va
rian e matrix.
  • One
eigenvalue is alw a ys small (left and right w alls).
  • The
  • ther
is big at b eginning
  • f
  • rrido
rs (1,2,. . . ) then de reases.
  • O lusions
a re not fatal and dete ted.
slide-26
SLIDE 26 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
15/25

Eigenvalues of estimated covariance

Green and red a re the eigenvalues
  • f
the estimated
  • va
rian e matrix.
  • One
eigenvalue is alw a ys small (left and right w alls).
  • The
  • ther
is big at b eginning
  • f
  • rrido
rs (1,2,. . . ) then de reases.
  • O lusions
a re not fatal and dete ted.
slide-27
SLIDE 27 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
16/25

Comparison with MbICP, IDC, ICP

Cited from [Minguez&al. IEEE T-RO'06℄. Real-w
  • rld
data; ea h s an is mat hed against itself; sea r h spa e is (0.4m, 0.4m, 90◦) . erro rs (m) Mbi p id i p gpm

< 0.001 80.3% 74.9% 52.2% 58.8% (0.001, 0.005) 18.8% 16.5% 42.0% 28.5% (0.005, 0.01) 0.3% 7.1% (0.01, 0.05) 0.8% 0.01% 5.3% > 0.05 0.7% 7.3% 5.8%

  • gpm
do es not have very la rge erro rs; erro r fo r ϕ is if s ans a re equal.
  • Mbi p
is mo re p re ise when it
  • nverges.
  • Probably
[Pster&al.'02℄, [Bib er&al.'03℄ w
  • uld
have results simila r to Mbi p.
slide-28
SLIDE 28 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
17/25

Conclusion and future work

  • gpm's
strong p
  • ints:
uses, soundly , an a rbitra ry evolution mo del (also multimo dal) ha ra terizes the un ertaint y analyti ally , also in under onstrained situations not iterative: result do es not dep end
  • n
rst guess
  • gpm's
w eak p
  • ints:
It is not usable in totally unstru tured environments. Iterative metho ds a re mo re p re ise when they
  • nverge
nea r the right solution.
  • gpm's
future w
  • rk:
Exploit the multimo dalit y
  • f
the pa rti le distribution. T ry some interp
  • lation
s hema to
  • mp
ensate fo r the spa rseness
  • f
the senso r data.
slide-29
SLIDE 29 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
18/25

GPM performance

Squa re environment, 4m × 4m . Random sampling
  • f
p
  • ses,
unifo rm

400mm, 20◦

.

|biasxy| √

msexy

|biasϕ| √

mseϕ 360 ra ys

0.6mm 11.1mm < 0◦ 0.10◦

180 ra ys

2.4mm 11.4mm 0.01◦ 0.13◦

90 ra ys

4.5mm 27.4mm 0.09◦ 0.40◦

45 ra ys

12.3mm 36.4mm 0.08◦ 0.59◦

slide-30
SLIDE 30 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
19/25

Fast correspondence search

W e an mak e gpm faster b y exploiting the radial
  • rdering
  • f
the s ans and sea r hing fo r a b
  • und
fo r ∆θ .

first scan second scan θ θ (θi, ρi) ∆θi ∆θi

slide-31
SLIDE 31 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
20/25

Fast correspondence search

Intuitively , the maximum va riation
  • urs
when the p
  • int
is (in either
  • rder)
translated b y |t|max p erp endi ula r to pi , then rotated b y |ϕ|max . Therefo re

∆θi = tan−1 |ϕ|max ρi

  • + |ϕ|max
slide-32
SLIDE 32 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
21/25

LSE formulation

W e derived

pj = Rϕpi + t ⇒ ˆ t = pj − R ˆ

ϕpi

T
  • nsider
the info rmation
  • nly
along dire tion αk = αi multiply b
  • th
sides b y the verso r (cos αk sin αk) whi h w e abb reviate as v(αk) .

v(αk)tˆ t = v(αk)t(pj − R ˆ

ϕpi) := yk

No w the set
  • f
hyp
  • theses
is a set
  • f
  • nstraints:

v(αk)tt = yk + m/wk · ǫ

where m is a tuning
  • nstant.
slide-33
SLIDE 33 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
22/25

LSE

Lt = Y + R · ǫ L =

  • v(α1) · · · v(αk) · · · v(αK)

t Y = (y1 . . . yk . . . yK)t R = m · diag{1/w1, . . . , 1/wk . . . 1/wK}

Bew a re
  • f
the assumptions that will lead to a diagonal noise
  • va
rian e matrix:
  • ea h
  • nstraint
is indep endent (instead, mo re than
  • ne
  • nstraint
a re generated b y the same reading)
  • the wk
do not have a p robabilisti interp retation
slide-34
SLIDE 34 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
23/25

LSE solution

The LSE solution is

t = (LtR−1L)−1LtR−1Y

W e must invert:
  • the
R
  • va
rian e matrix (assumed diagonal)
  • a 2 × 2
matrix C = (LtR−1L)−1 (invertible if L is full rank). The solution is

C = m(

  • k

[wkv(αk)v(αk)t])−1 t =

  • k

[wkv(αk)v(αk)t] −1

k

[wkykv(αk)]

The hoi e
  • f
a tuning
  • nstant m
do es not bias the estimate
  • f ϕ
.
slide-35
SLIDE 35 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
24/25

Improved covariance

The
  • va
rian e rep resents the un ertaint y b etter.
  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

  • 0.2
  • 0.15
  • 0.1
  • 0.05

0.05 0.1 0.15 0.2

  • It. covariance
  • Con. covariance
slide-36
SLIDE 36 Andrea Censi , “La Sapienza” Universit y
  • f
Rome S an mat hing in a p robabilisti framew
  • rk
  • p.
25/25

GPM vs HSM

hsm is a s an mat her p resented b y Censi, Grisetti, Io hi at ICRA'05 (pap er and sour e
  • de
  • n
my w ebsite).

−0.5 0.5 −0.6 −0.4 −0.2 0.2 0.4 Data points theta θ rho ρ Hough Transform for first set 45 90 135 180 225 270 315 360 −1.125 −0.75 −0.375 0.375 0.75 1.125 1.5 theta θ rho ρ Hough Transform for second set 45 90 135 180 225 270 315 360 −1.125 −0.75 −0.375 0.375 0.75 1.125 1.5 45 90 135 180 225 270 315 360 0.5 1 theta θ Hough spectra 45 90 135 180 225 270 315 360 0.5 1 theta θ Spectra cross−correlation

HSM’s pros:

  • hsm
do es global sea r hes.
  • hsm
is
  • rre t
and
  • mplete
fo r exa t input.
  • hsm
do es not need
  • rientation
info rmation.

HSM’s cons:

  • hsm
uses a ross- o rrelation
  • p
erato r: time is quadrati in resolution.
  • hsm
do es not ha ra terize un ertaint y .