研究室のゼミの論文紹介の発表資料です。
Magnusson, M., Lilienthal, A. and Duckett, T. (2007), Scan registration for autonomous mining vehicles using 3D-NDT. J. Field Robotics, 24: 803–827. doi: 10.1002/rob.20204
Scan Registration for Autonomous Mining Vehicles Using 3D-NDT
1. Nagoya University
Parallel & Distributed Systems Lab.
論⽂紹介 (2016.4.27)
Scan Registration for Autonomous
Mining Vehicles Using 3D-NDT
橘川 雄樹
yuki@ertl.jp
名古屋⼤学 ⼤学院
情報科学研究科 情報システム学専攻
枝廣・加藤研究室 博⼠後期課程1年
1
2. Nagoya University
Parallel & Distributed Systems Lab.
論⽂について
p タイトル
Scan Registration for Autonomous Mining Vehicles Using 3D-NDT
p 著者
Martin Magnusson, Achim Lilienthal, Tom Duckett
p 出典
Journal of Field Robotics, 2007
p 選定理由
üNDTのアルゴリズム、性能の調査
üICPとNDTの⽐較
ü評価⽅法の調査
論⽂URL - http://onlinelibrary.wiley.com/doi/10.1002/rob.20204/epdf
2
3. Nagoya University
Parallel & Distributed Systems Lab.
章構成
1. Introduction
2. Existing Scan Registration Algorithms
1. ICP
2. 2D-NDT
3. Registration with Approximants to the Distance Function
3. 3D-NDT
4. Alternative Methods Implemented
5. Experiments
6. Summary and Conclusions
3
4. Nagoya University
Parallel & Distributed Systems Lab.
章構成
1. Introduction
2. Existing Scan Registration Algorithms
1. ICP
2. 2D-NDT
3. Registration with Approximants to the Distance Function
3. 3D-NDT
4. Alternative Methods Implemented
5. Experiments
6. Summary and Conclusions
4
5. Nagoya University
Parallel & Distributed Systems Lab.
Introduction
レンジセンサ(レーザスキャナ)を⽤いたトンネルの計測・3次元地図作成
ü計測装置が⾼価
ü時間がかかる
ü作業員の健康問題
安価、⾼速、無⼈での計測⼿法が必要 → 3D-NDTを⽤いた地図作成
5
ドリルが取り付けられた⾞両
http://www.rockdrillsales.com/atlas-copco-distributor/
6. Nagoya University
Parallel & Distributed Systems Lab.
章構成
1. Introduction
2. Existing Scan Registration Algorithms
1. ICP
2. 2D-NDT
3. Registration with Approximants to the Distance Function
3. 3D-NDT
4. Alternative Methods Implemented
5. Experiments
6. Summary and Conclusions
6
7. Nagoya University
Parallel & Distributed Systems Lab.
Existing Scan Registration Algorithms
Registration (位置合わせ、重ね合わせ)
2つのデータ(点群など)がきれいに重なるように、
⽚⽅のデータを移動すること
位置合わせのアルゴリズム
ØICP
Ø2D-NDT
ØDistance Function
Ø3D-NDT
7
2つのスキャンデータの位置合わせ
http://pointclouds.org/documentation/tutorials/interactive_icp.php
8. Nagoya University
Parallel & Distributed Systems Lab.
ICP (Iterative Closest Point)
アルゴリズム
1. 2つのスキャンの最近傍点(Nearest Neighbor)を求める
2. NN間の距離の和を最⼩化
8
P. J. Besl and H. D. McKay, "A method for registration of 3-D shapes,”
in IEEE Transactions on Pattern Analysis and Machine Intelligence,vol. 14, no. 2, pp. 239-256,Feb 1992.
𝐸 𝑅, 𝑇 = & & 𝑤()
*+
),-
*.
(,-
‖ 𝒎 𝒊 − 𝑅𝒅 𝒋 + 𝑇 6
7
並進:T 回転:R
‖ 𝒎 𝒊 − 𝑅𝒅 𝒋 + 𝑇 6
7Data(スキャン)
Model(地図)
9. Nagoya University
Parallel & Distributed Systems Lab.
ICP (Iterative Closest Point)
ICPの問題点
Ø⾯の形状を利⽤しない
Ø最近傍点探索に時間がかかる
- KD-Tree(空間分割データ構造)を利⽤した探索の⾼速化*
* M. Greenspan and M. Yurick, "Approximate k-d tree search for efficient ICP," 3-D Digital Imaging and Modeling, 2003.
3DIM 2003.Proceedings. Fourth International Conference on, 2003,pp. 442-448.
9
10. Nagoya University
Parallel & Distributed Systems Lab.
2D-NDT (Normal Distributions Transform)
Normal Distributions Transform
-モデル(地図)点群を正規分布(平均・分散)で表現
-ICPと⽐較して、メモリ使⽤量 少
10
室内における2DレーザースキャンのNDT
11. Nagoya University
Parallel & Distributed Systems Lab.
2D-NDT (Normal Distributions Transform)
11
1. モデルを⼀定の⼤きさのをセルに分割
2. 各セルの平均・分散を計算
平均
分散
𝒒 =
1
𝑛
& 𝒙 𝒌
@
A,-
𝑪 =
1
𝑛 − 1
& 𝒙 𝒌 − 𝒒 (𝒙 𝒌 − 𝒒)E
@
A,-
確率密度関数(PDF*)
𝑝 𝒙 =
1
𝑐
exp −
𝒙 − 𝒒 E
𝑪K-
𝒙 − 𝒒
2
* Probability Density Function
セル(NDボクセル)
15. Nagoya University
Parallel & Distributed Systems Lab.
章構成
1. Introduction
2. Existing Scan Registration Algorithms
1. ICP
2. 2D-NDT
3. Registration with Approximants to the Distance Function
3. 3D-NDT
4. Alternative Methods Implemented
5. Experiments
6. Summary and Conclusions
15
17. Nagoya University
Parallel & Distributed Systems Lab.
章構成
1. Introduction
2. Existing Scan Registration Algorithms
1. ICP
2. 2D-NDT
3. Registration with Approximants to the Distance Function
3. 3D-NDT
4. Alternative Methods Implemented
5. Experiments
6. Summary and Conclusions
17
18. Nagoya University
Parallel & Distributed Systems Lab.
Alternative Methods Implemented
3D-NDTの実装法とパラメータについて
Ø Sampling Method - スキャンデータの間引き⽅について
Ø Cell Size - モデル(地図データ)のNDTのサイズについて
Ø Discretization Methods – NDTの格⼦構造について
ü Fixed Subdivision
ü Octree Subdivision
ü Additive Subdivision
ü Iterative Subdivision
ü Linked Cells and Infinite Outer Bounds
18
* Descretization:離散化
19. Nagoya University
Parallel & Distributed Systems Lab.
Sampling Method
スキャンデータのサブサンプリング → 計算コスト削減
p Uniformly Random Sampling - ランダムに点を削減
元の点群の分布を維持 (センサから遠い点がさらに減少)
p Spatially Distributed Sampling - セル内の点を除去
空間的に⼀様になるように間引く
ü VoxelGrid Filter - セル内の点群の重⼼に置き換え
p 法線ベクトルを考慮したサブサンプリング
19
20. Nagoya University
Parallel & Distributed Systems Lab.
Cell Size
20
細かい特徴を表現可能 細かい特徴を平滑化
セルサイズ ⼩ (2×2) ⼤ (1×1)
マッチング精度 ⾼ 低
メモリ使⽤量 ⼤ ⼩
適切なセルサイズはスキャンの形状に依存
21. Nagoya University
Parallel & Distributed Systems Lab.
Discretization Method
固定セルと可変セル
固定セル - 適切なセルサイズの選択が必要
可変セル(セル細分化) - 細かい形状の表現、精度向上
様々なセル構造
ü Fixed Subdivision - 固定セル
ü Octree Subdivision
ü Additive Subdivision 可変セル
ü Iterative Subdivision
ü Linked Cells and Infinite Outer Bounds - セルの隣接について
21
22. Nagoya University
Parallel & Distributed Systems Lab.
Fixed Subdivision, Octree Subdivision
Fixed Subdivision
ü オーバーヘッドが少ない
ü 各スキャン点に対応するセル探索が⾼速
Octree Subdivision
ü 地図空間全体をルートとし、
再帰的に分割
ü 細かい形状を表現可能かつ
平坦な部分は⼤きいセルで表現
22
https://ja.wikipedia.org/wiki/⼋分⽊
23. Nagoya University
Parallel & Distributed Systems Lab.
Additive Subdivision
Additive Subdivision
ü Octree Subdivisionの改良
(同じセルにスキャン点が存在しない場合の対処)
ü 各スキャン点のスコアを全ての⼦ノードを⽤いて計算
ü ⼦セルサイズだけでなく、親セルサイズも⽤いる
23
スキャン点 x のスコアの計算⽅法
Octree Subdivision: セルgの分布のみを⽤いて計算
Additive Subdivision: aの⼦セル(b-i)の分布の合計から計算
24. Nagoya University
Parallel & Distributed Systems Lab.
Iterative Subdivision
NDTの反復試⾏
⼤きいセルサイズ - ⼤域的に(おおまかに)マッチング
⼩さいセルサイズ – より細かくマッチング
(⼩さいセルサイズのNDTから⼤きいサイズのNDTは導出可能)
Takeuchiらの⼿法
収束モード - センサーの近傍 - セルサイズ⼩、遠⽅ - セルサイズ⼤
(センサーから遠い部分は、回転誤差の影響を⼤きく受けるため)
微調整モード - 全て⼩さいセルを利⽤
24
収束モード 微調整モード
near far near far
scanner
25. Nagoya University
Parallel & Distributed Systems Lab.
Linked Cells and Infinite Outer Bounds
Linked Cell - セルの影響範囲を拡張
スキャン点が属するセルに地図がない場合、最も近いセルを使⽤
点が存在しないセル内のスキャンもマッチングに寄与
25
dditive subdivision. A
the left, and the tree
he PDF of cell a has a
within the cell are not
efore it is split, and the
mputed instead. Point
cally, within subcell g.
ribution to the score
Using additive subdi- Figure 5. Matching two 2D scans of a tunnel section. The
—2007
別のセルに格納
26. Nagoya University
Parallel & Distributed Systems Lab.
章構成
1. Introduction
2. Existing Scan Registration Algorithms
1. ICP
2. 2D-NDT
3. Registration with Approximants to the Distance Function
3. 3D-NDT
4. Alternative Methods Implemented
5. Experiments
6. Summary and Conclusions
26
28. Nagoya University
Parallel & Distributed Systems Lab.
Experiments
Results with Single Scan Pairs – 2つのスキャンデータのみの評価
Ø スキャンデータのサンプルレートの影響
Ø スキャンデータのサンプリング⽅法の影響
Ø モデル(地図)のセルサイズの影響
Ø 初期並進誤差の影響
Ø 初期回転誤差の影響
Ø NDTの格⼦構造 (Discretization Method) の影響
28
29. Nagoya University
Parallel & Distributed Systems Lab.
Data
Junctionデータ
ü ⾏き⽌まり、横に抜ける通り道がある
ü 同じ位置、姿勢での2スキャン
Tunnelデータ
ü 4m離れての2スキャン
ü 壁、天井のみ(特徴が少ない)
29
Data 1: Junction データセット Data 2: Tunnel データセットOptabスキャナ
30. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs)
NDTとICPの性能⽐較
• スキャンデータを1/10にサンプリング(Spatial Distributed Sampling)
• モデル(地図データ)はサンプリングなし
• 初期並進誤差 1m
• 初期回転誤差 0.1rad
• それぞれのパラメータにおいて、100回試⾏
30
Parameter ICP NDT
Sample Ratio ✓ ✓
Sampling Method ✓ ✓
Initial Translation Error ✓ ✓
Initial Rotation Error ✓ ✓
Cell Size - ✓
Descretization Method - ✓
Junction,Tunnelデータセットにおいて、変更されたパラメータ
31. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Sample Ratio
31
この線以下だったら
マッチング成功と判断
並進誤差回転誤差計算時間
上位四分位(75%)
下位四分位(25%)
中央値
最⼩値
Pe
ICPはサンプルレートが低い場合においても誤差⼩
ICPの計算時間はNDTの約3倍
Junction データにおけるサンプルレートの影響
32. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Sample Ratio
ü ICPはサンプルレートが低い場合においても誤差⼩
ü ICPの計算時間はNDTの約3倍
ü NDTはどのような初期位置でも成功するが、サンプルレートが低い場
合外れる
ü Junction データセットの場合、10%のスキャンデータで⼗分
ü NDT サンプルレート12%まで失敗がある
ü ICPの場合、8%で⼗分
ü サンプルレートが20%より⼤きくなると回転誤差が⼤きくなる
(ノイズに引っ張られる、オーバーフィッティング)
32
33. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Sample Ratio
33
並進誤差回転誤差計算時間
Tunnel データにおけるサンプルレートの影響
34. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Sample Ratio
ü 中央値誤差はNDTの⽅が⼩さいが、初期位置誤差1m、初期回転誤差
が0.1radを超えた場合、マッチング失敗
34
35. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Sample Ratio
35
Tunnel データにおけるサンプルレートの影響 (Uniform Random Samplingを⽤いた場合)
Spatially Distributed Samplingを
⽤いた⽅が、中央値誤差⼩
36. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Sample Ratio
ü Uniform Random Samplingはスキャンの形状を維持 → センサー付近
のデータが密
ü 3D-NDT、Uniform Random Samplingを⽤いた場合の⽅が誤差⼤
ü 3D-NDTの場合、四分位数範囲(25%-75%の範囲)が⼤きい(どちらの
Sampling Methodの場合)が、並進・回転の中央値誤差はSpatially
Distributed Samplingを⽤いた場合の⽅が⼩さい
ü Junction データ - Uniform Random SamplingとSpatially Distributed
Samplingと変わらない、Overlap100%
36
37. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Cell Size
37
Cell Sizeの影響
セルサイズが⼤きいほど、計算時間少ない
セルサイズ 1m-2m程度が適切
38. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Cell Size
ü 計算時間 - セルサイズが⼤きい(セル数が少ない)⽅が早い
ü セルサイズ⼩: マッチングに⼗分な点を引きつけられない
ü セルサイズ⼤: 表⾯の形状が失われてしまう
ü セルサイズ - 1-2mが適切
38
39. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Initial Error
39
Junctionデータにおける初期並進誤差の影響(どの程度初期値がずれていても正しくマッチングするか)
3D-NDT: より⼩さい誤差でマッチング失敗
計算時間: ICP-増加 NDT-⼀定
40. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Initial Error
40
Junctionデータにおける初期回転誤差の影響(どの程度初期値がずれていても正しくマッチングするか)
3D-NDT: より⼩さい誤差でマッチング失敗
計算時間: ICP-増加 NDT-⼀定
41. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Initial Error
ü 3D-NDTは多くのケースで中央値誤差が⼩さいが、ICPより⼩さい初期
並進誤差でマッチングできないことがある
ü Junction データ、デフォルト(baseline)のパラメータでは,
• ICP: 初期並進誤差-2.5m 初期回転誤差 - 0.35radまで
• 3D-NDT: 初期並進誤差 2m、初期回転誤差0.3radまで
正しくマッチング
ü 計算時間: ICPは誤差が⼤きくなると増⼤、NDTは変わらず
41
42. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Discretization Methods
42
NDTの格⼦構造 (Discretization Method) の影響
43. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Discretization Methods
セルサイズ – 2m
Junctionデータでは、同様のパフォーマンス
Junctionデータ: 初期並進誤差 𝑒d = 1m,初期回転誤差 𝑒e = 0.2rad
Tunnelデータ: 初期並進誤差𝑒d = 1m,初期回転誤差 𝑒e = 0.1rad
Octree Subdivision(O,OI)
ü Junctionデータでは、性能向上は⾒られない
ü Tunnelデータでは、中央値誤差が約半分になった
43
44. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Discretization Methods
Additive octree subdivision(A, AI)
ü Tunnelデータにおいては、わずかに性能向上(計算の時間の増加は
わずか)
ü Junctionデータでは、100回中2回マッチング失敗
Iterative subdivision with varying cell size(I,II)
ü Junctionデータでは、全てのマッチングが成功 (計算時間増)
ü iterative subdivisionとadditive subdivision、Tunnelデータで唯⼀
75%以上の初期位置で正しくマッチングできた
ü セルサイズ 2m → 1.5m → 1.125m (×0.75)
44
45. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Single Scan Pairs) – Discretization Methods
Using linked cells
ü Tunnelデータ、回転についてはわずかな性能向上
ü Junctionデータでは、あまり変わらず
Iterative subdivision with infinite outer boundsが1番良い
他の3D-NDTよりかは少し時間がかかるが、ICPよりかは早い
45
46. Nagoya University
Parallel & Distributed Systems Lab.
Experiment
Results with Mobile Robot Data – 複数のスキャンデータ間の評価
Ø モデル(地図)のセルサイズの影響
Ø NDTの格⼦構造 (Discretization Method) の影響 (Octree or Iterative)
Ø Infinite Boundsの影響
Ø NDT、ICPの⽐較
46
47. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Mobile Robot Data)
Kvarntorp-loopデータセット
ü 移動ロボット(SICK LMS 200 + 2次元オドメトリ)
ü 4-5mおきにスキャン(1回あたり約95,000ポイント)
ü オドメトリの誤差⼤(約1.5m, 0.2rad)
47
Data 3: Kvarntorp-Loop データセットTjorven – SICK LMS 200搭載 オドメトリの誤差
48. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Mobile Robot Data)
ü スキャンデータ – 約8,000ポイント
ü モデル - 全ての(約95,000)ポイント
ü Infinite outer boundsを⽤いる
マッチングの成功判定
Ground Truth - 何回か位置合わせのベストマッチの平均
Good – Ground Truthから0.10m, 0.005rad以内
Acceptable – Ground Truthから0.20, 0.010rad以内
48
49. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Mobile Robot Data)
49
モデル(地図)の
セルサイズの影響
上位四分位(75%)
下位四分位(25%)
中央値
50回の
マッチングの
分布
50. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Mobile Robot Data)
ü ⼩さいセル - オドメトリが実際の位置と離れている時、失敗
ü ⼤きいセル - マッチングに必要な細かい特徴がなくなってしまい失敗
ü 2mがベスト
ü 回転は位置よりマッチングしやすい、おおまかな特徴で⼗分正しい値
が求まる
50
51. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Mobile Robot Data)
51
NDTの格⼦構造の影響(Octree, Iterative)
Iterativeが最も良い
52. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Mobile Robot Data)
Octree Subdivision – Fixed Cellより性能向上
Additive subdivision - Octreeと変わらず
Iterative Subdivision – 48/50成功
3回のマッチングをしているため、他の3D-NDTより計算時間 増加
計算時間の増加は他の2倍
52
53. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Mobile Robot Data)
53
Infinite Outer Boundsの影響
54. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Mobile Robot Data)
Infinite Outer Boundsの影響
ü Linked cellは性能向上にあまりつながらない
ü Infinite boundsはいくらか有効
54
56. Nagoya University
Parallel & Distributed Systems Lab.
Results (Results with Mobile Robot Data)
ICPとの⽐較
ü Outlier Rejection 2mから0mに変化
ü 最も⼤きな違いは計算時間、3D-NDTの約3倍
56
57. Nagoya University
Parallel & Distributed Systems Lab.
章構成
1. Introduction
2. Existing Scan Registration Algorithms
1. ICP
2. 2D-NDT
3. Registration with Approximants to the Distance Function
3. 3D-NDT
4. Alternative Methods Implemented
5. Experiments
6. Summary and Conclusions
57
58. Nagoya University
Parallel & Distributed Systems Lab.
Summary and Conclusions
ü 3次元スキャンの位置合わせ⼿法(3D-NDT)を提案
ü 実環境(坑道)でのデータを⽤いて、ICPと⽐較
ICPと⽐較して、NDTの優れている点
Ø ⾼速(最近傍点探索がないため)
Ø 省メモリ
Ø ⾼精度(Iterative Subdivision, Infinite Outer Boundsを⽤いる)
58