Scan Registration for Autonomous Mining Vehicles Using 3D-NDT

Nagoya UniversityParallel & Distributed Systems Lab.

論⽂紹介 (2016.4.27)

Scan Registration for Autonomous

Mining Vehicles Using 3D-NDT

橘川雄樹[email protected]

名古屋⼤学⼤学院情報科学研究科情報システム学専攻枝廣・加藤研究室博⼠後期課程1年

1


論⽂について

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


章構成

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


章構成

1. Introduction


1. ICP

2. 2D-NDT


3. 3D-NDT


5. Experiments


4


Introduction

レンジセンサ(レーザスキャナ)を⽤いたトンネルの計測・3次元地図作成

ü計測装置が⾼価

ü時間がかかる

ü作業員の健康問題

安価、⾼速、無⼈での計測⼿法が必要 → 3D-NDTを⽤いた地図作成

5

ドリルが取り付けられた⾞両http://www.rockdrillsales.com/atlas-copco-distributor/


章構成

1. Introduction


1. ICP

2. 2D-NDT


3. 3D-NDT


5. Experiments


6


Existing Scan Registration Algorithms

Registration (位置合わせ、重ね合わせ)

2つのデータ(点群など)がきれいに重なるように、⽚⽅のデータを移動すること

位置合わせのアルゴリズムØICP

Ø2D-NDT

ØDistance Function

Ø3D-NDT

7

2つのスキャンデータの位置合わせhttp://pointclouds.org/documentation/tutorials/interactive_icp.php


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.

𝐸 𝑅, 𝑇 = &&𝑤()

*+

),-

*.

(,-

‖𝒎𝒊 − 𝑅𝒅𝒋 + 𝑇 67

並進:T回転:R

‖𝒎𝒊 − 𝑅𝒅𝒋 + 𝑇 67Data（スキャン）

Model（地図)


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


2D-NDT (Normal Distributions Transform)

Normal Distributions Transform

-モデル(地図)点群を正規分布(平均・分散)で表現-ICPと⽐較して、メモリ使⽤量少

10

室内における2DレーザースキャンのNDT



11

1. モデルを⼀定の⼤きさのをセルに分割

2. 各セルの平均・分散を計算

平均

分散

𝒒 =1𝑛&𝒙𝒌

@

A,-

𝑪 =1

𝑛 − 1& 𝒙𝒌 − 𝒒 (𝒙𝒌 − 𝒒)E@

A,-

確率密度関数(PDF*)

𝑝 𝒙 =1𝑐 exp −

𝒙 − 𝒒 E𝑪K- 𝒙 − 𝒒2

* Probability Density Function

セル(NDボクセル)



（tx,ty,tz,α,β,γ）

3. ⼊⼒スキャンの各点に対応する要素を求める

4. 評価値を計算

5. ニュートン法により、⼊⼒スキャンの座標変換値を更新

6. 3-5を収束するまで繰り返し

評価関数 𝑠 𝒑 = −&𝑝 𝑇(𝒑, 𝒙A)@

A,-



（tx,ty,tz,α,β,γ）

計算量: スキャンデータに依存（地図データに依存しない）Takeuchi Eijiro, and Takashi Tsubouchi. "A 3-D scan matching using improved 3-D normal distributions transform for mobile robotic mapping." Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on. IEEE, 2006.

3. ⼊⼒スキャンの各点に対応する要素を求める

4. 評価値を計算

5. ニュートン法により、⼊⼒スキャンの座標変換値を更新

6. 3-5を収束するまで繰り返し

評価関数 𝑠 𝒑 = −&𝑝 𝑇(𝒑, 𝒙A)@

A,-



マッチングのパラメータ 𝒑 = 𝑡P,𝑡Q,𝜙

2次元座標変換 𝑇S 𝒑, 𝒙 = cos𝜙 sin𝜙−sin𝜙 cos𝜙 𝒙 +

𝑡P𝑡Q

点の集合 𝜒 = 𝑥-,… , 𝑥@ に 𝒑を適⽤して座標変換した時のスコア

𝑠 𝒑 = −& 𝑝 𝑇 𝒑,𝒙A

@

A,-

𝒑をニュートン法を⽤いて算出

* ニュートン法の計算⽅法については省略

14


章構成

1. Introduction


1. ICP

2. 2D-NDT


3. 3D-NDT


5. Experiments


15


3D-NDT

3次元の座標変換 –並進・回転軸・回転⾓の7パラメータ

𝑇\ 𝒑, 𝒙 =𝑒𝑟P7 + 𝑐 𝑒𝑟P𝑟Q − 𝑠𝑟_ 𝑒𝑟P𝑟_ + 𝑠𝑟Q

