Download as PDF, PPTX
![Graph Based SLAM
1D Example of Graph Based SLAM
pose
landmark
𝒙𝟎 𝒙𝟏 𝒙𝟐
𝑥 𝑥 𝑥
𝑥 2 -1 0
𝑥 -1 2 -1
𝑥 0 -1 1
-8
2
3
Information vector
𝑥 = −3
𝑥 = 𝑥 + 5
𝑥 = 𝑥 + 3
𝝁 = 𝛀 𝟏
𝝃
[𝑥 𝑥 𝑥 ] = −3 2 5
Information matrix](https://image.slidesharecdn.com/graphbasedslamusingposegraph-240814183225-e36ee6cd/85/huongdanGraph-Based-SLAM-Using-Pose-Graph-pdf-7-320.jpg)
More Related Content
Similar to huongdanGraph Based SLAM Using Pose Graph.pdf
huongdanGraph Based SLAM Using Pose Graph.pdf
- 1. Graph-Based SLAM with PoseGraphs 2020년 11월 15일 최승원 csw3575@snu.ac.kr 1
- 2. Graph Based SLAM Modeling Usea 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 1DExample of Graph Based SLAM 𝒙𝟎 pose landmark 𝒙𝟏 𝒙𝟐 𝑥 𝑥 𝑥 𝑥 𝑥 𝑥 Initial position: -3 1st movement: 5 2nd movement: 3 Information vector Information matrix
- 4. Graph Based SLAM 1DExample of Graph Based SLAM pose landmark 𝒙𝟎 𝒙𝟏 𝒙𝟐 𝑥 = −3 𝑥 = 𝑥 + 5 𝑥 = 𝑥 + 3 Initial position: -3 1st movement: 5 2nd movement: 3 𝑥 𝑥 𝑥 𝑥 1 𝑥 𝑥 -3 Information vector Information matrix
- 5. Graph Based SLAM 1DExample of Graph Based SLAM pose landmark 𝒙𝟎 𝒙𝟏 𝒙𝟐 𝑥 𝑥 𝑥 𝑥 2 -1 𝑥 -1 1 𝑥 -8 5 𝑥 = −3 𝑥 = 𝑥 + 5 𝑥 = 𝑥 + 3 𝑥 − 𝑥 = −5 −𝑥 + 𝑥 = 5 Information vector Information matrix
- 6. Graph Based SLAM 1DExample of Graph Based SLAM pose landmark 𝒙𝟎 𝒙𝟏 𝒙𝟐 𝑥 𝑥 𝑥 𝑥 2 -1 𝑥 -1 2 -1 𝑥 -1 1 -8 2 3 𝑥 = −3 𝑥 = 𝑥 + 5 𝑥 = 𝑥 + 3 𝑥 − 𝑥 = −3 −𝑥 + 𝑥 =3 Information vector Information matrix
- 7. Graph Based SLAM 1DExample of Graph Based SLAM pose landmark 𝒙𝟎 𝒙𝟏 𝒙𝟐 𝑥 𝑥 𝑥 𝑥 2 -1 0 𝑥 -1 2 -1 𝑥 0 -1 1 -8 2 3 Information vector 𝑥 = −3 𝑥 = 𝑥 + 5 𝑥 = 𝑥 + 3 𝝁 = 𝛀 𝟏 𝝃 [𝑥 𝑥 𝑥 ] = −3 2 5 Information matrix
- 8. Graph Based SLAM Concept 𝑥𝑥 𝑥 𝑥 𝑥 𝑥 Node – pose of robot Edge – spatial constraint between two poses(transformation
- 9. Graph Based SLAM Howto build pose graph Odometry ICP(Iterative closest point)
- 10. Graph Based SLAM Howto build pose graph Constraint 𝒊𝒋 𝒊𝒋
- 11. Graph Based SLAM GraphOptimization Node – pose of robot Edge – spatial constraint between two poses Constraint : measure of uncertainty 𝒊𝒋 𝒊𝒋 ∗ 𝒊𝒋 𝒊𝒋
- 12. Graph Based SLAM Probabilisticmeaning of Graph Optimization – MAP(Maximum A Posterior) p x0:t z1:t, u1:t = η × p x0 ∏ p xt xt−1, ut p zt xt t (Bayes filter posterior) log p x0:t z1:t, u1:t = const + log p x0 + ∑ log p xt xt−1, ut + log p zt xt t Assume ∶ all probability ~ N x, μ, Σ (from bayes filter theory) logN x, μ, Σ = const − 1 2 x − μ TΣ−1(x − μ) log p x0:t z1:t, u1:t = const − 1 2 Ep x − 1 2 ∑ Eut x + Ezt x t where (Ei = ei T 𝐱 Ωiei(𝐱)) Maximize Posterior == Minimize error E x + ∑ E x + E x
- 13. Graph Based SLAM LeastSquare Method in SLAM Overdetermined System(# of states < # of observations) Non linear transformation
- 14. Graph Based SLAM LeastSquare Method in SLAM X∗ = argmin ∑ 𝐞ij T Ωij𝐞ij ij 𝐞ij 𝐱 + ∆𝐱 , 𝐱 + ∆𝐱 = 𝐞 𝐱 + ∆𝐱 ≈ 𝐞ij 𝐱 + ∂eij 𝐱 ∂𝐱 ∆𝐱 ≈ eij 𝐱 + Jij(𝐱)∆𝐱 F 𝐱 + ∆𝐱 = 𝐞ij T 𝐱 + ∆𝐱 Ωij𝐞ij 𝐱 + ∆𝐱 ≈ eij 𝐱 + Jij 𝐱 ∆𝐱 𝐓 Ωij eij 𝐱 + Jij 𝐱 ∆𝐱 𝐞ij T Ωij𝐞ij = cij, 𝐞ij T ΩijJij = bij T , Jij T ΩijJij = Hij Let c = ∑ cij ij , b = ∑ bij T ij , H = ∑ Hij → ij F 𝐱 + ∆𝐱 ≈ c + 2bT∆𝐱 + ∆𝐱𝐓H∆𝐱 ∂F 𝐱+∆𝐱 ∂∆𝐱 ≈ 2b + 2H∆𝐱 , If set ∶ 2b + 2H∆𝐱 = 0, ∆𝐱∗ = −H−1b → ∆𝐱∗ = −H−1b, 𝐱𝐧𝐞𝐱𝐭 = 𝐱 + ∆𝐱∗ (state update) 𝑥 𝑥 𝑥 𝑥 𝑥 𝑥
- 15. Graph Based SLAM LeastSquare Method in SLAM ∂F 𝐱+∆𝐱 ∂∆𝐱 ≈ 2b + 2H∆𝐱 , If set ∶ 2b + 2H∆𝐱 = 0, ∆𝐱∗ = −H−1b → ∆𝐱∗ = −H−1b, 𝐱𝐧𝐞𝐱𝐭 = 𝐱 + ∆𝐱∗ (state update)
- 16. Graph Based SLAM NonLinear Constraint - -
- 17. Graph Based SLAM Algorithm* *G.Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, "A Tutorial on Graph-Based SLAM," in IEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925. A = −R 𝜃 R 𝜃 (R 𝜃 (R 𝜃 )) (𝒕𝒋 − 𝒕𝒊) 𝟎 -1 B = R 𝜃 R 𝜃 𝟎 𝟎 1
- 18. Graph Based SLAM Algorithm* *G.Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, "A Tutorial on Graph-Based SLAM," in IEEE 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. Stachniss and W. Burgard, "A Tutorial on Graph-Based SLAM," in IEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925.
- 20. Graph Based SLAM Howto fix first node? *G. Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, "A Tutorial on Graph-Based SLAM," in IEEE 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.