gggggraziegrazie

graizegrazieさんのやったこと、学んだことを記録する雑記帳です

NDT( Normal Distributions Transform )

 NDT[1]とは、2つの点群のマッチングをさせる手法の1つです。点群マッチングとは、ある点群(参照点群と呼ぶ)に対し、ある点群(入力点群と呼ぶ)がどれだけ重なっているか、重ねるためには点群をどの様に移動させるべきかを判断するための手法です。点群マッチングの手法としては、他にICP(Iterative Closest Point)があります。ただしICPは、入力点群の各点が参照点群に対してどれだけ離れているかを計算する必要があります。そのため、入力点群の数が多くなるほど計算量が増えるという問題があります。
 これに対してNDTは、ボクセルやグリッドを使って点群が存在する空間を等間隔に分割するし、ボクセル又はグリッド毎に求めた正規分布を使って点群の表現、マッチングを行うため、計算量を削減することができます。正規分布の計算方法は下記の通りです。
①各ボクセル又はグリッド毎に、点群の平均座標を求める
②求めた平均座標を使い、共分散行列を求める
③この平均座標と共分散行列を元に、点群を正規分布で表現する

イメージとしては、下記のスライドと画像が参考になるかと思います。
https://camo.qiitausercontent.com/f07fa661da685d0d6f45f1650c7737b2a0b80547/68747470733a2f2f71696974612d696d6167652d73746f72652e73332e616d617a6f6e6177732e636f6d2f302f3133313535382f33333136356235392d653038622d333861622d666338302d3464623362323163613562382e706e67
このスライドはソースはここ
そしてスライドの大元のソースはここ

f:id:graziegrazie:20180701000424p:plain
出典:田窪ら, "高解像度NDTグリッドマップを用いた環境地図生成", 日本機械学会論文集(C編)78巻793号, 2012

点群のマッチングは、

  • p = (t_x, t_y, \theta)^T: 入力点群の座標系から見た参照点群の座標系の相対座標。
  • T(p): 入力点群座標系から参照点群座標系への変換行列
  • x_i: 入力点群中の点iの座標(生データ)。
  • x^{'}_i: 入力点群を参照点群の座標系に変換した後の入力点群中の点iの座標。つまり、 x^{'}_i = T(p) x_i
  • \Sigma_i, q_i: 変換後の入力点群{x^{'}_i}の共分散と平均座標。共分散と平均は各ボクセル又はグリッド毎に計算することに注意。

このとき、下記の関数score(p)を使って点群同士がどれくらいマッチングしているかを評価する。
f:id:graziegrazie:20180701003934p:plain
出典:[1]

SLAM等のアプリケーションでNDTを使い、この評価関数の値が低い場合は、参照点群と入力点群が重なる(つまり地図と現在得た点群がマッチする様に調整し、自己位置を見つける)ように、pの位置を調整していく。調整アルゴリズムについては[1]を参照のこと。