𝑒𝑟P𝑟Q + 𝑠𝑟_ 𝑒𝑟Q7 + 𝑐 𝑒𝑟Q𝑟_ − 𝑠𝑟P𝑒𝑟P𝑟_ − 𝑠𝑟Q 𝑒𝑟Q𝑟_ + 𝑠𝑟P 𝑒𝑟_7 + 𝑐

𝑥 +𝑡P𝑡Q𝑡_

𝒑 = 𝒕|𝒓|𝜙 , 𝑡 = 𝑡P,𝑡Q, 𝑡_ , 𝑟 = 𝑟P, 𝑟Q, 𝑟_

𝑠 = sin𝜙, 𝑐 = cos𝜙, 𝑒 = 1− cos𝜙 ,𝜙:回転⾓

*この回転⾏列を⽤いた場合のニュートン法の計算についても省略

16

トンネルにおけるスキャンデータのNDTセルの1辺 : 1m


章構成

1. Introduction


1. ICP

2. 2D-NDT


3. 3D-NDT


5. Experiments


17


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: 離散化


Sampling Method

スキャンデータのサブサンプリング →計算コスト削減

p Uniformly Random Sampling -ランダムに点を削減

元の点群の分布を維持 (センサから遠い点がさらに減少)

p Spatially Distributed Sampling -セル内の点を除去

空間的に⼀様になるように間引く

ü VoxelGrid Filter -セル内の点群の重⼼に置き換え

p 法線ベクトルを考慮したサブサンプリング

19


Cell Size

20

細かい特徴を表現可能細かい特徴を平滑化

セルサイズ⼩ (2×2) ⼤ (1×1)

マッチング精度⾼低

メモリ使⽤量⼤⼩

適切なセルサイズはスキャンの形状に依存


Discretization Method

固定セルと可変セル

固定セル -適切なセルサイズの選択が必要

可変セル(セル細分化) -細かい形状の表現、精度向上

様々なセル構造

ü Fixed Subdivision -固定セル

ü Octree Subdivision

ü Additive Subdivision 可変セル

ü Iterative Subdivision

ü Linked Cells and Infinite Outer Bounds -セルの隣接について

21


Fixed Subdivision, Octree Subdivision

Fixed Subdivision

ü オーバーヘッドが少ないü 各スキャン点に対応するセル探索が⾼速

Octree Subdivision

ü 地図空間全体をルートとし、再帰的に分割

ü 細かい形状を表現可能かつ平坦な部分は⼤きいセルで表現

22

https://ja.wikipedia.org/wiki/⼋分⽊


Additive Subdivision

Additive Subdivision

ü Octree Subdivisionの改良(同じセルにスキャン点が存在しない場合の対処)

ü 各スキャン点のスコアを全ての⼦ノードを⽤いて計算ü ⼦セルサイズだけでなく、親セルサイズも⽤いる

23

スキャン点 x のスコアの計算⽅法Octree Subdivision:セルgの分布のみを⽤いて計算Additive Subdivision: aの⼦セル(b-i)の分布の合計から計算


Iterative Subdivision

NDTの反復試⾏

⼤きいセルサイズ -⼤域的に(おおまかに)マッチング

⼩さいセルサイズ –より細かくマッチング(⼩さいセルサイズのNDTから⼤きいサイズのNDTは導出可能)

Takeuchiらの⼿法収束モード -センサーの近傍 -セルサイズ⼩、遠⽅ -セルサイズ⼤

(センサーから遠い部分は、回転誤差の影響を⼤きく受けるため)微調整モード -全て⼩さいセルを利⽤

24

収束モード微調整モードnear far near far

scanner


Linked Cells and Infinite Outer Bounds

Linked Cell -セルの影響範囲を拡張スキャン点が属するセルに地図がない場合、最も近いセルを使⽤点が存在しないセル内のスキャンもマッチングに寄与

25

During the preparation of this paper, Takeuchiand Tsubouchi presented another way of using NDTfor 3D scan registration !Takeuchi & Tsubouchi,2006". Their implementation is rather similar to theversion described in this paper in that they also usean iterative subdivision scheme similar to that de-scribed here. An important difference is that theyused smaller cells in the space that is near the sensorlocation and larger cells farther away in the earlyiterations, and used only the smaller size in the lateriterations, when the scans were almost aligned. Thereasoning behind this is that error in the rotationestimate caused larger displacements further fromthe sensor location, so larger cells are needed thereto make sure that more points from the data scan areused. The linked cells strategy described in Section4.3.5 is another solution to the same problem. Takeu-chi and Tsubouchi tested their algorithm on datafrom a computer lab with good results, though theydid not make a direct comparison of their algorithmwith other registration algorithms.

