2018.05.27, 3D勉強会@関東 発表資料 LiDAR-SLAM チュートリアル

1. 3D勉強会＠関東発表資料

3. •
•
•
-
SICK LMS291 FARO Focus3D Velodyne HDL64e

4. •
•
•
https://www.geospatialworld.net

5. As-build modeling
Forensic inspection
Autonomous driving
Reverse engineering / Inspection
https://www.faro.com
http://asbuiltmodelingservices.com/ http://eijournal.com/
https://papottigabor.wordpress.com/

6. •
•
http://pointclouds.org

7. •
-
•
-
•

8. •
-
-
𝑹, 𝒕 = argmin 𝑹,𝑻 𝑑 𝑿/, 𝑇 𝑿1; 𝑹, 𝒕

9. •
-
-
•
-
-
•
-
-
•
-
-

10. •
•
𝑗∗ = argmin5 𝑑 𝒑78, 𝑇 𝒑95; 𝑹, 𝒕
𝑹, 𝒕 = argmin 𝑹,𝑻 : 𝑑 𝒑78, 𝑇 𝒑95∗; 𝑹, 𝒕
対応付け
姿勢アップデート
http://www.sanko-shoko.net/note.php?id=3c5m

11. •
•
•
-
-
-
-
https://github.com/komi2/survey/blob/master/01/01/kdtree.md

12. •
𝐸 = : 𝒑8 − (𝑹𝒒8 + 𝒕) 1
8
𝒑B =
1
𝑁
: 𝒑8
8
𝒒B =
1
𝑁
: 𝒒8
8
𝑑𝐸
𝑑𝒕
= : 𝒑8 − 𝑹𝒒8 − 𝒕
8
= : 𝒑8
8
− : 𝑹𝒒8
8
− : 𝒕
8
= 𝑁𝒑B − 𝑹𝑁𝒒B − 𝑁𝒕 = 𝟎
𝒕 = 𝒑B − 𝑹𝒒B
t

13. 𝒑F = 𝒑 − 𝒑B 𝒒F = 𝒒 − 𝒒B
𝐸 = : 𝒑8
F
+ 𝒑B − 𝑹 𝒒8
F
+ 𝒒B − 𝒕 1
8
𝐸 = : 𝒑8′ − 𝑹𝒒8
F 1
8
𝒕 = 𝒑B − 𝑹𝒒B
= : 𝒑8
FH
𝒑8′
8
− 2 : 𝒑8
FH
𝑹𝒒8
F
8
+ : 𝒒8
FH
𝑹H 𝑹𝒒8
F
8
= : 𝒑8
FH
𝒑8′
8
− 2 : 𝒑8
FH
𝑹𝒒8
F
8
+ : 𝒒8
FH
𝒒8
F
8
これを最大化したい定数 定数

14. : 𝒑8
FH
𝑹𝒒8
F
8
= 𝑇𝑟𝑎𝑐𝑒 : 𝑹𝒑8
F
𝒒8
FH
8
= 𝑇𝑟𝑎𝑐𝑒 𝑹𝑯
𝑯 = : 𝒑8
F
𝒒8
FH
8
ただし、
ここで、正定値行列𝑨𝑨H
について、任意の直交行列Bに対し
て、以下が成り立つ（後述）。
𝑇𝑟𝑎𝑐𝑒 𝑨𝑨H
≥ 𝑇𝑟𝑎𝑐𝑒 𝑩𝑨𝑨H
Hを特異値分解し、 𝑯 = 𝑼𝜦𝑽H
回転行列Rを 𝑹 = 𝑽𝑼H と置いた場合、
とし、
𝑇𝑟𝑎𝑐𝑒 𝑹𝑯 = 𝑇𝑟𝑎𝑐𝑒 𝑽𝜦𝑽H
≥ 𝑇𝑟𝑎𝑐𝑒 𝑩𝑽𝜦𝑽H
任意の回転Bを掛けても、𝑹を𝑽𝑼H
R

15. Special case:
データにノイズが大きい場合に、Rが左右反転を含む変換になってし
まい、望ましい回転行列にならず、Rの行列式が-1になる。
この場合、
𝑹 = 𝑽diag(1,1, −1)𝑼H
と置くことで回避できる。
まとめると、
𝑹 = 𝑽diag 1,1, det 𝑽𝑼H 𝑼H
とすることで、常に正しい回転行列が求まる。
𝒕 = 𝒑B − 𝑹𝒒B
tはRを用いて、以下から求める

17. •
-
-
•
-
-
•
-
•
-
-
•
-
-

18. •
•
•
https://www.cc.gatech.edu/~kaess/loops.html

19. •
-
𝐸 = : (𝑹8 𝒑X + 𝒕8) − (𝑹5 𝒒X + 𝒕5)
1
8Y5,X
https://www.maptek.com/

20. R SO(3)であり、パラメータはr1,r2,r3の3つで構成され、
、
•
•

21. パラメータrで回転行列を微分すると・・・
例えばr1の場合は、
のように計算できる。

22. •
-
•
-
•
-

23. •
•
-
-
𝐸 = − : exp − 𝒙8 − 𝝁X 𝜮X 𝒙8 − 𝝁X
H
8
対応するグリッドの正規分布に
対するマハラノビス距離
NDT Map 地図への照合度合い

24. •
•
•
Outlierの確率計測点ijが分布kに属する確率

25. •
•
•

26. •
-
•
•
-
•
•
•
•