$$\def\mA{\mathbf{A}} \def\mB{\mathbf{B}} \def\mC{\mathbf{C}} \def\mD{\mathbf{D}} \def\mE{\mathbf{E}} \def\mF{\mathbf{F}} \def\mG{\mathbf{G}} \def\mH{\mathbf{H}} \def\mI{\mathbf{I}} \def\mJ{\mathbf{J}} \def\mK{\mathbf{K}} \def\mL{\mathbf{L}} \def\mM{\mathbf{M}} \def\mN{\mathbf{N}} \def\mO{\mathbf{O}} \def\mP{\mathbf{P}} \def\mQ{\mathbf{Q}} \def\mR{\mathbf{R}} \def\mS{\mathbf{S}} \def\mT{\mathbf{T}} \def\mU{\mathbf{U}} \def\mV{\mathbf{V}} \def\mW{\mathbf{W}} \def\mX{\mathbf{X}} \def\mY{\mathbf{Y}} \def\mZ{\mathbf{Z}} \def\va{\mathbf{a}} \def\vb{\mathbf{b}} \def\vc{\mathbf{c}} \def\vd{\mathbf{d}} \def\ve{\mathbf{e}} \def\vf{\mathbf{f}} \def\vg{\mathbf{g}} \def\vh{\mathbf{h}} \def\vi{\mathbf{i}} \def\vj{\mathbf{j}} \def\vk{\mathbf{k}} \def\vl{\mathbf{l}} \def\vm{\mathbf{m}} \def\vn{\mathbf{n}} \def\vo{\mathbf{o}} \def\vp{\mathbf{p}} \def\vq{\mathbf{q}} \def\vr{\mathbf{r}} \def\vs{\mathbf{s}} \def\vt{\mathbf{t}} \def\vu{\mathbf{u}} \def\vv{\mathbf{v}} \def\vw{\mathbf{w}} \def\vx{\mathbf{x}} \def\vy{\mathbf{y}} \def\vz{\mathbf{z}} \def\veps{\boldsymbol{\epsilon}} \def\vnu{\boldsymbol{\nu}} \def \vmu{\boldsymbol{\mu}} \def\mSigma{\boldsymbol{\Sigma}} \def\mLambda{\boldsymbol{\Lambda}} $$

はじめに

前回はIterative Closest Point (ICP) の解説を行いました。ICPは現在のロボット姿勢の推定値でLidarで取得した最新の点群（現在スキャン）を変換し、その各点について参照スキャンの中から最近傍点を探し、その距離を最小化するようにロボット姿勢を更新する、というのを繰り返す手法で、最近傍探索の計算量の多さと、point-to-pointでありplaneを考慮できていない点などが問題になっていました。この記事で説明するNormal Distribution Transform (NDT) はこの２つの問題を緩和するための手法になっています。

参考文献

[1] Autowareではじめる自律移動技術入門
[2] パターン認識と機械学習下
[3] The Normal Distributions Transform: A New Approach to Laser Scan Matching, P. Biber et al., IEEE/RSJ IROS, 2003.
[4] The Three-Dimensional Normal-Distributions Transform --- an Efficient Representation for Registration, Surface Analysis, and Loop Detection, M. Magnusson, Doctoral Dissertation, 2009.
[5] Fast and accurate scan registration through minimization of the distance between compact 3D NDT representations, T. Stoyanov et al., Journal of Robotics Research, 2012.
[6] Beyond Points: Evaluating Recent 3D Scan-Matching Algorithms, M. Magnusson et al., IEEE ICRA, 2015.

数式表記

$x$ : 小文字＋イタリック $\rightarrow$ スカラー
$ \vx$ : 小文字+太字 $\rightarrow$ 列ベクトル
$\mA$ : 大文字＋太字 $\rightarrow$ 行列
$\approx$ : 近似
$\vx \sim p(\vx)$ : 確率変数 $\vx$ が分布 $p(\vx)$ に従う

Normal Distribution Transform

NDTではまず参照スキャンが存在する空間をvoxel（立方体）に分割します (voxelのサイズは状況に応じて自分で設定する必要があり、正しく設定できていないと性能が低下する原因となってしまいます)。 NDTのポイントは参照スキャンを各voxelごとにガウス分布で表現し、同じvoxelに含まれる現在スキャンは、このガウス分布から生成されたと考えて、その尤度が最大となるようにロボットの姿勢を求める、ということです。ICPの場合はkd木で領域が決まってからも最近傍点を探索するためにいくつかのステップが必要でしたが、NDTではどのvoxelに含まれるかを決めるだけで良いため計算量が削減されます。また、ガウス分布の分散共分散行列を用いることで平面を考慮することができています。

基礎