4.3.5. Linked Cells and Infinite Outer BoundsUsing the discretization methods described so far,points from the data scan lying in unoccupied cellsare discarded, thus rendering large parts of the inputspace “dead.” Instead of doing so, the PDF from theclosest occupied cell can be used for those points.

This increases the region of influence of cells and isillustrated in Figure 5. Even though the value of thePDF of many cells is almost zero outside the cellbounds, so that it makes no substantial contributionto the score anyway, for cells with a very elongatedpoint distribution, the influence outside the cell canalso make a difference.

The same idea can also be applied to points fall-ing outside of the cell lattice altogether. The score forthose points can be computed using the closest cellon the edge of the lattice, so that the outer cells ineffect have infinite outer bounds. However, doing sointroduces a certain “drag” bias, as points from non-overlapping regions of the data scan will be at-tracted to border regions of the model.

Linked cells can be implemented either by let-ting each cell store a pointer to the nearest occupiedcell, or by storing only occupied cells and puttingthem in a kd-tree. The latter should be preferable ifthere are many unoccupied cells.

5. EXPERIMENTS

This section covers experiments performed with un-derground mine data to compare the performance ofdifferent varieties of 3D-NDT and ICP.

Figure 4. Comparing octree and additive subdivision. Asubdivided grid cell is shown on the left, and the treestructure is shown on the right. The PDF of cell a has alarge spread, because the points within the cell are notaligned along a planar region. Therefore it is split, and thePDFs of eight subregions b– i are computed instead. Pointx is within cell a, and, more specifically, within subcell g.Using octree subdivision, x’s contribution to the scorefunction is computed from g alone. Using additive subdi-vision, the score is a sum computed from nodes b– i. Inthis example, nodes b–e are empty and will not add any-thing to the score.

Figure 5. Matching two 2D scans of a tunnel section. Thedotted scan is being registered to the solid scan. Occupiedcells are shaded. If linked cells are not used, the parts ofthe scan that are in unshaded cells will be skipped. Other-wise the linked cell !shown with arrows" will be used. Ifusing infinite outer bounds, the outer cells extend asshown with dashed lines.

812 • Journal of Field Robotics—2007

Journal of Field Robotics DOI 10.1002/rob

別のセルに格納


章構成

1. Introduction


1. ICP

2. 2D-NDT


3. 3D-NDT


5. Experiments


26


Experiments

デフォルト(Baseline)パラメータ、設定

ICP

• モデルとスキャンの対応点ユークリッド距離を最⼩化• 外れ値除去のしきい値: 1m

• 最⼩⼆乗法を⽤いた最適化• 最近傍点探索にはkd-treeを使⽤• 対応点は全て同じ重み

NDT

• セルサイズは1m

• ニュートン法による直線探索のステップ幅 0.05 (|Δp|=0.05)

ICP,NDT共通• 収束判定 |p|<= 0.0001

27


Experiments

Results with Single Scan Pairs – 2つのスキャンデータのみの評価

Ø スキャンデータのサンプルレートの影響

Ø スキャンデータのサンプリング⽅法の影響

Ø モデル(地図)のセルサイズの影響

Ø 初期並進誤差の影響

Ø 初期回転誤差の影響

Ø NDTの格⼦構造 (Discretization Method) の影響

28


Data

Junctionデータü ⾏き⽌まり、横に抜ける通り道があるü 同じ位置、姿勢での2スキャン

Tunnelデータü 4m離れての2スキャンü 壁、天井のみ(特徴が少ない)

29

Data 1: Junction データセット Data 2: Tunnel データセットOptabスキャナ


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データセットにおいて、変更されたパラメータ


Results (Results with Single Scan Pairs) – Sample Ratio

31

この線以下だったらマッチング成功と判断

並進誤差

回転誤差

計算時間

上位四分位(75%)

下位四分位(25%)

中央値

最⼩値

Per mille:

ICPはサンプルレートが低い場合においても誤差⼩

ICPの計算時間はNDTの約3倍

Junction データにおけるサンプルレートの影響



ü ICPはサンプルレートが低い場合においても誤差⼩

ü ICPの計算時間はNDTの約3倍

ü NDTはどのような初期位置でも成功するが、サンプルレートが低い場合外れる

ü Junction データセットの場合、10%のスキャンデータで⼗分

ü NDT サンプルレート12%まで失敗がある

ü ICPの場合、8%で⼗分

ü サンプルレートが20%より⼤きくなると回転誤差が⼤きくなる(ノイズに引っ張られる、オーバーフィッティング)

32



33

並進誤差

回転誤差

計算時間

Tunnel データにおけるサンプルレートの影響



ü 中央値誤差はNDTの⽅が⼩さいが、初期位置誤差1m、初期回転誤差が0.1radを超えた場合、マッチング失敗

34



35

Tunnel データにおけるサンプルレートの影響 (Uniform Random Samplingを⽤いた場合)

