huongdanGraph Based SLAM Using Pose Graph.pdf

fdasfsa

1. Graph-Based SLAM with
Pose Graphs
2020년11월15일
최승원
[email protected]
1

2. Graph Based SLAM
Modeling
Use a graph to represent the problem
Node –pose of robot
Edge –spatial constraint between two poses(transformation)
Front End & Back End
Front End –Build pose graph by matching sensor data
Back End –Optimize poses using Least Square Method

3. Graph Based SLAM
1D Example of Graph Based SLAM
??????
??????
pose
landmark
??????
??????
??????
??????
??????
4
??????
5
??????
6
??????
4
??????
5
??????
6
Initial position: -3
1
st
movement: 5
2
nd
movement: 3 Information vectorInformation matrix

4. Graph Based SLAM
1D Example of Graph Based SLAM
pose
landmark
??????
??????
??????
??????
??????
??????
??????
4
=−3
??????
5
=??????
4
+5
??????
6
=??????
5
+3
Initial position: -3
1
st
movement: 5
2
nd
movement: 3
??????
4
??????
5
??????
6
??????
4
1
??????
5
??????
6
-3
Information vectorInformation matrix

5. Graph Based SLAM
1D Example of Graph Based SLAM
pose
landmark
??????
??????
??????
??????
??????
??????
??????
4
??????
5
??????
6
??????
4
2-1
??????
5
-11
??????
6
-8
5
??????
4
=−3
??????
5
=??????
4
+5
??????
6
=??????
5
+3
??????
4
−??????
5
=−5
−??????
4
+??????
5
=5
Information vectorInformation matrix

6. Graph Based SLAM
1D Example of Graph Based SLAM
pose
landmark
??????
??????
??????
??????
??????
??????
??????
4
??????
5
??????
6
??????
4
2-1
??????
5
-12-1
??????
6
-11
-8
2
3
??????
4
=−3
??????
5
=??????
4
+5
??????
6
=??????
5
+3
??????
5
−??????
6
=−3
−??????
5
+??????
6
=3
Information vectorInformation matrix

7. Graph Based SLAM
1D Example of Graph Based SLAM
pose
landmark
??????
??????
??????
??????
??????
??????
??????
4
??????
5
??????
6
??????
4
2-10
??????
5
-12-1
??????
6
0-11
-8
2
3
Information vector
??????
4
=−3
??????
5
=??????
4
+5
??????
6
=??????
5
+3
??????= ??????
???????
??????
[??????
4
??????
5
??????
6
]=−3 2 5
?
Information matrix

8. Graph Based SLAM
Concept
??????
5
??????
6
??????
7
??????
8
??????
9
??????
:
Node –pose of robot
Edge –spatial constraint between
two poses(transformation

9. Graph Based SLAM
How to build pose graph
Odometry
ICP(Iterative closest point)

10. Graph Based SLAM
How to build pose graph
Constraint
????????????
X
??????????????

11. Graph Based SLAM
Graph Optimization
Node –pose of robot
Edge –spatial constraint between two poses
Constraint : measure of uncertainty
????????????
X
??????????????

∗
? ????????????
X
??
 
  ????????????

12. Graph Based SLAM
Probabilistic meaning of Graph Optimization –MAP(Maximum A Posterior)
px
0:t
z
1:t
,u
1:t
=η×px
0
∏px
t
x
t−1
,u
t
pz
t
x
t
 
t
(Bayes filter posterior)
logpx
0:t
z
1:t
,u
1:t
=const+logpx
0
+∑logpx
t
x
t−1
,u
t
+logpz
t
x
t
 
t
Assume∶ all probability ~ Nx,μ,Σ (from bayes filter theory)
logNx,μ,Σ=const−
1
2
x−μ
T
Σ
−1
(x−μ)
logpx
0:t
z
1:t
,u
1:t
=const−
1
2
E
p
x−
1
2
∑E
u
t
x+E
z
t
x
 
t
where (E
i
= e
i
T
??????Ω
i
e
i
(??????))
Maximize Posterior== Minimize error
5
6
E
n
x+
5
6
∑E
s
j
x+E
x
j
x
 
r

13. Graph Based SLAM
Least Square Method in SLAM
Overdetermined System(# of states < # of observations)
Non linear transformation

14. Graph Based SLAM
Least Square Method in SLAM
X
∗
=argmin
\
∑??????
ij
T
Ω
ij
??????
ij
 
ij
??????
ij
??????
g
+∆??????
g
,??????
h
+∆??????
h
=??????
gh
??????+∆??????≈??????
ij
??????+
∂e
ij
??????
∂??????
∆??????≈e
ij
??????+J
ij
(??????)∆??????
F
gh
??????+∆??????=??????
ij
T
??????+∆??????Ω
ij
??????
ij
??????+∆??????≈e
ij
??????+J
ij
??????∆??????
??????
Ω
ij
e
ij
??????+J
ij
??????∆??????
??????
ij
T
Ω
ij
??????
ij
=c
ij
,??????
ij
T
Ω
ij
J
ij
=b
ij
T
, J
ij
T
Ω
ij
J
ij
=H
ij
Let c=∑c
ij
 
ij
,b
X
=∑b
ij
T 
ij
,H=∑H
ij
→
 
ij
F??????+∆??????≈c+2b
T
∆??????+∆??????
??????
H∆??????

∂F??????+∆??????
∂∆??????
≈2b+2H∆??????, If set∶2b+2H∆??????=0,∆??????
∗
=−H
−1
b→∆??????
∗
=−H
−1
b, ??????
????????????????????????
=??????+∆??????
∗
(state update)
??????
5
??????
6
??????
7
??????
8
??????
9
??????
:

15. Graph Based SLAM
Least Square Method in SLAM

∂F??????+∆??????
∂∆??????
≈2b+2H∆??????, If set∶2b+2H∆??????=0,∆??????
∗
=−H
−1
b→∆??????
∗
=−H
−1
b, ??????
????????????????????????
=??????+∆??????
∗
(state update)

16. Graph Based SLAM
Non Linear Constraint
?? gh
-5
g
-5
h

17. Graph Based SLAM
Algorithm*
*G. Grisetti, R. Kümmerle, C. Stachnissand W. Burgard, "A Tutorial on Graph-Based SLAM," inIEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925.
A
??
=
−R??????
x
X
R??????
g
X
(R??????
x
X
(R??????
g
?5
)) (??????
??????
−??????
??????
)
?????? -1
B
??
=
R??????
x
X
R??????
g
X
??????
??????1

18. Graph Based SLAM
Algorithm*
*G. Grisetti, R. Kümmerle, C. Stachnissand W. Burgard, "A Tutorial on Graph-Based SLAM," inIEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925.

19. Graph Based SLAM
Algorithm*
*G. Grisetti, R. Kümmerle, C. Stachnissand W. Burgard, "A Tutorial on Graph-Based SLAM," inIEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925.

20. Graph Based SLAM
How to fix first node?
*G. Grisetti, R. Kümmerle, C. Stachnissand W. Burgard, "A Tutorial on Graph-Based SLAM," inIEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925.
Algorithm*
https://github.com/tmadl/python-LS-SLAM

21. facebook.com/groups/slamkr/