I
AE
S In
t
er
na
t
io
na
l J
o
urna
l o
f
Ro
bo
t
ics a
nd
Aut
o
m
a
t
io
n
(
I
J
RA)
Vo
l.
14
,
No
.
3
,
Sep
tem
b
er
20
25
,
p
p
.
4
3
8
~
4
5
0
I
SS
N:
2722
-
2
5
8
6
,
DOI
:
1
0
.
1
1
5
9
1
/i
jr
a
.
v
14
i
3
.
pp
4
3
8
-
450
438
J
o
ur
na
l ho
m
ep
a
g
e
:
h
ttp
:
//ij
r
a
.
ia
esco
r
e.
co
m
Ro
bo
t
G
a
uss
ia
n
-
histo
rica
l
relo
ca
li
za
tion
:
iner
tial
m
ea
surem
ent
unit
-
LiD
AR
li
keli
ho
o
d
field
ma
t
chi
ng
Ye
-
M
ing
Sh
e
n
1
,
M
i
n
K
a
ng
1
,
J
ia
-
Q
ia
ng
Ya
ng
1
,
2
,
Z
ho
ng
-
H
o
u
Ca
i
2
1
S
c
h
o
o
l
of
A
u
t
o
ma
t
i
o
n
a
n
d
E
l
e
c
t
r
i
c
a
l
En
g
i
n
e
e
r
i
n
g
,
Z
h
e
j
i
a
n
g
U
n
i
v
e
r
s
i
t
y
of
S
c
i
e
n
c
e
a
n
d
T
e
c
h
n
o
l
o
g
y
,
H
a
n
g
z
h
o
u
,
Z
h
e
j
i
a
n
g
,
C
h
i
n
a
2
S
c
h
o
o
l
of
E
l
e
c
t
r
i
c
a
l
E
n
g
i
n
e
e
r
i
n
g
,
Z
h
e
j
i
a
n
g
U
n
i
v
e
r
s
i
t
y
,
H
a
n
g
z
h
o
u
,
Zh
e
j
i
a
n
g
,
C
h
i
n
a
Art
icle
I
nfo
AB
S
T
RAC
T
A
r
ticle
his
to
r
y:
R
ec
eiv
ed
J
an
20,
2
0
2
5
R
ev
is
ed
J
u
l
19,
2
0
2
5
Acc
ep
ted
Au
g
2
6
,
2
0
2
5
Ro
b
o
t
l
o
c
a
li
z
a
ti
o
n
is
a
fo
u
n
d
a
ti
o
n
a
l
tec
h
n
o
lo
g
y
fo
r
a
u
to
n
o
m
o
u
s
n
a
v
ig
a
ti
o
n
,
e
n
a
b
li
n
g
tas
k
e
x
e
c
u
t
io
n
a
n
d
a
d
a
p
tatio
n
to
d
y
n
a
m
ic
e
n
v
ir
o
n
m
e
n
ts.
Ho
we
v
e
r,
fa
il
u
re
to
re
tu
rn
to
th
e
c
o
rre
c
t
p
o
se
a
fter
p
o
we
r
lo
ss
or
su
d
d
e
n
d
is
p
lac
e
m
e
n
t
(th
e
“
k
id
n
a
p
p
in
g
”
p
r
o
b
lem
)
can
lea
d
to
c
rit
ica
l
sy
ste
m
fa
il
u
re
s.
Ex
isti
n
g
m
e
th
o
d
s
o
fte
n
su
ffe
r
fro
m
slo
w
r
e
lo
c
a
li
z
a
ti
o
n
,
h
ig
h
c
o
m
p
u
tatio
n
a
l
c
o
st,
a
n
d
p
o
o
r
r
o
b
u
stn
e
ss
to
d
y
n
a
m
ic
o
b
sta
c
les
.
We
p
r
o
p
o
se
a
n
o
v
e
l
in
e
rti
a
l
m
e
a
su
re
m
e
n
t
u
n
it
(
IM
U
)
-
Li
DA
R
fu
sio
n
re
l
o
c
a
li
z
a
ti
o
n
fra
m
e
wo
rk
b
a
se
d
on
G
a
u
ss
ian
h
isto
rica
l
c
o
n
stra
in
ts
a
n
d
a
d
a
p
ti
v
e
li
k
e
li
h
o
o
d
field
m
a
tch
in
g
.
By
in
c
o
rp
o
ra
ti
n
g
I
M
U
-
d
e
riv
e
d
y
a
w
c
o
n
stra
in
ts
a
n
d
m
o
d
e
li
n
g
h
isto
r
i
c
a
l
p
o
se
s
with
in
a
3σ
G
a
u
ss
ian
re
g
io
n
,
o
u
r
m
e
th
o
d
e
ffe
c
ti
v
e
ly
n
a
rro
ws
t
h
e
Li
DA
R
se
a
rc
h
sp
a
c
e
.
Cu
rv
a
tu
re
a
n
d
n
o
r
m
a
l
v
e
c
to
r
-
b
a
se
d
fe
a
tu
re
e
x
trac
ti
o
n
re
d
u
c
e
s
p
o
i
n
t
c
lo
u
d
v
o
l
u
m
e
by
50
–
7
0
%
,
wh
il
e
d
y
n
a
m
ic
o
b
sta
c
le
fil
terin
g
v
ia
m
u
lt
i
-
fra
m
e
d
iffere
n
c
in
g
a
n
d
n
e
ig
h
b
o
r
h
o
o
d
v
a
li
d
a
ti
o
n
e
n
h
a
n
c
e
s
r
o
b
u
st
n
e
ss
.
An
a
d
a
p
ti
v
e
sp
iral
se
a
rc
h
stra
teg
y
f
u
rth
e
r
re
fi
n
e
s
p
o
se
e
stim
a
ti
o
n
.
C
o
m
p
a
re
d
to
ORB
-
S
LAM
3
a
n
d
a
d
a
p
ti
v
e
M
o
n
te
Ca
rlo
lo
c
a
li
z
a
ti
o
n
(AMCL
),
o
u
r
m
e
th
o
d
m
a
in
tain
s
c
o
m
p
a
ra
b
le
a
c
c
u
ra
c
y
wh
il
e
sig
n
ifi
c
a
n
tl
y
re
d
u
c
in
g
re
lo
c
a
li
z
a
ti
o
n
ti
m
e
a
n
d
CP
U
u
sa
g
e
.
Ex
p
e
rime
n
t
a
l
re
su
lt
s
sh
o
w
a
re
lo
c
a
li
z
a
ti
o
n
su
c
c
e
ss
r
a
te
of
8
4
%
,
a
v
e
ra
g
e
ti
m
e
of
1
.
6
8
se
c
o
n
d
s,
a
n
d
CP
U
u
sa
g
e
of
3
8
.
4
%
,
d
e
m
o
n
stra
ti
n
g
h
ig
h
e
fficie
n
c
y
a
n
d
ro
b
u
st
n
e
ss
in
d
y
n
a
m
ic
e
n
v
ir
o
n
m
e
n
ts.
K
ey
w
o
r
d
s
:
Ad
ap
tiv
e
lik
elih
o
o
d
f
ield
L
iDAR
-
in
er
tial
m
ea
s
u
r
em
en
t
u
n
it
f
u
s
io
n
L
o
ca
lizatio
n
lo
s
s
es
R
elo
ca
lizatio
n
R
OS
r
o
b
o
ts
T
h
is
is
an
o
p
e
n
a
c
c
e
ss
a
rticle
u
n
d
e
r
th
e
CC
BY
-
SA
li
c
e
n
se
.
C
o
r
r
e
s
p
o
nd
ing
A
uth
o
r
:
Min
Kan
g
Sch
o
o
l
of
A
u
to
m
atio
n
a
n
d
E
le
ctr
ical
E
n
g
in
ee
r
in
g
,
Z
h
ejian
g
Un
iv
er
s
ity
of
Scien
ce
an
d
T
ec
h
n
o
lo
g
y
Han
g
zh
o
u
,
Z
h
ejian
g
,
C
h
in
a
E
m
ail:
k
an
g
m
in
@
zu
s
t.e
d
u
.
c
n
1.
I
NT
RO
D
UCT
I
O
N
A
k
ey
p
r
er
eq
u
is
ite
f
o
r
m
o
b
il
e
r
o
b
o
ts
to
ac
co
m
p
lis
h
au
to
n
o
m
o
u
s
n
a
v
ig
atio
n
is
p
o
s
itio
n
i
n
g
,
wh
ich
en
tails
f
ig
u
r
in
g
o
u
t
th
e
r
o
b
o
t
’
s
p
o
s
tu
r
e
in
its
o
p
er
atio
n
al
en
v
ir
o
n
m
en
t.
T
ask
s
s
u
ch
as
m
ap
p
in
g
,
o
b
s
tacle
av
o
id
an
ce
,
ar
ea
co
v
er
a
g
e,
a
n
d
tar
g
et
tr
ac
k
in
g
r
ely
on
ac
c
u
r
a
te
lo
ca
lizatio
n
.
Ho
wev
e
r
,
one
m
ajo
r
ch
allen
g
e
is
th
e
“
r
o
b
o
t
k
id
n
ap
p
in
g
”
p
r
o
b
le
m
[
1
]
,
wh
er
e
a
r
o
b
o
t
is
s
u
d
d
e
n
ly
r
elo
ca
ted
to
an
u
n
k
n
o
wn
p
o
s
itio
n
with
o
u
t
an
y
p
r
io
r
i
n
f
o
r
m
atio
n
,
ca
u
s
in
g
a
l
o
s
s
of
lo
ca
lizatio
n
.
To
en
s
u
r
e
th
e
s
af
e
an
d
ef
f
ec
tiv
e
o
p
er
atio
n
of
r
o
b
o
ts
in
r
ea
l
-
wo
r
ld
ap
p
licatio
n
s
s
u
ch
as
in
d
u
s
tr
ial
au
to
m
atio
n
,
s
er
v
ice
r
o
b
o
tics
,
an
d
au
to
n
o
m
o
u
s
d
r
iv
in
g
,
it
is
cr
u
cial
to
ad
d
r
ess
th
e
r
elo
ca
lizatio
n
p
r
o
b
lem
af
ter
th
e
r
o
b
o
t
lo
s
es
its
p
o
s
itio
n
in
g
[
2
]
.
Par
ticle
f
ilter
in
g
a
n
d
v
is
u
al
s
im
u
ltan
eo
u
s
l
o
ca
lizatio
n
a
n
d
m
ap
p
in
g
(
Vis
u
al
SLAM
)
a
r
e
t
wo
p
o
p
u
la
r
m
eth
o
d
s
in
r
o
b
o
t
r
elo
ca
lizatio
n
.
Vis
u
al
SLAM
u
s
es
ca
m
er
a
im
ag
e
d
ata
to
l
o
ca
te
r
o
b
o
ts
an
d
cr
ea
te
m
ap
s
,
a
n
d
is
s
tr
o
n
g
ly
af
f
ec
ted
by
ch
an
g
e
s
in
lig
h
tin
g
[
3
]
–
[
5
]
.
Par
ticle
f
ilter
in
g
u
s
es
a
g
r
o
u
p
of
p
ar
ti
cles
to
esti
m
ate
th
e
r
o
b
o
t
’
s
s
tate,
but
it
is
co
m
p
u
tatio
n
ally
d
em
an
d
i
n
g
,
p
ar
ticu
l
ar
ly
in
lar
g
e
-
s
ca
le
en
v
ir
o
n
m
e
n
ts
[
6
]
–
[
1
0
]
.
T
h
ese
lim
itatio
n
s
,
s
u
ch
as
Vis
u
al
SLAM
’
s
s
en
s
it
iv
ity
to
illu
m
in
atio
n
ch
an
g
es
an
d
p
ar
ti
cle
f
ilter
in
g
’
s
h
ig
h
co
m
p
u
tatio
n
al
c
o
s
ts
,
u
n
d
er
s
co
r
e
th
e
n
ee
d
f
o
r
more
r
eliab
le
a
n
d
ef
f
ec
tiv
e
r
elo
ca
lizatio
n
tec
h
n
iq
u
es.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
AE
S
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
o
b
o
t G
a
u
s
s
ia
n
-
h
is
to
r
ica
l relo
ca
liz
a
tio
n
:
in
erti
a
l m
ea
s
u
r
eme
n
t u
n
it
-
LiDAR
… (
Ye
-
Min
g
S
h
en
)
439
At
p
r
ese
n
t
,
m
a
n
y
s
ch
o
l
ar
s
h
a
v
e
r
ese
ar
c
h
e
d
r
e
lo
ca
l
iz
ati
o
n
a
n
d
ac
h
ie
v
ed
f
r
u
it
f
u
l
r
esu
lts
.
F
o
r
e
x
a
m
p
le
,
Hu
et
a
l.
[
1
1
]
p
r
o
p
o
s
ed
a
f
u
s
i
o
n
p
o
s
iti
o
n
i
n
g
m
e
th
o
d
c
o
m
b
i
n
i
n
g
Wi
-
Fi
,
L
iD
AR
,
a
n
d
m
a
p
s
b
ase
d
on
Ka
lm
an
f
ilt
e
r
i
n
g
.
T
h
is
m
et
h
o
d
ad
d
r
es
s
es
tw
o
is
s
u
es
:
t
h
e
p
o
o
r
p
e
r
f
o
r
m
a
n
ce
of
s
i
n
g
le
Wi
-
Fi
p
o
s
iti
o
n
in
g
in
in
d
o
o
r
en
v
i
r
o
n
m
e
n
ts
an
d
i
n
co
r
r
e
ct
L
iDAR
p
o
s
it
i
o
n
in
g
d
u
e
to
i
n
e
f
f
ec
t
iv
e
f
ea
tu
r
e
e
x
tr
ac
ti
o
n
in
s
p
e
ci
f
ic
s
ce
n
a
r
i
o
s
.
Ho
w
ev
er
,
it
r
eq
u
i
r
es
s
u
b
s
t
an
t
ial
c
o
m
p
u
tati
o
n
a
l
r
eso
u
r
c
es.
W
an
g
et
a
l
.
[
1
2
]
p
r
o
p
o
s
e
d
a
g
l
o
b
al
p
o
s
it
io
n
i
n
g
m
et
h
o
d
b
ase
d
on
th
e
p
r
i
n
ci
p
les
of
l
ase
r
o
d
o
m
et
r
y
a
n
d
i
m
p
r
o
v
e
d
a
d
a
p
t
iv
e
M
o
n
te
C
a
r
lo
.
T
h
e
y
u
s
e
d
l
ase
r
o
d
o
m
e
tr
y
b
as
ed
on
th
e
p
o
i
n
t
-
to
-
li
n
e
ite
r
a
ti
v
e
cl
o
s
est
p
o
i
n
t
(
P
L
-
I
C
P)
m
et
h
o
d
to
r
ep
la
ce
t
h
e
t
r
a
d
it
io
n
al
o
d
o
m
e
tr
y
an
d
i
n
t
r
o
d
u
ce
d
t
h
e
i
d
ea
of
d
e
o
x
y
r
i
b
o
n
u
cle
ic
a
ci
d
(
DNA
)
c
r
o
s
s
o
v
er
an
d
m
u
ta
ti
o
n
in
g
e
n
et
ics
i
n
t
o
t
h
e
p
a
r
t
icl
e
ite
r
at
io
n
p
r
o
ce
s
s
of
AM
C
L
.
Ho
w
ev
er
,
th
e
i
te
r
at
iv
e
cl
o
s
es
t
p
o
i
n
t
(
I
C
P
)
o
b
tai
n
s
th
e
o
p
tim
al
ap
p
r
o
x
i
m
a
te
p
o
s
iti
o
n
s
o
l
u
t
io
n
t
h
r
o
u
g
h
i
te
r
at
io
n
.
I
ts
h
ea
v
y
ca
l
cu
lat
io
n
o
f
t
e
n
ca
u
s
es
t
h
e
r
o
b
o
t
’
s
s
tat
e
to
c
h
an
g
e
b
e
f
o
r
e
g
e
tti
n
g
th
e
p
o
s
i
ti
o
n
,
r
es
u
lt
in
g
in
a
l
ar
g
e
c
u
m
u
l
ati
v
e
e
r
r
o
r
.
L
a
n
et
a
l
.
[
1
3
]
p
r
o
p
o
s
e
d
n
a
r
r
o
w
f
iel
d
of
v
iew
(
NFOV
)
er
r
o
r
r
e
co
g
n
it
io
n
to
d
et
e
r
m
i
n
e
t
h
e
s
t
at
u
s
of
b
as
e
s
ta
ti
o
n
s
th
r
o
u
g
h
t
h
e
s
li
d
i
n
g
wi
n
d
o
w
te
ch
n
i
q
u
e
a
n
d
s
t
an
d
a
r
d
d
e
v
i
ati
o
n
th
r
esh
o
l
d
,
eli
m
i
n
a
ti
n
g
ab
n
o
r
m
al
d
at
a.
G
r
a
p
h
o
p
t
i
m
iz
ati
o
n
f
u
s
i
o
n
p
o
s
i
ti
o
n
in
g
c
o
m
b
i
n
es
u
l
tr
a
-
wi
d
e
b
a
n
d
(
UW
B
)
m
ea
s
u
r
e
m
e
n
t
v
alu
es
a
n
d
o
d
o
m
et
er
i
n
f
o
r
m
a
ti
o
n
to
o
b
t
ai
n
ac
cu
r
ate
r
o
b
o
t
t
r
aj
ec
t
o
r
i
es
th
r
o
u
g
h
o
p
ti
m
iz
ati
o
n
al
g
o
r
i
th
m
s
.
UW
B
s
ig
n
a
ls
m
a
y
be
af
f
e
cte
d
by
en
v
i
r
o
n
m
e
n
t
al
i
n
te
r
f
e
r
e
n
c
e,
a
f
f
e
cti
n
g
t
h
e
r
el
ia
b
il
it
y
of
m
e
as
u
r
em
en
t
r
esu
lts
.
W
an
g
et
a
l
.
[
1
]
s
u
g
g
este
d
a
r
es
id
u
al
n
etw
o
r
k
(
R
esN
et
)
-
b
ase
d
r
o
b
o
t
r
el
o
ca
liz
ati
o
n
tec
h
n
i
q
u
e
t
h
at
c
o
m
b
i
n
e
d
c
o
a
r
s
e
a
n
d
f
i
n
e
m
atc
h
i
n
g
to
g
r
ea
t
ly
in
c
r
ea
s
e
p
o
s
it
io
n
i
n
g
s
u
c
ce
s
s
r
ate
a
n
d
e
f
f
ic
ie
n
c
y
.
E
v
en
t
h
o
u
g
h
cu
r
r
e
n
t
a
p
p
r
o
a
c
h
es
h
av
e
s
o
m
e
b
en
ef
its
,
t
h
e
y
s
till
h
a
v
e
d
r
a
wb
ac
k
s
li
k
e
a
s
lo
w
r
el
o
c
ali
za
ti
o
n
r
es
p
o
n
s
e
a
n
d
h
i
g
h
c
o
m
p
u
t
ati
o
n
al
c
o
m
p
l
ex
it
y
.
C
o
n
v
e
n
ti
o
n
al
a
p
p
r
o
ac
h
es
s
h
o
w
li
m
ite
d
a
cc
u
r
ac
y
in
d
y
n
a
m
ic
o
b
s
ta
cle
d
e
tec
ti
o
n
u
n
d
e
r
c
h
a
lle
n
g
in
g
c
o
n
d
it
io
n
s
s
u
c
h
as
lig
h
t
in
g
v
a
r
ia
ti
o
n
s
an
d
o
b
je
c
t
o
c
cl
u
s
i
o
n
,
w
h
i
ch
p
r
e
v
e
n
ts
t
h
e
m
f
r
o
m
m
e
eti
n
g
o
p
e
r
a
ti
o
n
al
r
eq
u
i
r
e
m
e
n
ts
in
c
o
m
p
le
x
en
v
i
r
o
n
m
en
ts
.
M
o
r
e
o
v
er
,
t
h
ey
o
f
te
n
f
a
il
to
ef
f
e
cti
v
e
ly
in
te
g
r
at
e
h
is
t
o
r
ica
l
p
o
s
e
d
at
a
a
n
d
i
n
e
r
t
ial
m
e
as
u
r
e
m
e
n
t
u
n
it
(
I
MU
)
m
ea
s
u
r
e
m
e
n
ts
to
e
n
a
b
le
r
o
b
u
s
t
r
el
o
ca
li
za
ti
o
n
in
d
y
n
a
m
ic
s
c
e
n
ar
i
o
s
,
r
es
u
l
ti
n
g
in
r
ed
u
ce
d
ad
a
p
ta
b
i
lit
y
a
n
d
s
l
o
w
r
ec
o
v
e
r
y
af
t
er
d
is
r
u
p
ti
o
n
s
.
In
o
r
d
er
to
o
v
er
c
o
m
e
th
ese
lim
itatio
n
s
,
th
is
s
tu
d
y
p
r
o
p
o
s
es
a
n
o
v
el
L
iDAR
-
I
MU
f
u
s
io
n
-
b
ased
r
elo
ca
lizatio
n
tech
n
iq
u
e.
T
h
e
m
eth
o
d
f
u
s
es
I
MU
y
aw
an
g
les
with
h
is
to
r
ical
p
o
s
e
d
ata
to
ef
f
ec
tiv
ely
co
m
p
r
ess
th
e
s
ea
r
ch
s
p
ac
e,
an
d
it
u
tili
ze
s
cu
r
v
atu
r
e
a
n
d
n
o
r
m
al
v
e
cto
r
an
aly
s
is
f
o
r
f
ea
tu
r
e
e
x
tr
ac
tio
n
,
s
ig
n
if
ican
tly
r
ed
u
cin
g
d
ata
v
o
lu
m
e
wh
ile
p
r
eser
v
in
g
m
o
s
t
cr
itical
en
v
ir
o
n
m
en
tal
in
f
o
r
m
atio
n
.
Fu
r
th
er
m
o
r
e,
we
p
r
o
p
o
s
e
a
f
r
a
m
e
-
d
i
f
f
e
r
e
n
c
e
-
b
a
s
e
d
d
y
n
a
m
i
c
o
b
s
ta
c
l
e
d
e
t
e
ct
i
o
n
a
p
p
r
o
a
c
h
f
o
r
a
c
c
u
r
a
t
e
d
y
n
a
m
i
c
e
n
v
i
r
o
n
m
e
n
t
d
i
s
c
r
i
m
i
n
a
ti
o
n
,
alo
n
g
with
an
a
d
ap
tiv
e
lik
e
lih
o
o
d
f
ield
m
atch
in
g
al
g
o
r
i
th
m
th
at
d
y
n
am
ically
o
p
tim
i
ze
s
co
m
p
u
tatio
n
al
r
eso
u
r
ce
allo
ca
tio
n
ac
c
o
r
d
in
g
to
e
n
v
ir
o
n
m
en
tal
c
o
m
p
lex
ity
f
o
r
L
iDAR
-
to
-
m
ap
m
atch
in
g
.
T
h
e
ex
p
e
r
im
en
tal
r
e
s
u
l
ts
d
e
m
o
n
s
t
r
a
t
e
t
h
at
t
h
e
p
r
o
p
o
s
e
d
m
e
t
h
o
d
s
u
b
s
t
a
n
t
i
al
l
y
im
p
r
o
v
e
s
s
y
s
t
e
m
r
e
s
p
o
n
s
i
v
e
n
e
s
s
a
n
d
e
n
v
i
r
o
n
m
e
n
t
a
l
ad
ap
tab
ilit
y
,
p
r
o
v
id
in
g
an
ef
f
e
ctiv
e
s
o
lu
tio
n
f
o
r
r
o
b
o
t
r
elo
ca
l
izatio
n
in
co
m
p
lex
d
y
n
am
ic
s
c
en
ar
io
s
.
2.
RE
L
O
C
AL
I
Z
A
T
I
O
N
AP
P
R
O
ACH
T
h
e
r
elo
ca
lizatio
n
im
p
lem
en
t
atio
n
b
lo
ck
d
iag
r
am
is
s
h
o
wn
in
Fig
u
r
e
1
.
It
m
ain
ly
co
n
s
is
ts
of
a
d
ata
in
p
u
t
lay
er
,
p
r
o
ce
s
s
in
g
m
o
d
u
l
e,
an
d
o
u
tp
u
t
lay
e
r
.
T
h
e
d
ata
i
n
p
u
t
lay
er
i
n
clu
d
es
d
ata
s
o
u
r
c
es
s
u
ch
as
h
is
to
r
ical
co
o
r
d
in
ates,
g
r
id
m
ap
s
,
I
MU
,
an
d
L
iDAR
,
r
esp
o
n
s
ib
le
f
o
r
co
llectin
g
v
ar
io
u
s
r
aw
d
ata
to
p
r
o
v
id
e
b
asic
in
f
o
r
m
atio
n
f
o
r
th
e
s
y
s
tem
.
T
h
e
p
r
o
ce
s
s
in
g
m
o
d
u
le
is
th
e
co
r
e
p
ar
t
of
t
h
e
r
elo
ca
lizatio
n
im
p
lem
en
tatio
n
,
it
p
er
f
o
r
m
s
an
aly
tical
p
r
o
ce
s
s
in
g
an
d
d
ata
f
u
s
io
n
on
th
e
in
p
u
t
s
ig
n
als
to
d
eter
m
in
e
th
e
o
p
tim
al
r
o
b
o
t
p
o
s
e
esti
m
atio
n
.
T
h
e
o
u
t
p
u
t
lay
er
t
h
en
p
r
o
d
u
ce
s
o
p
tim
al
p
o
s
e
esti
m
atio
n
b
ased
on
th
e
p
r
o
ce
s
s
in
g
m
o
d
u
le
’
s
r
esu
lts
,
d
eter
m
in
in
g
t
h
e
r
o
b
o
t
’
s
p
o
s
itio
n
an
d
o
r
ien
tatio
n
.
2
.
1
.
M
ulti
-
s
o
urce
info
rm
a
t
io
n
f
us
io
n
co
m
bin
ing
T
h
e
m
u
lti
-
s
o
u
r
ce
in
f
o
r
m
atio
n
f
u
s
io
n
m
o
d
u
le
d
ee
p
ly
in
t
eg
r
ates
h
is
to
r
ical
p
o
s
e
d
ata,
g
r
id
m
ap
in
f
o
r
m
atio
n
,
a
n
d
f
ea
tu
r
e
-
e
x
tr
ac
ted
L
iDAR
an
d
I
MU
m
ea
s
u
r
em
en
ts
.
T
h
e
h
is
to
r
ical
p
o
s
es
an
d
g
r
i
d
m
a
p
p
r
o
v
id
e
m
ac
r
o
s
co
p
ic
p
o
s
itio
n
an
d
en
v
ir
o
n
m
e
n
tal
in
f
o
r
m
atio
n
,
o
f
f
er
i
n
g
a
g
lo
b
al
r
e
f
er
en
ce
f
r
am
ewo
r
k
.
Me
an
wh
ile,
th
e
f
u
s
ed
L
iDAR
an
d
I
MU
d
ata
co
n
tr
i
b
u
te
p
r
ec
i
s
e
lo
ca
l
en
v
ir
o
n
m
en
tal
f
ea
t
u
r
e
s
an
d
m
o
tio
n
s
tate
in
f
o
r
m
atio
n
.
T
h
is
co
m
p
r
eh
en
s
iv
e
in
teg
r
atio
n
en
h
an
ce
s
th
e
r
o
b
u
s
tn
ess
an
d
a
cc
u
r
ac
y
of
th
e
r
elo
ca
lizatio
n
p
r
o
ce
s
s
in
co
m
p
le
x
d
y
n
am
ic
e
n
v
ir
o
n
m
en
ts
.
2
.
1
.
1
.
G
a
us
s
ia
n
3σ
co
ns
t
ra
int
m
o
delin
g
f
o
r
his
t
o
rica
l
po
s
es
Data
co
llectio
n
in
v
o
l
v
es
o
b
t
ain
in
g
th
e
p
o
s
e
d
ata
of
th
e
latest
10
s
u
cc
ess
f
u
l
lo
ca
lizatio
n
s
f
r
o
m
th
e
r
o
b
o
t
lo
ca
lizatio
n
s
y
s
tem
.
E
ac
h
p
o
s
e
is
ty
p
ically
r
ep
r
ese
n
ted
as
a
v
ec
to
r
=
[
,
,
]
,
wh
er
e
x
an
d
y
ar
e
th
e
r
o
b
o
t
’
s
two
-
d
im
en
s
io
n
al
p
o
s
itio
n
co
o
r
d
in
ates
an
d
is
its
y
aw
an
g
le.
Den
o
te
th
ese
10
p
o
s
es
as
1
,
2
,
⋯
,
10
.
To
ca
lcu
late
th
e
m
ea
n
v
ec
to
r
,
g
iv
en
=
10
d
ata
p
o
in
ts
,
f
o
r
th
e
tr
an
s
latio
n
p
ar
t
(x
an
d
y)
an
d
r
o
tatio
n
p
ar
t
(
),
we
u
s
e
=
1
∑
=
1
.
In
th
e
two
-
d
im
e
n
s
io
n
al
tr
an
s
latio
n
an
d
o
n
e
-
d
im
e
n
s
io
n
al
r
o
ta
tio
n
ca
s
e,
=
[
,
,
]
with
=
1
∑
=
1
,
=
1
∑
=
1
,
an
d
=
1
∑
=
1
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
7
2
2
-
2
5
8
6
I
AE
S
I
n
t
J
R
o
b
&
A
u
to
m
,
Vo
l
.
14
,
No
.
3
,
Sep
tem
b
er
20
25
:
4
3
8
-
4
5
0
440
Fig
u
r
e
1
.
B
lo
ck
d
iag
r
am
of
th
e
r
elo
ca
lizatio
n
im
p
lem
e
n
tatio
n
T
h
e
co
v
ar
ian
ce
m
atr
ix
,
a
3
×
3
m
atr
ix
f
o
r
p
o
s
e
d
ata,
d
escr
ib
es
d
ata
d
is
tr
ib
u
tio
n
an
d
in
ter
-
d
im
en
s
io
n
al
c
o
r
r
elatio
n
s
.
I
ts
elem
en
ts
ar
e
ca
lcu
lated
as
=
1
−
1
∑
(
−
)
(
−
)
=
1
f
o
r
,
∈
{
1
,
2
,
3
}
co
r
r
esp
o
n
d
in
g
to
x,
y,
a
n
d
.
E
x
p
an
d
e
d
,
th
e
c
o
v
ar
ian
ce
m
atr
i
x
is
g
iv
en
by
(
1
)
,
=
[
2
2
2
]
(
1
)
wh
er
e
2
an
d
2
ar
e
x
an
d
y
v
a
r
ian
ce
s
,
2
is
th
e
v
ar
ian
ce
,
an
d
=
,
=
,
=
ar
e
th
e
r
esp
ec
tiv
e
co
v
ar
i
an
ce
s
.
T
h
e
co
v
ar
ian
ce
m
atr
ix
is
d
y
n
am
ically
ad
ju
s
ted
to
en
s
u
r
e
2
=
(
2
,
2
)
≤
0
.
25
2
f
o
r
tr
an
s
latio
n
v
a
r
ian
ce
an
d
2
=
2
≤
0
.
09
2
f
o
r
r
o
tatio
n
v
ar
ian
ce
,
with
ad
ju
s
tm
en
ts
2
=
(
2
,
0
.
25
)
,
2
=
(
2
,
0
.
25
)
,
2
=
(
2
,
0
.
09
)
.
In
a
Gau
s
s
ian
d
is
tr
ib
u
tio
n
,
th
e
3σ
r
an
g
e
,
co
v
er
in
g
a
b
o
u
t
9
9
.
7
%
of
th
e
d
a
ta,
d
ef
in
es
th
e
ef
f
ec
tiv
e
s
ea
r
c
h
r
eg
io
n
.
Fo
r
two
-
d
im
en
s
io
n
al
tr
a
n
s
latio
n
(x
a
n
d
y
)
,
it
’
s
an
ellip
s
e
o
b
tain
ed
f
r
o
m
th
e
c
o
v
ar
ian
ce
m
atr
ix
’
s
eig
en
v
al
u
e
d
ec
o
m
p
o
s
itio
n
,
an
d
f
o
r
one
-
d
im
en
s
io
n
al
r
o
tatio
n
(
),
it
is
th
e
in
ter
v
al
[
−
3
√
2
,
+
3
√
2
]
.
W
h
en
a
n
ew
p
o
s
e
=
[
,
,
]
is
o
b
s
er
v
ed
,
th
e
M
ah
alan
o
b
is
d
is
tan
ce
is
ca
lcu
l
ated
to
m
ea
s
u
r
e
its
d
ev
iatio
n
f
r
o
m
th
e
h
is
to
r
ical
Gau
s
s
ian
m
o
d
el.
T
h
e
f
o
r
m
u
la
f
o
r
th
e
Ma
h
alan
o
b
is
d
is
tan
ce
is
g
iv
en
by
(
2
)
.
=
√
(
−
)
−
1
(
−
)
(
2
)
T
h
e
s
y
s
tem
in
itiates
th
e
r
elo
ca
lizatio
n
p
r
o
ce
s
s
if
th
e
Ma
h
alan
o
b
is
d
is
tan
ce
s
u
r
p
ass
es
a
p
r
ed
eter
m
in
ed
th
r
esh
o
ld
T,
s
ig
n
i
f
y
in
g
a
n
o
ta
b
le
d
ep
ar
tu
r
e
f
r
o
m
th
e
h
is
to
r
ical
d
is
tr
ib
u
tio
n
.
2
.
1
.
2
.
IMU
pre
-
inte
g
ra
t
io
n
f
o
r
re
lo
ca
liza
t
io
n
W
h
en
u
s
in
g
in
f
o
r
m
atio
n
f
r
o
m
an
I
MU
f
o
r
lo
ca
lizatio
n
or
r
elo
ca
lizatio
n
,
d
ir
ec
tly
i
n
teg
r
atin
g
r
aw
ac
ce
ler
o
m
eter
a
n
d
g
y
r
o
s
co
p
e
d
ata
o
f
te
n
s
u
f
f
er
s
f
r
o
m
h
ig
h
-
f
r
eq
u
en
c
y
n
o
is
e
an
d
ac
cu
m
u
lat
ed
d
r
if
t.
Mo
r
eo
v
e
r
,
s
in
ce
th
e
s
am
p
lin
g
f
r
eq
u
en
c
y
of
th
e
I
MU
is
ty
p
ically
m
u
ch
h
ig
h
er
th
an
th
at
of
L
iDAR
,
r
ep
ea
ted
ly
in
teg
r
atin
g
I
MU
d
ata
f
r
o
m
t
h
e
in
itial
m
o
m
en
t
to
th
e
cu
r
r
en
t
f
r
am
e
in
c
u
r
s
co
n
s
id
er
ab
le
c
o
m
p
u
tatio
n
al
co
s
t.
To
ad
d
r
ess
th
ese
is
s
u
es,
th
e
p
r
e
-
in
teg
r
atio
n
m
eth
o
d
h
as
b
ee
n
p
r
o
p
o
s
ed
to
ef
f
icien
tly
a
g
g
r
e
g
ate
h
ig
h
-
f
r
eq
u
en
cy
I
MU
d
at
a
o
v
er
th
e
lo
wer
-
f
r
e
q
u
en
c
y
in
te
r
v
al
b
etwe
en
two
co
n
s
ec
u
tiv
e
k
ey
f
r
am
es,
th
er
eb
y
p
r
o
v
id
in
g
a
s
tab
le
r
elativ
e
m
o
tio
n
co
n
s
tr
ain
t.
T
h
e
I
MU
p
r
e
-
in
teg
r
atio
n
m
eth
o
d
was
f
ir
s
t
f
o
r
m
alize
d
by
Fo
r
s
ter
et
a
l.
[
1
4
]
,
en
a
b
lin
g
ef
f
icien
t
in
c
o
r
p
o
r
atio
n
of
h
ig
h
-
f
r
eq
u
e
n
cy
in
er
tial
d
ata
in
to
o
p
tim
izatio
n
-
b
ased
esti
m
atio
n
f
r
am
ewo
r
k
s
with
o
u
t
r
ed
u
n
d
an
t
re
-
i
n
teg
r
atio
n
.
T
h
e
co
r
e
id
ea
of
I
MU
p
r
e
-
i
n
teg
r
atio
n
is
to
in
teg
r
ate
th
e
an
g
u
lar
v
elo
city
an
d
lin
ea
r
ac
ce
le
r
atio
n
o
v
er
th
e
tim
e
in
ter
v
al
b
etwe
en
two
f
r
am
es
to
o
b
tain
a
r
elativ
e
p
o
s
e
in
cr
em
en
t.
T
h
is
ap
p
r
o
ac
h
a
v
o
id
s
r
ed
u
n
d
an
t
re
-
in
teg
r
atio
n
f
r
o
m
th
e
in
itial
s
ta
te
d
u
r
in
g
each
o
p
tim
izatio
n
.
In
ad
d
itio
n
,
to
ac
co
m
m
o
d
ate
t
h
e
u
p
d
ates
of
s
tate
v
ar
iab
les
(
e.
g
.
,
o
r
ien
tatio
n
)
d
u
r
in
g
th
e
n
o
n
lin
ea
r
o
p
tim
izat
io
n
p
r
o
ce
s
s
,
th
e
J
ac
o
b
ian
s
of
th
e
p
r
e
-
in
te
g
r
ated
q
u
an
titi
es
with
r
esp
ec
t
to
th
e
i
n
itial
s
tate
s
ar
e
al
s
o
co
m
p
u
ted
.
T
h
is
f
ac
ilit
ate
s
th
e
in
co
r
p
o
r
at
io
n
of
I
MU
f
ac
to
r
s
in
to
o
p
tim
izatio
n
-
b
ased
f
r
a
m
e
wo
r
k
s
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
AE
S
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
o
b
o
t G
a
u
s
s
ia
n
-
h
is
to
r
ica
l relo
ca
liz
a
tio
n
:
in
erti
a
l m
ea
s
u
r
eme
n
t u
n
it
-
LiDAR
… (
Ye
-
Min
g
S
h
en
)
441
T
h
e
r
esu
ltin
g
p
r
e
-
in
te
g
r
ated
q
u
an
titi
es
in
clu
d
e
th
e
r
elativ
e
r
o
tatio
n
i
n
cr
em
en
t
ij
,
th
e
r
e
lativ
e
v
elo
city
in
cr
em
en
t
ij
,
a
n
d
th
e
r
elativ
e
p
o
s
itio
n
in
cr
em
en
t
ij
.
A
f
ter
r
em
o
v
i
n
g
th
e
ef
f
ec
ts
of
g
r
av
ity
an
d
s
en
s
o
r
b
ias,
th
e
I
MU
p
r
e
-
in
teg
r
atio
n
can
be
ap
p
r
o
x
im
ate
d
by
(
3
)
to
(
5
)
.
≈
∏
(
(
−
)
)
−
1
=
(
3
)
≈
∑
(
−
)
−
1
=
(
4
)
≈
∑
[
+
1
2
(
−
)
2
]
−
1
=
(
5
)
H
er
e
,
an
d
d
en
o
te
th
e
g
y
r
o
s
c
o
p
e
a
n
d
ac
ce
ler
o
m
eter
m
ea
s
u
r
em
en
ts
at
tim
e
,
r
esp
ec
tiv
ely
,
an
d
r
ep
r
esen
t
th
e
g
y
r
o
s
co
p
e
an
d
a
cc
eler
o
m
eter
b
iases
,
an
d
is
th
e
r
o
tatio
n
m
atr
i
x
at
tim
e
.
In
s
itu
atio
n
s
s
u
ch
as
r
o
b
o
t
k
id
n
ap
p
in
g
,
wh
er
e
th
e
r
o
b
o
t
’
s
p
r
i
o
r
p
o
s
e
esti
m
ate
b
ec
o
m
es
in
v
alid
,
I
MU
p
r
e
-
in
teg
r
atio
n
can
p
r
o
v
id
e
a
r
elativ
ely
s
tab
le
m
o
tio
n
p
r
io
r
to
ass
is
t
L
iDA
R
in
p
o
s
e
in
itializatio
n
.
T
h
is
en
ab
les
r
ap
id
r
elo
c
aliza
tio
n
.
Fu
r
th
er
m
o
r
e,
th
e
h
ig
h
-
f
r
eq
u
en
c
y
n
atu
r
e
of
I
MU
m
ea
s
u
r
e
m
en
t
s
allo
ws
th
e
s
y
s
tem
to
co
n
tin
u
o
u
s
ly
ca
p
tu
r
e
o
r
ien
t
atio
n
ch
an
g
es
o
v
e
r
s
h
o
r
t
tim
e
in
ter
v
als,
th
er
eb
y
im
p
r
o
v
i
n
g
it
s
r
esp
o
n
s
iv
en
ess
to
ab
r
u
p
t
m
o
tio
n
.
2
.
1
.
3
.
Dy
na
m
ic
pro
ce
s
s
ing
mo
du
le
W
ith
its
d
ata
s
er
v
in
g
as
th
e
f
o
u
n
d
atio
n
of
th
e
ad
a
p
tiv
e
lik
elih
o
o
d
f
ield
f
o
r
ass
ess
in
g
p
o
s
itio
n
p
r
o
b
a
b
ilit
y
d
is
tr
ib
u
tio
n
s
,
L
iD
AR
is
ess
en
tial
f
o
r
lo
ca
lizatio
n
an
d
en
v
ir
o
n
m
en
tal
p
er
ce
p
ti
o
n
.
H
o
wev
er
,
th
er
e
ar
e
two
m
ain
is
s
u
es
with
L
iDAR
d
ata.
First,
ev
en
th
o
u
g
h
2D
p
o
in
t
cl
o
u
d
d
ata
is
r
ic
h
,
d
y
n
am
ic
o
b
s
tacle
s
in
ter
f
er
en
ce
a
n
d
s
en
s
o
r
lim
it
atio
n
s
in
tr
o
d
u
ce
m
ea
s
u
r
em
en
t
er
r
o
r
s
.
Seco
n
d
,
th
e
m
ass
iv
e
am
o
u
n
t
of
L
iDAR
d
ata
r
aises
s
to
r
ag
e
r
eq
u
ir
em
e
n
ts
an
d
d
ec
r
ea
s
es
co
m
p
u
tatio
n
a
l
ef
f
icien
cy
.
T
h
e
d
y
n
am
ic
p
r
o
ce
s
s
in
g
m
o
d
u
le
is
m
ain
ly
r
esp
o
n
s
ib
le
f
o
r
p
r
o
ce
s
s
in
g
L
iDAR
d
ata.
Dy
n
am
ic
o
b
jects,
s
u
ch
as
m
o
v
i
n
g
v
e
h
icles
an
d
p
ed
estrian
s
,
ar
e
co
m
m
o
n
in
r
ea
l
-
wo
r
ld
ap
p
licatio
n
s
.
Acc
u
r
ate
p
o
s
itio
n
in
g
m
ay
be
co
m
p
r
o
m
i
s
ed
by
d
y
n
am
ic
o
b
ject
in
ter
f
e
r
en
ce
in
L
iDAR
p
o
in
t
clo
u
d
s
[
1
5
]
.
To
co
u
n
ter
ac
t
th
is
in
ter
f
er
en
ce
,
t
h
e
d
y
n
am
i
c
p
r
o
ce
s
s
in
g
m
o
d
u
le
p
r
o
ce
s
s
es
r
ad
ar
d
ata
u
s
in
g
a
m
u
lti
-
f
r
am
e
d
if
f
er
en
tial
d
etec
tio
n
tech
n
iq
u
e.
L
et
th
e
cu
r
r
e
n
t
f
r
am
e
be
th
e
c
-
th
f
r
am
e,
with
its
L
iDAR
s
ca
n
d
is
tan
ce
d
ata
s
eq
u
en
ce
r
ep
r
esen
ted
as
=
[
1
,
2
,
⋯
,
]
,
wh
er
e
N
d
en
o
tes
th
e
n
u
m
b
er
of
L
iDAR
s
ca
n
p
o
in
ts
.
Fo
r
M
h
is
to
r
ical
f
r
am
es,
th
e
d
is
tan
ce
d
ata
s
eq
u
en
ce
of
th
e
m
-
th
h
is
to
r
ical
f
r
am
e
is
ℎ
=
[
ℎ
1
,
ℎ
2
,
⋯
,
ℎ
]
,
wh
er
e
=
1
,
2
,
⋯
,
.
Fo
r
th
e
i
-
th
s
ca
n
p
o
in
t,
th
e
i
n
ter
-
f
r
am
e
d
if
f
er
e
n
ce
b
etwe
en
th
e
cu
r
r
e
n
t
f
r
am
e
an
d
th
e
m
-
th
h
is
to
r
ical
f
r
am
e
is
ca
lcu
lated
as
(
6
)
.
=
|
−
ℎ
|
(
6
)
T
h
e
n
eig
h
b
o
r
h
o
o
d
p
o
in
t
s
et
f
o
r
th
e
i
-
th
s
ca
n
p
o
in
t
is
d
e
f
in
ed
as
.
Fo
r
each
n
eig
h
b
o
r
in
g
p
o
in
t
∈
,
we
em
p
lo
y
an
in
d
icato
r
f
u
n
ctio
n
(
>
)
to
ev
alu
ate
wh
eth
er
th
e
i
n
ter
-
f
r
a
m
e
d
if
f
er
en
ce
ex
ce
ed
s
th
r
esh
o
ld
T.
T
h
e
f
u
n
c
tio
n
r
etu
r
n
s
1
wh
en
>
,
an
d
0
o
t
h
er
wis
e.
If
∑
∈
(
>
)
≥
1
,
th
e
i
-
th
s
ca
n
p
o
in
t
is
p
r
elim
in
ar
ily
id
en
tifie
d
as
p
o
te
n
tially
d
y
n
am
ic.
Ho
wev
er
,
a
s
p
ec
ial
ca
s
e
r
eq
u
ir
es
co
n
s
id
er
atio
n
b
e
f
o
r
e
f
in
al
d
eter
m
in
atio
n
.
In
r
ea
l
-
wo
r
l
d
s
ce
n
ar
io
s
,
wh
en
a
L
iDAR
b
ea
m
s
witch
es
f
r
o
m
a
d
y
n
am
ic
o
b
s
tacle
to
a
s
tatic
b
ac
k
g
r
o
u
n
d
due
to
o
b
ject
m
o
v
em
en
t,
th
e
p
o
in
t
m
ay
f
alsel
y
tr
ig
g
er
d
y
n
am
ic
d
etec
tio
n
.
To
r
eso
lv
e
th
is
,
we
an
aly
ze
b
o
th
d
is
tan
ce
ch
an
g
e
p
atter
n
s
(
s
u
d
d
en
in
cr
ea
s
e
f
o
llo
wed
by
s
tab
ilizatio
n
)
an
d
s
ca
n
an
g
les
to
d
is
tin
g
u
is
h
tr
u
e
d
y
n
a
m
ic
p
o
in
ts
f
r
o
m
r
ev
ea
led
s
tatic
f
ea
tu
r
e
s
.
T
h
is
ap
p
r
o
ac
h
p
r
ev
e
n
ts
m
is
class
if
icatio
n
wh
en
∑
∈
(
>
)
≥
1
o
cc
u
r
s
d
u
e
to
o
b
s
tacle
d
is
p
lace
m
en
t
r
ath
e
r
th
an
ac
tu
al
d
y
n
am
ics,
s
u
b
s
tan
tially
r
ed
u
cin
g
f
alse
p
o
s
itiv
es
in
th
e
d
etec
tio
n
s
y
s
tem
.
2
.
1
.
4
.
F
ea
t
ure
e
x
t
ra
ct
i
o
n
m
o
du
le
Featu
r
e
ex
tr
ac
tio
n
tech
n
iq
u
es
ad
d
r
ess
th
e
p
r
o
b
lem
of
m
a
s
s
iv
e
L
iDAR
d
ata
by
id
en
tif
y
in
g
k
ey
f
ea
tu
r
e
p
o
i
n
ts
an
d
s
h
ap
e
i
n
f
o
r
m
atio
n
,
s
ig
n
if
ican
tly
r
e
d
u
cin
g
d
ata
v
o
lu
m
e
w
h
ile
r
etain
in
g
cr
itical
en
v
ir
o
n
m
en
tal
d
etails.
T
h
is
ap
p
r
o
ac
h
ac
ce
le
r
ates
p
r
o
ce
s
s
in
g
an
d
im
p
r
o
v
es
an
aly
s
is
ac
cu
r
ac
y
[
1
6
]
.
Fin
d
in
g
ju
m
p
p
o
in
ts
—
p
o
in
ts
in
th
e
L
iDAR
d
ata
wh
er
e
th
e
d
is
tan
ce
b
etwe
en
ad
jace
n
t
m
ea
s
u
r
em
en
ts
s
u
b
s
tan
tially
d
ev
iates
f
r
o
m
th
e
ex
p
ec
ted
r
an
g
e
,
in
d
icatin
g
p
o
ten
tial
o
b
ject
b
o
u
n
d
ar
ies
or
d
ata
an
o
m
alies
[
1
7
]
.
th
e
p
o
in
t
clo
u
d
s
ar
e
s
eg
m
e
n
ted
u
s
in
g
th
e
p
r
o
p
e
r
th
r
esh
o
ld
s
an
d
r
u
les,
an
d
E
ac
h
g
r
o
u
p
of
s
eg
m
en
ted
p
o
in
ts
is
r
eg
ar
d
e
d
as
a
lo
ca
l
n
ei
g
h
b
o
r
h
o
o
d
.
Fo
r
m
u
la
(
4
)
y
ield
s
th
e
co
v
ar
ian
ce
m
atr
ix
of
th
e
lo
ca
l
n
eig
h
b
o
r
h
o
o
d
,
ass
u
m
in
g
th
at
each
lo
ca
l
n
eig
h
b
o
r
h
o
o
d
c
o
n
tain
s
k
p
o
in
t
clo
u
d
s
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
7
2
2
-
2
5
8
6
I
AE
S
I
n
t
J
R
o
b
&
A
u
to
m
,
Vo
l
.
14
,
No
.
3
,
Sep
tem
b
er
20
25
:
4
3
8
-
4
5
0
442
=
1
∑
(
−
̄
)
=
1
(
−
̄
)
(
7
)
Am
o
n
g
th
em
,
is
th
e
p
o
in
t
in
th
e
n
eig
h
b
o
r
h
o
o
d
,
an
d
̄
is
th
e
ce
n
tr
o
id
of
th
e
n
eig
h
b
o
r
h
o
o
d
.
T
h
en
,
p
er
f
o
r
m
eig
en
v
alu
e
d
ec
o
m
p
o
s
itio
n
on
th
e
co
v
a
r
ian
ce
m
atr
ix
C
to
o
b
tain
th
r
ee
eig
en
v
alu
es
1
,
2
,
an
d
3
.
T
h
e
cu
r
v
atu
r
e
is
ca
lcu
lated
by
th
e
r
atio
of
th
e
m
in
im
u
m
eig
en
v
al
u
e
to
th
e
s
u
m
of
t
h
e
eig
en
v
al
u
es,
as
s
h
o
wn
in
(
5
)
.
Po
in
ts
with
lar
g
er
cu
r
v
atu
r
e
u
s
u
ally
co
r
r
esp
o
n
d
to
co
r
n
er
p
o
i
n
ts
or
ed
g
es
in
t
h
e
en
v
i
r
o
n
m
e
n
t.
=
(
1
,
2
,
3
)
1
+
2
+
3
(
8
)
T
h
e
p
ar
am
ete
r
s
of
a
cir
cle
ar
e
f
itted
by
th
e
least
s
q
u
ar
es
m
eth
o
d
[
1
8
]
.
Su
p
p
o
s
e
th
e
eq
u
atio
n
of
a
cir
cle
is
(
−
)
2
+
(
−
)
2
=
2
.
W
h
er
e
(
,
)
ar
e
th
e
co
o
r
d
in
ates
of
th
e
ce
n
ter
of
th
e
ci
r
cle
an
d
is
th
e
r
ad
iu
s
.
Fo
r
th
e
p
o
in
ts
(
,
)
(
r
ep
r
es
en
ts
th
e
g
lo
b
al
co
o
r
d
i
n
ate
s
y
s
tem
co
o
r
d
in
ates
of
th
e
p
o
in
ts
p
r
o
jecte
d
o
n
to
th
e
m
ap
f
r
o
m
th
e
p
o
in
ts
s
ca
n
n
ed
by
t
h
e
laser
)
in
th
e
g
iv
en
p
o
in
t
clo
u
d
d
ata.
Su
b
s
titu
te
th
em
in
to
th
e
eq
u
atio
n
of
th
e
cir
cle
to
c
o
n
s
tr
u
ct
a
s
er
ies
of
eq
u
atio
n
s
a
b
o
u
t
,
an
d
.
T
h
e
n
,
m
in
im
ize
t
h
e
s
u
m
of
t
h
e
s
q
u
ar
ed
d
is
tan
ce
s
f
r
o
m
th
e
p
o
i
n
ts
to
th
e
cir
cle,
th
at
is
,
s
o
lv
e
f
o
r
th
e
v
al
u
es
th
at
m
ak
e
(
6
)
r
ea
ch
its
m
in
im
u
m
.
,
,
∑
[
√
(
−
)
2
+
(
−
)
2
−
2
]
2
(
9
)
To
m
itig
ate
th
e
ex
ce
s
s
iv
e
in
f
lu
en
ce
of
lo
n
g
s
tr
aig
h
t
lin
es
d
u
r
in
g
d
ata
p
r
o
ce
s
s
in
g
,
a
s
t
r
ateg
y
of
wea
k
en
in
g
th
eir
weig
h
t
is
ad
o
p
ted
.
T
h
is
m
eth
o
d
g
u
ar
a
n
tees
th
e
ef
f
icien
t
r
eten
tio
n
of
en
v
ir
o
n
m
en
tal
ch
ar
ac
ter
is
tic
in
f
o
r
m
atio
n
.
T
h
e
p
o
s
itio
n
in
g
alg
o
r
ith
m
can
more
ef
f
ec
tiv
ely
i
d
en
tify
p
o
t
en
tial
p
o
s
itio
n
s
by
u
s
in
g
th
e
p
r
o
ce
s
s
ed
d
ata
f
o
r
p
r
o
b
ab
ilit
y
in
f
er
e
n
ce
,
w
h
ich
im
p
r
o
v
es
th
e
s
y
s
tem
’
s
o
v
er
all
ac
cu
r
ac
y
an
d
r
o
b
u
s
tn
ess
.
2
.
2
.
Ada
ptiv
e
li
k
eliho
o
d
f
ield
m
et
ho
d
T
h
e
b
ea
m
m
o
d
el
is
p
r
o
n
e
to
lo
ca
l
o
p
tim
a
an
d
h
ig
h
co
m
p
u
tatio
n
al
co
s
ts
.
T
h
e
a
d
ap
tiv
e
lik
elih
o
o
d
f
ield
ap
p
r
o
ac
h
o
v
er
co
m
es
t
h
e
b
ea
m
m
o
d
el
’
s
d
r
awb
ac
k
s
,
p
ar
ticu
lar
ly
its
n
o
n
-
s
m
o
o
th
n
ess
in
clu
tter
e
d
en
v
ir
o
n
m
en
ts
[
1
9
]
,
[
2
0
]
.
T
h
e
lik
elih
o
o
d
f
ield
m
o
d
el
by
r
ed
u
cin
g
co
m
p
u
tatio
n
al
c
o
m
p
l
ex
ity
an
d
b
lu
r
r
in
g
o
b
s
tacle
s
p
r
o
v
i
d
es
s
m
o
o
th
e
r
an
d
m
o
r
e
ef
f
icien
t
r
esu
lts
th
an
th
e
b
ea
m
m
o
d
el
[
2
1
]
,
[
2
2
]
.
T
h
e
p
r
o
b
ab
ilit
y
d
is
tr
ib
u
tio
n
of
th
e
lik
elih
o
o
d
f
ield
[
2
3
]
can
be
r
ep
r
esen
ted
by
(
7
)
.
(
|
,
)
=
∏
(
|
,
)
=
1
(
10
)
Am
o
n
g
th
em
,
is
th
e
i
-
th
L
iDAR
m
ea
s
u
r
em
en
t
v
alu
e
at
tim
e,
is
th
e
p
o
s
e
of
t
h
e
r
o
b
o
t
at
ti
m
e,
an
d
is
th
e
m
ap
.
T
h
e
lik
elih
o
o
d
f
ield
m
o
d
el
b
l
u
r
s
th
e
o
b
s
tacl
es
in
th
e
wo
r
k
s
p
ac
e
,
m
a
k
in
g
it
ap
p
licab
le
to
v
ar
io
u
s
s
p
atial
s
itu
atio
n
s
with
s
m
o
o
th
er
an
d
m
o
r
e
ef
f
icien
t
r
esu
lts
.
I
ts
co
r
e
id
ea
is
to
r
e
g
a
r
d
th
e
p
o
in
ts
on
t
h
e
g
r
id
as
f
o
r
m
in
g
a
m
ag
n
etic
f
ie
ld
th
at
attr
ac
ts
th
e
s
u
r
r
o
u
n
d
in
g
p
o
i
n
t
clo
u
d
s
,
an
d
th
e
attr
ac
ti
o
n
d
ec
a
y
s
with
th
e
s
q
u
ar
e
of
th
e
d
is
tan
ce
(
o
r
Gau
s
s
ian
d
ec
ay
can
be
u
s
ed
)
[
2
4
]
,
[
2
5
]
.
T
h
e
en
d
p
o
i
n
ts
o
b
tain
ed
by
L
iDAR
m
ea
s
u
r
em
en
t
in
th
e
g
lo
b
al
co
o
r
d
in
ate
s
y
s
tem
ar
e
p
r
o
je
cted
.
At
tim
e
t,
th
e
p
o
s
tu
r
e
of
th
e
r
o
b
o
t
is
=
(
,
,
)
,
th
e
in
s
tallatio
n
p
o
s
itio
n
of
th
e
L
iDAR
r
elativ
e
to
th
e
c
en
ter
co
o
r
d
i
n
ates
of
th
e
r
o
b
o
t
is
(
,
,
)
,
th
e
a
n
g
le
of
t
h
e
laser
b
ea
m
of
th
e
L
iDAR
r
elativ
e
to
th
e
o
r
ien
tatio
n
of
th
e
r
o
b
o
t
is
,
,
th
e
co
o
r
d
in
ates
of
th
e
laser
-
m
ea
s
u
r
ed
en
d
p
o
in
ts
r
elativ
e
to
t
h
e
ce
n
ter
of
th
e
L
iDAR
is
,
an
d
th
e
c
o
o
r
d
in
ates
of
th
e
p
o
in
ts
s
ca
n
n
ed
by
th
e
laser
p
r
o
jecte
d
o
n
t
o
th
e
g
l
o
b
al
co
o
r
d
in
ate
s
y
s
tem
of
th
e
m
ap
is
(
)
,
as
s
h
o
wn
in
Fig
u
r
e
2
,
th
eir
r
elatio
n
s
h
ip
s
ar
e
d
escr
ib
ed
by
(
8
)
.
T
h
is
tech
n
iq
u
e
o
f
f
e
r
s
a
m
o
r
e
d
ep
en
d
a
b
le
m
eth
o
d
f
o
r
r
o
b
o
t
r
elo
ca
lizatio
n
wh
ile
s
u
cc
ess
f
u
lly
ad
d
r
ess
in
g
th
e
d
r
awb
ac
k
s
of
th
e
b
ea
m
m
o
d
el.
T
h
e
r
o
b
o
t
u
s
es
th
is
f
o
r
m
u
la
to
u
p
d
ate
its
p
o
s
itio
n
esti
m
ate
in
th
e
g
lo
b
al
co
o
r
d
in
ate
s
y
s
tem
b
ased
on
it
s
cu
r
r
en
t
p
o
s
tu
r
e
an
d
s
en
s
o
r
m
ea
s
u
r
em
en
ts
,
ac
h
iev
in
g
p
r
ec
is
e
p
o
s
itio
n
in
g
.
(
)
=
(
)
+
(
−
)
(
,
,
)
+
(
(
+
,
)
(
+
,
)
)
(
11
)
Du
r
in
g
r
elo
ca
lizatio
n
in
itializatio
n
,
th
e
s
y
s
tem
co
n
s
tr
u
cts
a
3σ
-
co
n
s
tr
ain
e
d
s
ea
r
ch
r
e
g
io
n
on
th
e
2D
p
lan
e
by
lev
er
a
g
in
g
r
ea
l
-
tim
e
I
MU
y
aw
an
g
le
m
ea
s
u
r
em
en
t
s
(
).
T
h
is
ap
p
r
o
ac
h
r
e
d
u
ce
s
th
e
L
iDAR
h
ea
d
in
g
s
ea
r
ch
r
an
g
e
to
±
3
0
°
(
an
8
3
.
3
%
r
ed
u
ctio
n
c
o
m
p
a
r
ed
to
f
u
ll
3
6
0
°
s
ea
r
ch
)
wh
ile
i
n
itializin
g
th
e
p
o
s
itio
n
al
s
ea
r
ch
s
p
ac
e
u
s
in
g
h
is
to
r
ical
tr
ajec
to
r
y
v
ar
ia
n
ce
co
n
s
tr
ain
ts
(
2
,
2
≤
0
.
25
2
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
AE
S
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
o
b
o
t G
a
u
s
s
ia
n
-
h
is
to
r
ica
l relo
ca
liz
a
tio
n
:
in
erti
a
l m
ea
s
u
r
eme
n
t u
n
it
-
LiDAR
… (
Ye
-
Min
g
S
h
en
)
443
Fig
u
r
e
2
.
R
o
b
o
t
co
o
r
d
in
ate
tr
a
n
s
f
o
r
m
atio
n
F
o
r
each
c
a
n
d
i
d
a
t
e
p
o
s
e
[
,
,
]
,
t
h
e
s
y
s
t
e
m
p
r
o
c
e
s
s
es
L
i
DAR
d
a
t
a
t
h
r
o
u
g
h
d
y
n
a
m
i
c
o
b
s
t
a
c
le
r
e
m
o
v
a
l
a
n
d
s
ta
t
i
c
f
e
a
t
u
r
e
e
x
t
r
a
c
t
i
o
n
,
t
h
e
n
ca
l
c
u
l
at
e
s
t
h
e
li
k
e
li
h
o
o
d
of
m
a
t
c
h
i
n
g
b
e
t
w
ee
n
e
n
v
i
r
o
n
m
e
n
t
a
l
f
e
a
t
u
r
es
(
e
d
g
e
s
/
p
l
a
n
es
)
a
n
d
p
r
e
-
b
u
i
lt
m
a
p
e
l
e
m
e
n
ts
.
T
h
e
i
t
e
r
at
i
v
e
o
p
t
i
m
i
z
a
ti
o
n
m
a
x
i
m
i
z
es
t
h
e
li
k
e
l
i
h
o
o
d
f
u
n
c
t
i
o
n
(
|
,
)
,
w
h
e
r
e
d
e
n
o
t
e
s
c
u
r
r
e
n
t
o
b
s
e
r
v
a
t
i
o
n
s
,
r
e
p
r
e
s
e
n
ts
c
a
n
d
i
d
a
t
e
p
o
s
e
s
,
a
n
d
is
t
h
e
s
t
at
i
c
m
a
p
.
If
th
e
m
ax
im
u
m
m
atch
in
g
s
co
r
e
with
in
th
e
in
itial
s
ea
r
ch
r
eg
io
n
f
alls
b
elo
w
th
e
p
r
ed
ef
in
e
d
th
r
esh
o
ld
,
th
e
s
y
s
tem
ac
tiv
ates
an
ad
ap
tiv
e
s
p
ir
al
ex
p
an
s
io
n
s
tr
ateg
y
as
s
h
o
wn
in
Fig
u
r
e
3
.
T
ak
i
n
g
th
e
in
itial
s
ea
r
ch
ce
n
ter
as
th
e
o
r
ig
in
,
it
d
y
n
a
m
ically
ad
ju
s
ts
th
e
s
ea
r
ch
s
tep
s
ize
u
s
in
g
=
0
⋅
(
wh
er
e
0
=
0
.
5
is
th
e
in
itial
s
tep
s
ize
an
d
=
0
.
8
is
th
e
d
ec
ay
r
ate)
,
ex
te
n
d
in
g
t
h
e
s
ea
r
ch
ar
ea
in
a
s
p
ir
al
p
atter
n
o
u
twar
d
.
T
h
e
f
ea
tu
r
e
m
atc
h
in
g
an
d
o
p
tim
iz
atio
n
p
r
o
ce
d
u
r
e
is
r
e
p
ea
ted
f
o
llo
win
g
each
ex
p
a
n
s
io
n
u
n
til
a
p
o
s
e
th
at
s
atis
f
ies
th
e
r
eq
u
ir
em
en
ts
is
d
is
co
v
er
ed
.
T
h
e
f
in
al
ca
n
d
id
ate
p
o
s
e
is
v
alid
ated
twice:
f
ir
s
t,
it
is
ex
am
in
ed
f
o
r
v
alid
ity
with
in
th
e
b
o
u
n
d
a
r
ies
of
th
e
m
ap
an
d
f
o
r
th
e
ab
s
en
ce
of
c
o
llis
io
n
s
;
s
ec
o
n
d
,
I
MU
p
r
e
-
in
teg
r
atio
n
is
u
s
ed
to
co
n
f
ir
m
t
h
at
th
e
m
o
tio
n
co
n
s
tr
ain
ts
ar
e
co
n
s
is
ten
t.
Po
s
e
v
alid
atio
n
co
m
p
letes
th
e
r
el
o
c
aliza
tio
n
p
r
o
ce
s
s
by
p
u
b
lis
h
in
g
th
e
p
o
s
e
to
t
h
e
o
u
t
p
u
t
lay
er
.
Fig
u
r
e
3
.
E
x
p
an
s
io
n
m
ap
of
s
e
ar
ch
s
co
p
e
3.
RE
SU
L
T
S
AND
D
I
SCU
SS
I
O
N
T
h
is
s
ec
tio
n
ex
p
lain
s
th
e
r
esu
l
ts
of
r
esear
ch
an
d
at
th
e
s
am
e
tim
e
is
g
iv
en
co
m
p
r
eh
en
s
iv
e
d
is
cu
s
s
io
n
.
R
esu
lts
can
be
p
r
esen
ted
in
f
ig
u
r
es,
g
r
ap
h
s
,
ta
b
les
an
d
o
th
er
s
th
at
m
ak
e
th
e
r
ea
d
er
u
n
d
er
s
tan
d
ea
s
ily
[
1
4
]
,
[
1
5
]
.
T
h
e
d
is
cu
s
s
io
n
can
be
m
ad
e
in
s
ev
er
al
s
u
b
-
s
ec
tio
n
s
.
To
estab
lis
h
th
e
tech
n
ical
f
o
u
n
d
atio
n
f
o
r
s
u
b
s
eq
u
en
t
e
v
alu
a
tio
n
,
we
f
ir
s
t
an
aly
ze
th
e
co
r
e
p
er
ce
p
tio
n
m
o
d
u
le
’
s
p
er
f
o
r
m
an
ce
.
T
h
is
in
itial
v
alid
atio
n
f
o
cu
s
es
on
r
ea
l
-
tim
e
d
y
n
am
ic
o
b
s
tacle
d
etec
tio
n
u
s
in
g
L
iDAR
p
o
in
t
clo
u
d
p
r
o
ce
s
s
in
g
,
an
d
r
o
b
u
s
t
f
ea
tu
r
e
ex
tr
ac
tio
n
f
o
r
en
v
i
r
o
n
m
e
n
tal
ch
ar
ac
ter
iz
atio
n
.
T
h
e
v
er
if
ied
p
er
f
o
r
m
an
ce
of
th
ese
s
u
b
s
y
s
tem
s
d
ir
ec
tly
en
ab
les
th
e
r
elo
ca
lizatio
n
ca
p
ab
ilit
ies
d
em
o
n
s
tr
ated
in
later
b
en
ch
m
ar
k
an
d
f
ield
test
s
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
7
2
2
-
2
5
8
6
I
AE
S
I
n
t
J
R
o
b
&
A
u
to
m
,
Vo
l
.
14
,
No
.
3
,
Sep
tem
b
er
20
25
:
4
3
8
-
4
5
0
444
3
.
1
.
L
iDAR
-
ba
s
ed
dy
na
m
ic
o
bs
t
a
cle
det
ec
t
io
n
a
nd
f
ea
t
u
re
ex
t
ra
ct
i
o
n
T
h
e
s
y
s
tem
is
im
p
lem
en
ted
on
a
r
ea
l
r
o
b
o
tic
p
latf
o
r
m
e
q
u
ip
p
e
d
with
an
R
PLI
DAR
A2
L
iDAR
s
en
s
o
r
(
m
o
d
el:
R
PLI
DAR
A2
,
10
Hz
s
ca
n
n
i
n
g
f
r
eq
u
e
n
cy
,
16
m
m
a
x
im
u
m
r
an
g
e
,
0
.
1
5
m
m
in
im
u
m
r
an
g
e
,
0
.
0
0
3
2
r
ad
an
g
u
lar
r
eso
l
u
tio
n
,
an
d
0
.
0
8
5
8
s
s
ca
n
d
u
r
atio
n
)
,
wh
ich
is
u
s
ed
f
o
r
2D
en
v
ir
o
n
m
en
tal
s
ca
n
n
in
g
.
T
h
e
ex
p
er
im
e
n
tal
s
etu
p
in
clu
d
es
d
y
n
am
ic
h
u
m
an
s
u
b
jects
walk
in
g
at
d
if
f
er
e
n
t
s
p
ee
d
s
.
T
h
e
s
y
s
tem
o
p
er
ates
on
Ub
u
n
tu
2
0
.
0
4
with
th
e
r
o
b
o
t
o
p
er
atin
g
s
y
s
tem
(
R
OS)
,
an
d
th
e
r
esu
ltin
g
2D
s
ca
n
s
ar
e
v
i
s
u
alize
d
in
r
v
iz,
as
illu
s
tr
ated
in
Fig
u
r
e
4
.
Fig
u
r
e
4
(
a)
s
h
o
ws
th
e
p
r
o
c
ess
ed
L
iDA
R
d
ata
af
ter
d
y
n
am
ic
o
b
s
tacle
d
etec
tio
n
an
d
f
ea
tu
r
e
ex
tr
ac
tio
n
,
wh
er
e
th
e
r
ed
p
o
i
n
t
clo
u
d
r
ep
r
esen
ts
th
e
d
etec
ted
d
y
n
am
ic
o
b
s
tacle
s
an
d
th
e
g
r
ee
n
p
o
i
n
t
clo
u
d
co
r
r
esp
o
n
d
s
to
th
e
ex
tr
ac
ted
s
tatic
f
ea
tu
r
es.
C
o
m
p
ar
ed
to
th
e
r
aw
L
iDAR
d
ata
s
h
o
wn
in
Fig
u
r
e
4
(
b
)
,
w
h
ich
is
v
is
u
alize
d
in
y
ello
w,
th
e
p
r
o
ce
s
s
ed
d
ata
is
s
ig
n
if
ican
tly
r
ed
u
ce
d
in
v
o
l
u
m
e.
In
p
ar
ticu
lar
,
th
e
n
u
m
b
er
of
p
o
in
ts
is
r
ed
u
ce
d
by
ap
p
r
o
x
i
m
ately
50
%
to
7
0
%,
in
d
icati
n
g
a
s
u
b
s
tan
tial
d
ec
r
ea
s
e
in
r
ed
u
n
d
an
t
or
n
o
n
-
ess
en
tial
d
ata.
Desp
ite
th
is
r
ed
u
ctio
n
,
k
ey
e
n
v
ir
o
n
m
en
tal
f
ea
tu
r
es
s
u
ch
as
co
r
n
er
s
an
d
ar
cs
ar
e
lar
g
ely
p
r
eser
v
ed
,
e
n
s
u
r
in
g
th
at
th
e
s
t
r
u
ctu
r
al
in
teg
r
ity
of
th
e
s
ce
n
e
is
m
ain
tain
ed
f
o
r
r
elo
ca
lizatio
n
.
T
h
e
ex
p
er
im
en
tal
r
esu
lts
d
em
o
n
s
tr
ate
th
at
th
e
p
r
o
p
o
s
ed
m
et
h
o
d
ef
f
ec
tiv
ely
f
ilter
s
d
y
n
a
m
ic
elem
en
ts
wh
ile
p
r
eser
v
in
g
ess
en
tial
g
eo
m
etr
ic
f
ea
tu
r
es
of
th
e
en
v
ir
o
n
m
en
t.
T
h
is
not
o
n
l
y
im
p
r
o
v
es
d
ata
ef
f
icien
cy
b
u
t
also
en
h
an
ce
s
th
e
r
o
b
u
s
tn
ess
an
d
ac
cu
r
ac
y
of
s
u
b
s
eq
u
en
t
r
el
o
ca
lizatio
n
m
o
d
u
les.
(
a)
(
b
)
Fig
u
r
e
4
.
L
iDAR
d
ata
co
m
p
ar
is
o
n
:
(
a)
L
iDAR
d
ata
af
ter
d
y
n
am
ic
o
b
s
tacle
d
etec
tio
n
a
n
d
f
ea
tu
r
e
ex
tr
ac
tio
n
an
d
(
b
)
r
aw
L
iDAR
d
ata
3
.
2
.
B
enchm
a
rk
t
esting
on
O
penL
O
R
I
S
-
Sce
ne
Da
t
a
s
et
T
h
is
s
tu
d
y
u
s
es
th
e
Op
e
n
L
OR
I
S
-
Scen
e
p
u
b
lic
d
ataset
to
test
th
e
r
elo
ca
lizatio
n
s
y
s
tem
’
s
lo
ca
lizatio
n
ac
cu
r
ac
y
in
in
d
o
o
r
en
v
ir
o
n
m
en
ts
.
T
h
e
d
ataset
was
ac
q
u
ir
e
d
by
m
o
b
ile
r
o
b
o
ts
in
r
ea
l
-
w
o
r
ld
e
n
v
ir
o
n
m
en
ts
,
wh
ich
in
clu
d
e
d
g
r
o
u
n
d
tr
u
th
tr
ajec
to
r
ies
f
r
o
m
m
o
tio
n
ca
p
tu
r
e
d
ev
ices
or
h
ig
h
-
p
r
ec
is
io
n
L
iDAR
as
well
as
m
u
ltimo
d
al
s
en
s
o
r
d
ata
f
r
o
m
a
v
ar
iety
of
s
ettin
g
s
,
s
u
ch
as
ca
f
es,
co
r
r
id
o
r
s
,
an
d
o
f
f
ices.
Fig
u
r
e
5
illu
s
tr
ates
th
e
s
ele
ctio
n
of
f
o
u
r
d
ata
s
eq
u
e
n
ce
s
f
r
o
m
two
d
is
tin
ct
s
ce
n
ar
io
s
in
th
e
Op
en
L
OR
I
S
-
Scen
e
d
ataset
u
s
ed
f
o
r
ev
alu
atio
n
.
Sp
ec
if
ically
,
Fig
u
r
e
5
(
a)
an
d
Fig
u
r
e
5
(
b
)
co
r
r
esp
o
n
d
to
th
e
C
af
e1
-
1
an
d
C
af
e1
-
2
s
eq
u
e
n
c
es,
wh
ich
ca
p
tu
r
e
d
y
n
am
ic
in
d
o
o
r
en
v
ir
o
n
m
en
ts
with
f
r
eq
u
en
t
h
u
m
an
ac
tiv
ity
.
In
co
n
tr
ast,
Fig
u
r
e
5
(
c)
a
n
d
Fig
u
r
e
5
(
d
)
s
h
o
w
th
e
C
o
r
r
id
o
r
1
-
1
an
d
C
o
r
r
id
o
r
1
-
2
s
eq
u
e
n
ce
s
,
wh
ich
p
r
esen
t
ch
allen
g
es
s
u
ch
as
s
tr
o
n
g
g
lass
r
ef
lectio
n
s
an
d
illu
m
in
atio
n
ch
an
g
es.
T
h
ese
r
ep
r
esen
tativ
e
s
eq
u
en
ce
s
h
ig
h
lig
h
t
th
e
d
iv
er
s
e
e
n
v
ir
o
n
m
en
tal
co
n
d
itio
n
s
u
n
d
e
r
w
h
ich
o
u
r
r
elo
ca
lizatio
n
m
eth
o
d
is
test
ed
.
T
h
e
ex
p
er
im
en
tal
p
latf
o
r
m
c
o
n
s
is
ted
of
an
u
p
p
er
c
o
m
p
u
ter
b
ase
d
on
an
I
n
tel
C
o
r
e
i5
p
r
o
ce
s
s
o
r
r
u
n
n
in
g
Ub
u
n
t
u
2
0
.
0
4
with
th
e
R
OS
in
teg
r
at
ed
in
to
it.
An
o
f
f
lin
e
s
p
ar
s
e
s
em
an
tic
m
ap
of
th
e
tar
g
et
en
v
ir
o
n
m
en
t
was
g
en
er
ated
u
s
in
g
OR
B
-
SLAM
3
,
r
u
n
n
i
n
g
in
Mo
n
o
cu
lar
-
I
n
er
ti
al
mode
with
a
cu
s
to
m
co
n
f
ig
u
r
atio
n
f
ile
ad
a
p
ted
to
th
e
d
ataset.
T
h
is
m
ap
in
cl
u
d
ed
f
ea
tu
r
e
p
o
in
ts
,
k
e
y
f
r
am
es,
an
d
c
o
v
is
ib
ilit
y
g
r
a
p
h
s
.
B
o
th
our
alg
o
r
ith
m
an
d
AM
C
L
u
tili
ze
th
e
h
ig
h
-
p
r
ec
is
io
n
g
r
id
m
ap
s
cr
ea
ted
with
L
id
ar
I
n
er
tial
Od
o
m
etr
y
v
ia
Sm
o
o
th
in
g
a
n
d
Ma
p
p
in
g
(
LIO
-
SAM
)
as
g
r
o
u
n
d
tr
u
th
r
ef
er
en
ce
s
.
E
ac
h
d
a
taset
h
ad
20
ex
p
e
r
im
en
tal
tr
i
als
f
o
r
test
in
g
each
alg
o
r
ith
m
,
a
n
d
th
e
r
elo
ca
lizatio
n
er
r
o
r
was
d
ef
in
e
d
as
(
1
2
)
a
n
d
(
1
3
)
.
e
r
r
=
√
(
̂
−
)
2
+
(
̂
−
)
2
(
12
)
=
|
̂
−
|
×
180
(
13
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
AE
S
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
o
b
o
t G
a
u
s
s
ia
n
-
h
is
to
r
ica
l relo
ca
liz
a
tio
n
:
in
erti
a
l m
ea
s
u
r
eme
n
t u
n
it
-
LiDAR
… (
Ye
-
Min
g
S
h
en
)
445
(
a)
(
b
)
(
c)
(
d
)
Fig
u
r
e
5
.
Op
e
n
L
OR
I
S
-
Scen
e
d
ataset
en
v
ir
o
n
m
e
n
ts
:
(
a)
C
af
e1
-
1
s
er
ies
,
(
b
)
C
af
e1
-
2
s
er
ies
,
(
c)
C
o
r
r
id
o
r
1
-
1
s
er
ies
,
an
d
(
d
)
C
o
r
r
id
o
r
1
-
2
s
er
ies
Fig
u
r
e
6
s
h
o
ws
box
p
lo
ts
of
r
elo
ca
lizatio
n
ex
p
e
r
im
en
t
r
esu
l
ts
co
m
p
ar
in
g
d
if
f
e
r
en
t
alg
o
r
it
h
m
s
ac
r
o
s
s
m
u
ltip
le
ev
alu
atio
n
m
etr
ics.
A
box
-
p
lo
t
is
a
s
tatis
tical
g
r
ap
h
f
o
r
d
escr
ib
in
g
th
e
d
is
cr
ete
d
eg
r
ee
of
a
g
r
o
u
p
of
d
ata.
T
h
e
s
tab
ilit
y
of
th
e
r
elo
ca
lizatio
n
can
be
r
ef
lecte
d
by
t
h
e
box
-
p
lo
t.
T
h
e
h
o
r
izo
n
tal
li
n
e
in
s
id
e
th
e
box
of
a
b
o
x
p
l
o
t
r
e
p
r
esen
ts
th
e
av
er
ag
e
v
alu
e
.
As
s
h
o
wn
in
Fig
u
r
e
6
(
a)
,
th
e
b
o
x
p
lo
ts
of
o
u
r
alg
o
r
ith
m
d
em
o
n
s
tr
ate
a
r
elativ
ely
lo
wer
o
v
er
all
p
o
s
itio
n
(
av
er
ag
e
er
r
o
r
:
0
.
1
4
6
m)
an
d
s
m
aller
d
ata
d
is
p
er
s
i
o
n
.
Acc
o
r
d
in
g
to
co
m
p
ar
ativ
e
an
aly
s
is
,
our
ap
p
r
o
ac
h
o
u
tp
er
f
o
r
m
s
th
e
L
iDAR
-
d
ep
en
d
en
t
AM
C
L
alg
o
r
ith
m
(
0
.
3
2
1
m
av
er
ag
e
er
r
o
r
)
by
5
4
.
7
%
in
ter
m
s
of
o
v
er
all
p
o
s
itio
n
al
er
r
o
r
wh
ile
a
ch
iev
in
g
p
o
s
itio
n
in
g
ac
cu
r
ac
y
on
p
ar
with
OR
B
-
SLAM
3
(
0
.
1
5
0
m
av
er
ag
e
er
r
o
r
)
.
B
u
t
as
Fig
u
r
e
6
(
b
)
illu
s
tr
a
tes,
our
alg
o
r
ith
m
’
s
an
g
u
lar
e
r
r
o
r
n
o
ticea
b
ly
r
is
es
wh
en
co
m
p
a
r
ed
to
OR
B
-
SLAM
3
,
esp
ec
ially
in
co
r
r
id
o
r
d
atasets
.
T
h
e
m
ain
ca
u
s
es
of
th
is
p
er
f
o
r
m
an
ce
d
eg
r
ad
atio
n
ar
e
(
1
)
th
e
L
iDAR
s
y
s
tem
’
s
180°
s
ca
n
n
in
g
r
an
g
e
in
th
e
Op
en
L
OR
I
S
-
Scen
e
d
ataset
an
d
(
2
)
th
e
g
lass
s
u
r
f
ac
es
th
at
ar
e
co
m
m
o
n
in
co
r
r
id
o
r
en
v
ir
o
n
m
e
n
ts
,
wh
ich
s
ig
n
if
ican
tly
r
ed
u
ce
th
e
n
u
m
b
er
of
d
etec
tab
le
L
iDAR
f
ea
tu
r
e
p
o
i
n
ts
,
co
n
s
eq
u
en
tly
in
cr
ea
s
in
g
t
h
e
d
is
p
er
s
io
n
of
an
g
u
lar
m
ea
s
u
r
em
en
ts
.
Fu
r
th
er
a
n
aly
s
is
in
co
r
p
o
r
atin
g
Fig
u
r
e
6
(
c
)
a
n
d
Fig
u
r
e
6
(
d
)
r
ev
ea
ls
t
h
at
o
u
r
al
g
o
r
ith
m
a
ch
iev
es
an
av
er
ag
e
C
PU
u
tili
za
tio
n
of
ju
s
t
0
.
4
0
u
n
its
,
r
ep
r
esen
tin
g
an
8
4
.
7
%
r
ed
u
ctio
n
co
m
p
ar
e
d
to
OR
B
-
SL
AM
3
(
2
.
6
0
u
n
its
)
an
d
a
5
8
.
9
%
im
p
r
o
v
e
m
en
t
o
v
er
AM
C
L
(
0
.
9
7
u
n
it
s
)
.
In
ter
m
s
of
p
r
o
ce
s
s
in
g
tim
e,
our
alg
o
r
ith
m
co
m
p
letes
r
elo
ca
lizatio
n
in
1
.
3
0
s
ec
o
n
d
s
on
av
e
r
ag
e
;
o
n
ly
15%
s
lo
wer
th
a
n
OR
B
-
SLAM
3
’
s
1
.
1
3
s
ec
o
n
d
s
wh
ile
b
ein
g
7
4
.
4
%
f
aster
th
an
AM
C
L
’
s
5
.
0
8
-
s
ec
o
n
d
r
u
n
tim
e.
T
h
e
lo
we
r
p
o
s
itio
n
an
d
co
m
p
ac
t
d
is
p
er
s
io
n
of
its
box
p
l
o
ts
clea
r
ly
d
e
m
o
n
s
tr
ate
th
at
th
e
p
r
o
p
o
s
ed
m
eth
o
d
co
n
s
u
m
es
f
ewe
r
co
m
p
u
tatio
n
al
r
eso
u
r
ce
s
wh
ile
m
ain
tain
in
g
s
tab
le
p
er
f
o
r
m
an
c
e
d
u
r
in
g
r
el
o
ca
lizatio
n
p
r
o
ce
s
s
es.
T
h
ese
ch
ar
ac
ter
is
tics
co
n
f
i
r
m
th
e
alg
o
r
ith
m
’
s
h
ig
h
er
c
o
m
p
u
tatio
n
al
ef
f
icien
c
y
an
d
f
aster
r
elo
ca
lizatio
n
ca
p
ab
ilit
y
.
3
.
3
.
F
ield
v
a
lid
a
t
i
o
n
in
re
a
l
-
wo
rld
env
iro
nm
ent
s
To
v
er
if
y
th
e
ef
f
ec
tiv
e
n
ess
an
d
r
o
b
u
s
tn
ess
of
th
e
p
r
o
p
o
s
ed
m
eth
o
d
,
e
x
p
er
im
e
n
ts
wer
e
co
n
d
u
cted
in
an
ac
tu
al
r
o
o
m
en
v
i
r
o
n
m
e
n
t
u
s
in
g
a
co
m
p
r
e
h
en
s
iv
e
s
en
s
o
r
s
u
ite.
T
h
e
r
ea
l
r
o
b
o
t
p
latf
o
r
m
was
eq
u
ip
p
ed
with
an
R
PLI
DAR
A2
L
iDAR
s
e
n
s
o
r
f
o
r
2D
en
v
ir
o
n
m
e
n
tal
s
ca
n
n
in
g
,
a
W
HE
E
L
T
E
C
N1
0
0
I
MU
f
o
r
p
r
ec
is
e
m
o
tio
n
tr
ac
k
i
n
g
an
d
o
r
ien
tatio
n
esti
m
atio
n
,
an
d
an
u
p
p
e
r
co
m
p
u
ter
b
ased
on
I
n
tel
C
o
r
e
i5
p
r
o
ce
s
s
o
r
.
T
h
e
s
y
s
tem
o
p
er
ated
u
n
d
e
r
th
e
Ub
u
n
tu
2
0
.
0
4
e
n
v
ir
o
n
m
en
t
with
th
e
R
OS
f
r
am
ewo
r
k
.
Fo
r
co
m
p
ar
is
o
n
,
OR
B
-
SLAM
3
was
r
u
n
in
Mo
n
o
cu
lar
-
I
n
er
tial
mode
u
s
in
g
a
m
o
n
o
cu
lar
ca
m
er
a
an
d
th
e
W
HE
E
L
T
E
C
N1
0
0
I
MU
,
with
im
ag
e
-
I
MU
s
y
n
ch
r
o
n
izat
io
n
ac
h
iev
e
d
th
r
o
u
g
h
tim
estam
p
alig
n
m
e
n
t.
Fig
u
r
e
7
p
r
ese
n
ts
th
e
ex
p
er
im
e
n
tal
s
etu
p
,
wh
er
e
Fig
u
r
e
7
(
a
)
s
h
o
ws
th
e
p
h
y
s
ical
test
en
v
ir
o
n
m
en
t,
Fig
u
r
e
7
(
b
)
d
is
p
lay
s
th
e
co
r
r
esp
o
n
d
in
g
test
en
v
ir
o
n
m
en
t
m
ap
,
an
d
Fig
u
r
e
7
(
c)
illu
s
tr
ates
th
e
co
m
p
lete
m
o
b
ile
r
o
b
o
t
p
latf
o
r
m
with
all
i
n
teg
r
ated
s
en
s
o
r
s
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
7
2
2
-
2
5
8
6
I
AE
S
I
n
t
J
R
o
b
&
A
u
to
m
,
Vo
l
.
14
,
No
.
3
,
Sep
tem
b
er
20
25
:
4
3
8
-
4
5
0
446
(
a)
(
b
)
(
c)
(
d
)
Fig
u
r
e
6
.
R
elo
ca
lizatio
n
ex
p
er
im
en
t
d
ata
of
d
atasets
-
co
m
p
ar
is
o
n
of
d
if
f
er
en
t
alg
o
r
ith
m
s
:
(
a
)
p
o
s
itio
n
er
r
o
r
,
(
b
)
an
g
le
er
r
o
r
,
(c
)
C
PU
lo
ad
,
an
d
(
d
)
tim
e
co
s
t
(
a)
(
b
)
(
c)
Fig
u
r
e
7
.
R
elo
ca
lizatio
n
test
e
n
v
ir
o
n
m
en
t:
(
a)
test
en
v
ir
o
n
m
en
t,
(
b
)
test
en
v
ir
o
n
m
en
t
m
ap
,
an
d
(
c
)
r
o
b
o
t
m
o
b
ile
Evaluation Warning : The document was created with Spire.PDF for Python.
I
AE
S
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2722
-
2
5
8
6
R
o
b
o
t G
a
u
s
s
ia
n
-
h
is
to
r
ica
l relo
ca
liz
a
tio
n
:
in
erti
a
l m
ea
s
u
r
eme
n
t u
n
it
-
LiDAR
… (
Ye
-
Min
g
S
h
en
)
447
T
h
is
s
tu
d
y
co
n
d
u
cted
a
s
er
ie
s
of
“
r
o
b
o
t
k
id
n
ap
p
in
g
”
test
s
to
ev
alu
ate
th
e
p
er
f
o
r
m
a
n
c
e
of
th
r
ee
alg
o
r
ith
m
s
:
th
e
p
r
o
p
o
s
ed
m
et
h
o
d
,
OR
B
-
SLAM
3
,
an
d
AM
C
L
.
To
en
h
an
ce
th
e
co
m
p
ar
a
tiv
e
an
aly
s
is
u
n
d
e
r
d
y
n
am
ic
c
o
n
d
itio
n
s
,
t
h
e
ex
p
er
im
en
tal
en
v
ir
o
n
m
e
n
t
in
co
r
p
o
r
ated
m
o
v
i
n
g
o
b
s
tacle
s
th
at
ac
co
u
n
te
d
f
o
r
ap
p
r
o
x
im
ately
15%
of
b
o
th
L
iDAR
s
ca
n
s
an
d
ca
m
er
a
f
r
am
es.
Ad
d
itio
n
al
ch
allen
g
i
n
g
s
ce
n
ar
io
s
wer
e
in
tr
o
d
u
ce
d
at
s
p
ec
if
ic
lo
ca
tio
n
s
:
g
lass
in
ter
f
er
en
ce
at
Po
i
n
t
C
an
d
r
e
d
u
ce
d
illu
m
i
n
atio
n
at
Po
in
t
F.
T
h
e
ex
p
er
im
en
tal
e
v
alu
atio
n
in
v
o
l
v
ed
co
n
d
u
ctin
g
co
m
p
r
eh
en
s
iv
e
r
o
b
o
t
k
id
n
a
p
p
in
g
test
s
ac
r
o
s
s
m
u
ltip
le
s
ce
n
ar
io
s
(
A→B
,
B
→
C
,
C
→D,
D→E
,
an
d
E
→F),
with
s
y
s
tem
atic
p
er
f
o
r
m
a
n
ce
co
m
p
ar
is
o
n
s
m
ea
s
u
r
ed
th
r
o
u
g
h
f
o
u
r
k
ey
m
etr
ics:
p
o
s
itio
n
in
g
ac
cu
r
ac
y
(
m
ea
n
er
r
o
r
an
d
s
u
cc
ess
r
ate
d
o
cu
m
en
ted
in
T
a
b
le
1
)
an
d
co
m
p
u
tatio
n
al
ef
f
icien
cy
(
p
r
o
ce
s
s
in
g
tim
e
an
d
C
PU
u
tili
za
tio
n
p
r
esen
ted
in
T
ab
le
2
).
T
ab
le
1
.
Po
s
itio
n
er
r
o
r
a
n
d
s
u
c
ce
s
s
r
ate
co
m
p
ar
is
o
n
by
alg
o
r
ith
m
Ex
p
e
r
i
m
e
n
t
a
l
p
o
i
n
t
s
O
u
r
a
l
g
o
r
i
t
h
m
O
R
B
-
S
LA
M
3
A
M
C
L
M
e
a
n
e
r
r
o
r
S
u
c
c
e
ss
r
a
t
e
M
e
a
n
e
r
r
o
r
S
u
c
c
e
ss
r
a
t
e
M
e
a
n
e
r
r
o
r
S
u
c
c
e
ss
r
a
t
e
A
→B
0
.
0
7
3
m,
0
.
5
8
°
1
0
0
%
0
.
0
7
4
m,
0
.
5
7
°
9
5
%
0
.
2
1
0
m,
9
.
2
5
°
6
0
%
B
→C
0
.
1
3
7
m,
0
.
7
0
°
6
0
%
0
.
1
0
8
m,
0
.
6
9
°
8
0
%
0
.
3
9
4
m,
1
1
.
3
2
°
1
0
%
C
→D
0
.
1
0
9
m
，
0
.
7
2
°
8
0
%
0
.
1
1
2
m,
0
.
6
2
°
8
5
%
0
.
2
2
4
m,
1
0
.
4
9
°
4
5
%
D
→E
0
.
1
1
3
m,
0
.
9
2
°
9
5
%
0
.
0
9
0
m,
0
.
7
6
°
9
0
%
0
.
2
6
2
m,
1
.
5
6
°
3
0
%
E
→F
0
.
1
0
1
m,
0
.
6
1
°
8
5
%
0
.
1
3
7
m,
1
.
1
0
°
5
5
%
0
.
5
2
7
m,
1
.
3
1
°
4
0
%
M
e
a
n
0
.
1
0
6
m
,
0
.
6
5
°
8
4
%
0
.
1
0
4
m
,
0
.
7
4
°
8
1
%
0
.
3
2
3
m
,
6
.
7
8
°
3
7
%
T
ab
le
2
.
E
x
ec
u
tio
n
tim
e
a
n
d
C
PU
u
tili
za
tio
n
co
m
p
ar
is
o
n
by
alg
o
r
ith
m
Ex
p
e
r
i
m
e
n
t
a
l
p
o
i
n
t
s
O
u
r
a
l
g
o
r
i
t
h
m
O
R
B
-
S
LA
M
3
A
M
C
L
M
e
a
n
t
i
m
e
C
P
U
l
o
a
d
M
e
a
n
t
i
m
e
C
P
U
l
o
a
d
M
e
a
n
t
i
m
e
C
P
U
l
o
a
d
A
→B
0
.
6
8
s
3
5
.
4
%
2
.
8
2
s
2
7
8
.
2
%
4
.
5
5
s
1
0
0
.
6
%
B
→C
2
.
6
7
s
3
7
.
3
%
2
.
1
7
s
2
6
5
.
7
%
7
.
5
8
s
9
9
.
6
%
C
→D
1
.
7
4
s
3
9
.
9
%
1
.
9
3
s
2
4
6
.
4
%
4
.
6
2
s
9
8
.
4
%
D
→E
1
.
1
8
s
4
1
.
9
%
2
.
2
0
s
2
7
7
.
4
%
4
.
4
8
s
1
0
1
.
7
%
E
→F
2
.
1
5
s
3
7
.
6
%
2
.
6
6
2
6
4
.
1
%
5
.
1
5
s
8
8
.
9
%
M
e
a
n
1
.
6
8
s
3
8
.
4
%
2
.
3
4
s
2
6
6
.
3
%
5
.
2
7
s
9
7
.
9
%
T
h
e
p
r
o
p
o
s
ed
alg
o
r
ith
m
d
em
o
n
s
tr
ates
s
u
p
er
io
r
r
elo
ca
lizatio
n
ac
cu
r
ac
y
due
to
I
MU
-
b
ased
h
ea
d
in
g
co
n
s
tr
ain
ts
,
ac
h
iev
in
g
an
8
9
.
6
%
r
ed
u
ctio
n
in
m
ea
n
y
aw
er
r
o
r
co
m
p
ar
ed
to
AM
C
L
.
Fu
r
th
er
m
o
r
e,
t
h
e
o
v
e
r
all
s
u
cc
ess
r
ate
is
im
p
r
o
v
ed
by
4
7
%
r
elativ
e
to
AM
C
L
.
T
h
e
co
n
v
en
tio
n
al
AM
C
L
ap
p
r
o
ac
h
is
p
r
o
n
e
to
p
r
em
at
u
r
e
p
ar
ticle
co
n
v
e
r
g
en
ce
,
wh
er
e
p
ar
ticles
er
r
o
n
eo
u
s
ly
co
n
ce
n
t
r
ate
in
lo
ca
lly
o
p
tim
al
—
but
g
lo
b
ally
in
c
o
r
r
ec
t
—
r
eg
io
n
s
,
lead
in
g
to
s
u
b
o
p
tim
al
p
o
s
e
esti
m
atio
n
.
In
Scen
ar
io
C
with
g
lass
in
ter
f
er
en
ce
,
th
e
p
r
o
p
o
s
ed
m
eth
o
d
m
ain
tain
s
a
p
o
s
itio
n
al
er
r
o
r
of
m
e
r
ely
0
.
1
0
9
m
with
a
60%
s
u
cc
ess
r
ate,
wh
er
ea
s
AM
C
L
ex
h
ib
its
s
ig
n
if
ican
tly
h
ig
h
er
er
r
o
r
s
an
d
a
d
r
asti
ca
lly
r
ed
u
ce
d
s
u
cc
ess
r
ate
of
1
0
%,
r
en
d
er
in
g
it
n
ea
r
ly
in
ef
f
ec
tiv
e
.
T
h
ese
r
esu
lts
u
n
d
er
s
co
r
e
th
e
cr
itical
r
o
le
of
t
h
e
Gau
s
s
ian
3σ
co
n
s
tr
ain
t
m
o
d
elin
g
f
o
r
h
is
to
r
ical
p
o
s
es
in
en
h
a
n
cin
g
r
o
b
u
s
tn
ess
.
W
h
ile
OR
B
-
SLAM
3
ac
h
iev
es
an
80%
s
u
cc
ess
r
ate
in
Sc
en
ar
io
C,
its
p
er
f
o
r
m
a
n
ce
s
ig
n
if
ican
tly
d
eg
r
ad
es
u
n
d
er
lo
w
-
lig
h
t
co
n
d
itio
n
s
(
Scen
ar
io
F),
ex
h
ib
itin
g
a
2
6
.
9
%
in
cr
ea
s
e
in
p
o
s
itio
n
al
er
r
o
r
an
d
a
3
1
.
3
%
d
ec
lin
e
in
s
u
cc
ess
r
ate
(
f
r
o
m
80%
to
5
5
%).
In
co
n
t
r
ast,
th
e
p
r
o
p
o
s
ed
alg
o
r
ith
m
m
ain
tain
s
s
tab
le
p
er
f
o
r
m
an
ce
in
Scen
ar
io
F,
d
e
m
o
n
s
tr
atin
g
its
illu
m
in
atio
n
-
in
v
ar
ian
t
r
el
iab
ilit
y
.
T
h
is
co
n
s
is
ten
t
p
er
f
o
r
m
an
ce
ef
f
ec
tiv
ely
h
ig
h
lig
h
ts
th
e
in
h
er
en
t
a
d
v
an
t
ag
e
of
L
iDAR
’
s
in
s
en
s
itiv
ity
to
lig
h
tin
g
v
ar
iatio
n
s
.
T
h
e
co
m
p
u
tatio
n
al
ef
f
icien
c
y
ev
alu
atio
n
d
em
o
n
s
tr
ates
th
at
t
h
e
p
r
o
p
o
s
ed
m
eth
o
d
ac
h
iev
es
s
ig
n
if
ican
t
im
p
r
o
v
em
e
n
ts
ac
r
o
s
s
all
m
etr
ics
co
m
p
ar
e
d
to
b
aselin
e
alg
o
r
ith
m
s
.
R
elativ
e
to
A
MCL
(
9
7
.
9
%
C
PU
u
tili
za
tio
n
)
,
th
e
p
r
o
p
o
s
ed
s
o
lu
tio
n
s
h
o
ws
a
6
8
.
1
%
im
p
r
o
v
e
m
en
t
in
lo
ca
lizatio
n
s
p
ee
d
(
1
.
6
8
s
vs
5
.
2
7
s)
w
h
ile
r
ed
u
cin
g
C
PU
u
s
ag
e
by
6
0
.
8
%
(
3
8
.
4
%
vs
9
7
.
9
%).
W
h
en
co
m
p
a
r
ed
to
OR
B
-
SLA
M3
(
2
6
6
.
3
%
C
PU
u
tili
za
tio
n
)
,
it
m
ain
tain
s
a
2
8
.
5
%
s
p
ee
d
a
d
v
an
ta
g
e
with
an
8
5
.
6
%
r
ed
u
ctio
n
in
co
m
p
u
tati
o
n
al
lo
a
d
(
3
8
.
4
%
vs
2
6
6
.
3
%).
Alth
o
u
g
h
p
r
o
ce
s
s
in
g
d
em
a
n
d
s
in
cr
ea
s
e
in
f
ea
tu
r
e
-
d
en
s
e
s
ce
n
ar
i
o
s
(C
an
d
F)
due
to
e
x
ten
s
iv
e
f
ea
tu
r
e
ex
tr
ac
tio
n
a
n
d
lo
n
g
-
r
a
n
g
e
r
elo
ca
lizatio
n
r
e
q
u
ir
em
e
n
ts
,
th
e
alg
o
r
ith
m
’
s
o
p
tim
ized
L
iDAR
p
r
o
ce
s
s
in
g
p
ip
elin
e
an
d
ef
f
icien
t
s
p
ir
al
s
e
ar
ch
s
tr
ateg
y
en
ab
le
it
to
co
n
s
is
ten
tly
o
u
tp
er
f
o
r
m
AM
C
L
in
b
o
th
ac
cu
r
ac
y
an
d
r
eso
u
r
ce
u
tili
za
tio
n
.
T
h
ese
r
e
s
u
lts
v
alid
ate
th
e
s
y
s
tem
’
s
a
b
ilit
y
to
m
ain
tain
h
ig
h
co
m
p
u
tatio
n
al
ef
f
icien
c
y
wh
ile
d
eliv
er
in
g
r
o
b
u
s
t
p
er
f
o
r
m
an
ce
ac
r
o
s
s
d
iv
er
s
e
o
p
er
atin
g
co
n
d
itio
n
s
.
Evaluation Warning : The document was created with Spire.PDF for Python.