Spatially Distributed Samplingを⽤いた⽅が、中央値誤差⼩



ü 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


Results (Results with Single Scan Pairs) – Cell Size

37

Cell Sizeの影響

セルサイズが⼤きいほど、計算時間少ない

セルサイズ 1m-2m程度が適切


Results (Results with Single Scan Pairs) – Cell Size

ü 計算時間 -セルサイズが⼤きい(セル数が少ない)⽅が早い

ü セルサイズ⼩: マッチングに⼗分な点を引きつけられない

ü セルサイズ⼤: 表⾯の形状が失われてしまう

ü セルサイズ - 1-2mが適切

38


Results (Results with Single Scan Pairs) – Initial Error

39

Junctionデータにおける初期並進誤差の影響(どの程度初期値がずれていても正しくマッチングするか)

3D-NDT: より⼩さい誤差でマッチング失敗

計算時間: ICP-増加 NDT-⼀定



40

Junctionデータにおける初期回転誤差の影響(どの程度初期値がずれていても正しくマッチングするか)

3D-NDT: より⼩さい誤差でマッチング失敗

計算時間: ICP-増加 NDT-⼀定



ü 3D-NDTは多くのケースで中央値誤差が⼩さいが、ICPより⼩さい初期並進誤差でマッチングできないことがある

ü Junction データ、デフォルト(baseline)のパラメータでは,

• ICP: 初期並進誤差-2.5m 初期回転誤差 - 0.35radまで

• 3D-NDT: 初期並進誤差 2m、初期回転誤差0.3radまで

正しくマッチング

ü 計算時間: ICPは誤差が⼤きくなると増⼤、NDTは変わらず

41


Results (Results with Single Scan Pairs) – Discretization Methods

42

NDTの格⼦構造 (Discretization Method) の影響



セルサイズ – 2m

Junctionデータでは、同様のパフォーマンス

Junctionデータ: 初期並進誤差 𝑒d = 1m,初期回転誤差 𝑒e = 0.2rad

Tunnelデータ: 初期並進誤差𝑒d = 1m,初期回転誤差 𝑒e = 0.1rad

Octree Subdivision(O,OI)

ü Junctionデータでは、性能向上は⾒られない

ü Tunnelデータでは、中央値誤差が約半分になった

43



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



Using linked cells

ü Tunnelデータ、回転についてはわずかな性能向上

ü Junctionデータでは、あまり変わらず

Iterative subdivision with infinite outer boundsが1番良い

他の3D-NDTよりかは少し時間がかかるが、ICPよりかは早い

45


Experiment

Results with Mobile Robot Data –複数のスキャンデータ間の評価

Ø モデル(地図)のセルサイズの影響

Ø NDTの格⼦構造 (Discretization Method) の影響 (Octree or Iterative)

Ø Infinite Boundsの影響

Ø NDT、ICPの⽐較

46


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搭載オドメトリの誤差



ü スキャンデータ –約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

モデル(地図)のセルサイズの影響

上位四分位(75%)

下位四分位(25%)

中央値

50回のマッチングの分布



ü ⼩さいセル -オドメトリが実際の位置と離れている時、失敗

ü ⼤きいセル -マッチングに必要な細かい特徴がなくなってしまい失敗

ü 2mがベスト

ü 回転は位置よりマッチングしやすい、おおまかな特徴で⼗分正しい値が求まる

50



51

NDTの格⼦構造の影響(Octree, Iterative)

Iterativeが最も良い



Octree Subdivision – Fixed Cellより性能向上

Additive subdivision - Octreeと変わらず

Iterative Subdivision – 48/50成功

3回のマッチングをしているため、他の3D-NDTより計算時間増加

計算時間の増加は他の2倍

52



53

Infinite Outer Boundsの影響



Infinite Outer Boundsの影響

ü Linked cellは性能向上にあまりつながらない

ü Infinite boundsはいくらか有効

54



55

NDT, ICPの⽐較



ICPとの⽐較

ü Outlier Rejection 2mから0mに変化

ü 最も⼤きな違いは計算時間、3D-NDTの約3倍

56


章構成

1. Introduction


1. ICP

2. 2D-NDT


3. 3D-NDT


5. Experiments


57


Summary and Conclusions

ü 3次元スキャンの位置合わせ⼿法(3D-NDT)を提案

ü 実環境（坑道）でのデータを⽤いて、ICPと⽐較

ICPと⽐較して、NDTの優れている点

Ø ⾼速（最近傍点探索がないため）

Ø 省メモリ

Ø ⾼精度（Iterative Subdivision, Infinite Outer Boundsを⽤いる）

58

Engineering

Scan Registration for Autonomous Mining Vehicles Using 3D-NDT