Embed Size (px)
Citation preview
PolySLAMA 2D Polygon-based SLAM Algorithm
Johann Dichtl Xuan Sang Le Luc FabresseGuillaume Lozenguez Noury Bouraqadi
IMT Lille Douai, Univ. Lillehttp://car.imt-lille-douai.fr
April 19, 2019
Dichtl et al PolySLAM April 19, 2019 1 / 39
Context
Our context: Multi-robot exploration
Requirements towards the map format:
lightweight map format for easy sharing between robots/agentssupport for navigationsupport for frontier-based exploration
Dichtl et al PolySLAM April 19, 2019 2 / 39
Context
State-of-the-Art Map Formats
Map format Lightweight Navigation support FrontiersGrid maps no yes only implicit
Feature-based yes no noVector-based yes no no
Dichtl et al PolySLAM April 19, 2019 3 / 39
Context
no common map format supports all threerequirements.
Dichtl et al PolySLAM April 19, 2019 4 / 39
PolyMap Format
PolyMap Format
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
Dichtl et al PolySLAM April 19, 2019 5 / 39
PolyMap Format
Placeholder Slide
ToDo:
make obstacle vectors thicker in figuresif time left: show the different SLAM steps step-by-step withfigures, starting with a pointcloud and ending with a (small)mapadd numbers to map examples (trajectory length, max. error)update last page according to noury’s comments
Dichtl et al PolySLAM April 19, 2019 6 / 39
PolySLAM
PolySLAM
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisitionpointcloud pose estimate
pointcloud
polygon
polygon/PolyMap
PolyMappose estimate
laser scan odometry
map
Dichtl et al PolySLAM April 19, 2019 7 / 39
PolySLAM
PolySLAM
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisitionpointcloud pose estimate
pointcloud
polygon
polygon/PolyMap
PolyMappose estimate
laser scan odometry
map
Dichtl et al PolySLAM April 19, 2019 8 / 39
PolySLAM
Data Acquisition
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
Dichtl et al PolySLAM April 19, 2019 9 / 39
PolySLAM
Data Acquisition: collect raw sensor data
-0.5
0
0.5
1
1.5
2
-1 -0.5 0 0.5 1 1.5 2 2.5 3
Robot's local frame
Dichtl et al PolySLAM April 19, 2019 10 / 39
PolySLAM
Data Acquisition: transform from local to global frame
1
1.5
2
2.5
3
3.5
2 2.5 3 3.5 4 4.5
Global (map) frame
Dichtl et al PolySLAM April 19, 2019 11 / 39
PolySLAM
PolySLAM
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisitionpointcloud pose estimate
pointcloud
polygon
polygon/PolyMap
PolyMappose estimate
laser scan odometry
map
Dichtl et al PolySLAM April 19, 2019 12 / 39
PolySLAM
PolySLAM - point-to-vector ICP
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: setup
robot
Dichtl et al PolySLAM April 19, 2019 13 / 39
PolySLAM
PolySLAM - point-to-vector ICP
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: laser scan data
robot
Dichtl et al PolySLAM April 19, 2019 14 / 39
PolySLAM
PolySLAM - point-to-vector ICP
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: match pairs
robot
Dichtl et al PolySLAM April 19, 2019 15 / 39
PolySLAM
PolySLAM - point-to-vector ICP
0
0.5
1
1.5
2
2.5
3
0 0.5 1 1.5 2 2.5 3 3.5 4
point-to-vector ICP: aligned point cloud
robot
Dichtl et al PolySLAM April 19, 2019 16 / 39
PolySLAM
PolySLAM - backface culling
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - setup
robot
Dichtl et al PolySLAM April 19, 2019 17 / 39
PolySLAM
PolySLAM - backface culling
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - laser scan
robot
Dichtl et al PolySLAM April 19, 2019 18 / 39
PolySLAM
PolySLAM - backface culling
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - without backface culling
robot
Dichtl et al PolySLAM April 19, 2019 19 / 39
PolySLAM
PolySLAM - backface culling
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - with backface culling
robot
Dichtl et al PolySLAM April 19, 2019 20 / 39
PolySLAM
PolySLAM - backface culling
0
0.5
1
1.5
2
2.5
1 1.5 2 2.5 3 3.5 4
ICP example - with backface culling
robot
Dichtl et al PolySLAM April 19, 2019 21 / 39
PolySLAM
PolySLAM: Keyframe Craetion
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisitionpointcloud pose estimate
pointcloud
polygon
polygon/PolyMap
PolyMappose estimate
laser scan odometry
map
Dichtl et al PolySLAM April 19, 2019 22 / 39
PolySLAM
PolySLAM: Keyframe Craetion
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
Dichtl et al PolySLAM April 19, 2019 23 / 39
PolySLAM
PolySLAM: Keyframe Craetion
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
19 vectors
Dichtl et al PolySLAM April 19, 2019 24 / 39
PolySLAM
PolySLAM: Polygon Refinement
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisitionpointcloud pose estimate
pointcloud
polygon
polygon/PolyMap
PolyMappose estimate
laser scan odometry
map
Dichtl et al PolySLAM April 19, 2019 25 / 39
PolySLAM
PolySLAM: Polygon Refinement
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
19 vectors
Dichtl et al PolySLAM April 19, 2019 26 / 39
PolySLAM
PolySLAM: Polygon Refinement
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
robot
8 vectors
Dichtl et al PolySLAM April 19, 2019 27 / 39
PolySLAM
PolySLAM: Merging
PolyMap Merging
Polygon Refinement
Keyframe Creation
Data Alignment
Data Acquisitionpointcloud pose estimate
pointcloud
polygon
polygon/PolyMap
PolyMappose estimate
laser scan odometry
map
Dichtl et al PolySLAM April 19, 2019 28 / 39
PolySLAM
PolySLAM: Merging
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
Dichtl et al PolySLAM April 19, 2019 29 / 39
PolySLAM
PolySLAM: Merging
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
Dichtl et al PolySLAM April 19, 2019 30 / 39
PolySLAM
PolySLAM: Merging
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
Dichtl et al PolySLAM April 19, 2019 31 / 39
PolySLAM
PolySLAM: Merging
0
0.5
1
1.5
2
2.5
3
3.5
4
0 1 2 3 4 5
Dichtl et al PolySLAM April 19, 2019 32 / 39
Experiments
Experiments - Simulator
Dichtl et al PolySLAM April 19, 2019 33 / 39
Experiments
Experiments - Real Robots
-20
-15
-10
-5
0
5
-10 -5 0 5 10 15
otf=0.4, ost=1.2, mi=50, st=0.0009, od=0.3distance traveled=339.7, time spent=28m 45.27s
Dichtl et al PolySLAM April 19, 2019 34 / 39
Experiments
Experiments - Real Robots
-10 0
10
20
30
40 -2
0-1
5-1
0-5
0 5
10
15
otf
=0
.5,
ost=
0.2
, m
i=4
0,
st=
0.0
00
4,
od=
0.2
dis
tance t
ravele
d=
69
.0,
tim
e s
pent=
3m
47
.57
s
Dichtl et al PolySLAM April 19, 2019 35 / 39
Experiments
Experiments - Real Robots
-5
0
5
10
15
-5 0 5 10 15 20 25 30 35 40
Dichtl et al PolySLAM April 19, 2019 36 / 39
Conclusion
Conclusion
ToDo...
Dichtl et al PolySLAM April 19, 2019 37 / 39
Outlook
Outlook
upcoming paper (IntelliSys2019): topological graphs onPolyMapsfurther optimizations, e.g. pose graph optimization &cross-keyframe polygon refinement
Dichtl et al PolySLAM April 19, 2019 38 / 39
Thanks
Thanks
Dichtl et al PolySLAM April 19, 2019 39 / 39