各voxelについて閾値以上の点が含まれていた場合には、その点から平均と分散共分散行列を計算します。（閾値は最低でも5以上に設定されるようで、例えば3点しかvoxelに含まれていない場合にはその3点を全て通る平面を考えることができてしまうので分散共分散行列がランク落ちしてしまいます) この記事では、$\va_n$で$n$番目の現在スキャンを、$\vb_{k,n}$で$k$番目のvoxelに属する参照スキャン点のうちの$n$番目を表すこととします。$k$番目のvoxelの平均$\vmu_k$と分散共分散行列$\mSigma_k$は以下のように計算されます。

\begin{align} 
\vmu_k &= \frac{1}{N_k} \sum_{n=1}^{N_k} \vb_{k,n} \\
\mSigma_k &= \frac{1}{N_k} \sum_{n=1}^{N_k} (\vb_{k,n} - \vmu_k) (\vb_{k,n} - \vmu_k)^T
\end{align}

ここで、$N_k$は$k$番目のvoxelに含まれる参照スキャンの点の数を表します。$N_k=3$の場合には$\{ \vb_{k,n} - \vmu_k \}_{n=1}^3$ は全て同一平面上のベクトルになっているため線形従属であり、$\mSigma_k$のランクは最大でも2にしかなりません。

現在のロボット姿勢の推定値$\mR, \vt$で$\va_n$を変換した結果を以下のように$\hat{\va}_n$とします。
$$\hat{\va}_n = T(\va_n) = \mR \va_n + \vt$$

この$\hat{\va}_n$が$k$番目のvoxelに属すると仮定すると、$\hat{\va}_n$が生成される確率は
$$ p(\hat{\va}_n | \vmu_k, \mSigma_k) = \mathcal{N}( \hat{\va}_n | \vmu_k, \mSigma_k) = \frac{1}{|2\pi\mSigma_k|^{1/2}} \exp \left(-\frac{1}{2} (\hat{\va}_n - \vmu_k)^T \mSigma_k^{-1} (\hat{\va}_n - \vmu_k) \right)$$

これを全ての現在スキャン点について考えると、$\{\hat{\va}_n \}_{n=1}^N$が生成される確率は

\begin{align} 
p(\{\hat{\va}_n\}_{n=1}^N |\{ \vmu_k\}_{k=1}^K, \{\mSigma_k\}_{k=1}^K) = \prod_{n=1}^N \mathcal{N}( \hat{\va}_n | \vmu_{c_n}, \mSigma_{c_n})
\end{align}

ここで、$K$はvoxelの総数、$c_n$は点$\va_n$が属するvoxelのインデックスを表します。NDTでは、この確率が最大となるような$\mR, \vt$を求めることが目的となります。この確率を最大化する$\mR, \vt$は対数をとっても変わらないので、対数を取って整理すると

\begin{align} 
\log p(\{\hat{\va}_n\}_{n=1}^N |\{ \vmu_k\}_{k=1}^K, \{\mSigma_k\}_{k=1}^K) &= \sum_{n=1}^N \log \mathcal{N}( \hat{\va}_n | \vmu_{c_n}, \mSigma_{c_n}) \\
&= -\frac{1}{2} \sum_{n=1}^N (\hat{\va}_n - \vmu_{c_n})^T \mSigma_{{c_n}}^{-1} (\hat{\va}_n - \vmu_{c_n}) + \text{const}
\end{align}

２行目では、ガウス分布の正規化係数は$\mR, \vt$と関係しないため省略しています。

このようなガウス分布を考慮する利点について説明します。$\mSigma$は正定値行列であるため固有値分解により $\mSigma = \mU \mLambda \mU^T$と書くことができ、例えば$\mLambda = \text{Diag}(10, 10, 0.01)$ のように１つだけ他と比べて小さい固有値を持つとします。ガウス分布の$\exp$の中身の、$\vx^T \mSigma^{-1} \vx$の部分はマハラノビス距離と呼ばれており、以下のように書き直すことができます。

\begin{align} 
\vx^T \mSigma^{-1} \vx &= \vx^T \mU \mLambda^{-1} \mU^T \vx \\
&= \vy^T \mLambda^{-1} \vy \\
&= 0.1 y_1^2 + 0.1 y_2^2 + 100 y_3^2
\end{align}

$\mU$が固有ベクトルを列ベクトルに持つ行列なので、$\vy = \mU^T \vx$ は$\vx$を固有ベクトルが張る空間に射影したベクトルであり、例えば、$\vx = \vu_1$ ($\vu_1$は$\mU$の第一列）とすると、$\vy = (1, 0, 0)^T$となり、マハラノビス距離は0.1となります。一方、$\vx = \vu_3$ とすると、$\vy = (0, 0, 1)^T$となり、マハラノビス距離は100となります。つまり、大きい固有値に対応する固有ベクトル（$\vu_1, \vu_2$）が張る平面上に$\vx$がある場合にはマハラノビス距離は小さくなり、小さい固有値に対応する固有ベクトル方向（$\vu_3$）の成分を持つとマハラノビス距離は大きくなります。あるvoxelにおいて、全ての参照スキャン点が１つの平面付近に存在する場合には、この例のように２つの固有値が大きくなり、その２つの固有ベクトルは平面上のベクトルとなるので（主成分分析の導出などを見ると理由がわかると思います）、ガウス分布の確率が大きくなるように（つまりマハラノビス距離が小さくなるように）姿勢を求めようとすると、現在スキャン点が参照スキャン点と同一の平面上に来ているかが重要となり、平面上の位置はあまり重要ではなくなります。これは前回の記事で説明したpoint-to-plane ICPと非常に似ていることがわかると思います。

外れ値への対応

ガウス分布をそのまま用いた場合、外れ値が入ってきた際にその値に大きく影響されてしまうという問題点があります。ICPでは、外れ値とみなす閾値を予め決めておいて、最近傍との距離がそれより遠い場合は外れ値とみなし考慮しないという方法を使っていましたが、NDTではガウス分布の代わりに以下のようにガウス分布と（ある範囲内での）一様分布を足し合わせた分布（左側の図の赤線）を使用することで、外れ値の影響を緩和します。

\begin{align} 
\bar{p}(\hat{\va}_n | \vmu_k, \mSigma_k) = c_1 \exp \left(-\frac{1}{2} (\hat{\va}_n - \vmu_k)^T \mSigma_k^{-1} (\hat{\va}_n - \vmu_k) \right) + c_2 p_u
\end{align}

ここで、$c_1, c_2$は定数であり、$c_2$で一様分布の大きさが決まり、$c_1$は（ある範囲内での）確率の積分が１となるように決定します。このような分布を考えることで、$\hat{\va}_n$が平均から大きく離れている場合でも、その影響を小さく抑えることができます。

ガウス分布を用いていた場合には、対数を取ることによって$\exp$が消えて扱いやすかったのですが、$c_2 p_u$の項が加わったことにより$\exp$が消えなくなってしまいました。しかし、対数を取った値をプロットしてみると（右側の図の赤線）、ガウス分布と似た形をしていることがわかります。そのため、対数尤度を以下のように近似します。

\begin{align} 
\log \bar{p}(\hat{\va}_n | \vmu_k, \mSigma_k) \ \approx \ 
&d_1 \exp \left( -\frac{d_2}{2} (\hat{\va}_n - \vmu_{c_n})^T \mSigma_{{c_n}}^{-1} (\hat{\va}_n - \vmu_{c_n} )\right) + d_3
\end{align}

ただし、$d_1, d_2, d_3$は以下で与えられます。

\begin{align} 
&d_1 = - \log(c_1 + c_2) \\
& d_2 = -2 \log((- \log(c_1 \exp(-\frac{1}{2})+c_2)-d_3) / d_1) \\
& d_3 = - \log(c_2)
\end{align}

これで目的関数を導出することができたので、あとはこの関数が最大となるようなロボットの姿勢をニュートン法などを用いて推定するのみです。更新式は、[3]などを参照してください。

Distribution-to-Distribution NDT (D2D-NDT)

NDTでは、各voxelについて現在スキャンの各点がガウス分布から生成されたとして、その尤度を最大化するという仕組みで、point-to-planeに近いpoint-to-distributionのようなやり方でした。これに対し、[4,5]で紹介されているD2D-NDTは現在スキャン点もガウス分布で表現してしまって、参照スキャンから計算されるガウス分布と現在スキャンから計算されるガウス分布の距離が近づくように姿勢を求めます。

導出

NDTは各voxelごとにガウス分布を計算していましたが、全体で見ると混合ガウスモデル(Gaussian Mixture Model = GMM)を計算していた、と捉えることができます。GMMについて詳しくない方は [2]を参照すると良いと思います。D2D-NDTでも同じ考えで、参照スキャンから以下のようなGMMを計算します。

\begin{align} 
p_{\text{ref}}(\va) = \sum_{k=1}^{K_\text{ref}} \pi_k \mathcal{N}( \va | \vmu_{k}, \mSigma_{k}) 
\end{align}

ここで、$K_\text{ref}$は混合されるガウス分布の数を表しており、$p_{\text{ref}}(\va)$は参照スキャンから計算される分布という意味です。しかし、$\{ \vmu_k \}_k$と$\{ \mSigma_k \}_k$を参照スキャン全体から計算するにはExpectation-Maximization (EM)アルゴリズムが必要であり、実際に計算しようとすると計算量が大きくなってしまうため、従来のNDTと同様にvoxelに区切って、それぞれでガウス分布を計算しているようです。以降では、重み$\pi_k = 1/K_\text{ref}$として計算していきます。

現在スキャンについても、まず何も変換していない場合に、以下のようなGMMを表すとします。

\begin{align} 
p_{\text{cur}}(\va) = \frac{1}{K_\text{cur}} \sum_{k=1}^{K_\text{cur}} \mathcal{N}( \va | \vmu_{k}^
{\text{cur}}, \mSigma_{k}^
{\text{cur}}) 
\end{align}

すると、ある姿勢$(\mR, \vt)$によって変換された場合、GMMは以下のように表されます。

\begin{align} 
\hat{p}_{\text{cur}}(\va) &= \frac{1}{K_\text{cur}} \sum_{k=1}^{K_\text{cur}} \mathcal{N}( \va | \mR \vmu_{k}^
{\text{cur}} + \vt, \mR \mSigma_{k}^
{\text{cur}} \mR^T ) \\ 
&= \frac{1}{K_\text{cur}} \sum_{k=1}^K{_\text{cur}} \mathcal{N}( \va |  \hat{\vmu}_{k}^
{\text{cur}} , \hat{\mSigma}_{k}^
{\text{cur}})
\end{align}

D2D-NDTでは、この$p_{\text{ref}}(\va)$と$\hat{p}_{\text{cur}}(\va)$が近づくように姿勢を求めていきます。

分布間の距離といえばKL-divergenceが有名ですが、GMM間のKL-divergenceは解析的に計算できず、上界を考えてそれを最小化する必要があるが実際に試したところうまくいかなかった、とのことで、代わりに以下の式で表される分布間の２乗誤差を考えます。

\begin{align} 
D &= \int \left( \hat{p}_{\text{cur}}(\va) - p_{\text{ref}}(\va) \right)^2 d \va \\
&= \int \hat{p}_{\text{cur}}(\va)^2 d \va + \int p_{\text{ref}}(\va)^2 d \va - 2 \int \hat{p}_{\text{cur}}(\va) p_{\text{ref}}(\va) d \va
\end{align}

２行目の最初の２項については姿勢に依らない定数となるため、姿勢を最適化する際には最後の項のみを考えれば良いということになります。（第一項について、$p_{\text{cur}}(\va)$は姿勢に依存していますが、どのような姿勢で変換したとしても積分した値は同じになるので無視できます）

以下の式が成り立つことが知られているので、

\begin{align} 
\int \mathcal{N}( \va | \vmu_{i}, \mSigma_{i}) 
\mathcal{N}( \va | \vmu_{j}, \mSigma_{j}) d \va &= \mathcal{N}(\mathbf{0} | \vmu_{i} - \vmu_{j}, \mSigma_{i}+\mSigma_{j}) \\
\end{align}

最後の項は以下のように整理することができます。

\begin{align} 
D &= -2 \int \hat{p}_{\text{cur}}(\va) p_{\text{ref}}(\va) d \va + \text{const} \\
&= -2 \int \frac{1}{K_\text{cur} K_\text{ref}} \sum_{i=1}^{K_\text{cur}} \sum_{j=1}^{K_\text{ref}}  
  \mathcal{N}( \va |  \hat{\vmu}_{i}^{\text{cur}},\hat{\mSigma}_{i}^{\text{cur}})
  \mathcal{N}( \va | \vmu_{j}^\text{ref}, \mSigma_{j}^\text{ref}) d\va + \text{const} \\
&= \frac{-2}{K_\text{cur} K_\text{ref}} \sum_{i=1}^{K_\text{cur}} \sum_{j=1}^{K_\text{ref}}
  \mathcal{N}( \mathbf{0} |  
    \hat{\vmu}_{i}^{\text{cur}} - \vmu_{j}^\text{ref},
    \hat{\mSigma}_{i}^{\text{cur}} + \mSigma_{j}^\text{ref})  + \text{const}
\end{align}

これで最小化したい関数を定義できたので、あとは今までと同様にニュートン法などを用いてパラメータを推定していきます。上の式だと、全てのガウス分布の組み合わせを考慮していることになりますが、平均が大きく離れている組み合わせはほとんど考慮しても意味がないので、[4]では計算量を削減するために最も近い１つのみを計算、[5]では、最も近いものと、その（水平方向の）近傍８つだけを考慮しています。

[4, 5]の実験結果を見ると、計算量はNDTより小さく、精度は似たようなものらしいですが、Autoware AutoではNDTが使われていたりして、あまりメジャーではないのかもしれないです。

まとめ

この記事では点群をガウス分布で表現して扱うNDTについて説明しました。次の記事ではGeneralized ICP (GICP)と呼ばれるもう一つの有名なスキャンマッチング手法について説明します。