$$\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\mSigma{\boldsymbol{\Sigma}} \def\mLambda{\boldsymbol{\Lambda}} $$

概要

ICPとはLidarなどで得られた点群を使ってロボットの姿勢を推定する最も基本的な手法です。Lidarで得られる点群はLidarの位置を原点としたLidar座標系上での値となっており、これをロボットの姿勢の推定値に基づいてGlobal座標系に変換することを考えます。ロボットの姿勢が正しく推定できていれば、変換した点群はGlobal座標系上に登録された過去の時刻の点群と（部分的に）一致するはずです。この考え方に基づいて、現在の点群（現在スキャン）が過去の点群（参照スキャン）となるべく一致するようなロボットの姿勢を求めます。そのため、まず現在スキャンをロボット姿勢の推定値の初期値によってGlobal座標系に変換し、各点について参照スキャンの中から最近傍の点を見つけます。次に、この最近傍との距離が最小となるようにニュートン法などでロボット姿勢の推定値を更新します。この操作を反復するのがICPです。

参考文献

この記事は主にMagnussonさんのNDTに関する博士論文のICPに関する部分をまとめたような内容になっています。
ICPの説明はこれから説明するNDTやGICPのための導入的な役割のため、細かい更新式の導出については省略します。

[1] The Three-Dimensional Normal-Distributions Transform --- an Efficient Representation for Registration, Surface Analysis, and Loop Detection, M. Magnusson, Doctoral Dissertation, 2009. [(URL)](https://www.researchgate.net/publication/229213868_The_Three-Dimensional_Normal-Distributions_Transform_---_an_Efficient_Representation_for_Registration_Surface_Analysis_and_Loop_Detection: 3D-NDT)
[2] SLAM入門

手法詳細

1. 座標の変換

この記事では説明の簡単化のため、ロボット座標系とLidar座標系が一致していると仮定します。実際にはこれらの座標系は異なることもありますが、ロボットのどの位置にLidarが搭載されているかが既知であれば、これから説明する座標変換を用いて、Lidar座標系で定義された点群をロボット座標系に変換することができます。

ICPにおいては、ロボットの姿勢は、Global座標系上でのロボットの位置 $\vt$ と、向き $\theta$ で表現します。ロボット座標系上のある点$\vx$をGlobal座標系上の点$\vy$に変換するためには、$\theta$ から計算される回転行列 $\mR_\theta$と $\vt$ を用いて、以下の式で計算します。
$$ \vy = \mR_\theta \vx + \vt$$

2. 最近傍の探索

各点の最近傍の探索は、参照スキャンの点の数が$N_r$、現在スキャンの点の数が$N_c$とすると、愚直に行うと$N_r N_c$の全ての組み合わせについて距離を計算する必要があり、点の数が多い場合には問題となります。

効率的に最近棒を探索する手法の１つとしてkd木 (wiki)（k-dimensional tree）を使う方法があります。
kd木では、あるk次元のベクトルの集合に対して、１つの次元を選び閾値を設定することで、ベクトルの集合を閾値以上か以下かの２つの領域に分割します。この処理を繰り返し行い２分木を作成していき（同じ次元に対して何回も分割しても良い）、各領域に含まれる点の数が一定以下になったら終了します。例えば、kd木によって$N$個の領域に分割したとすると、新たな点がどの領域に含まれるかを計算するためには$\log_2N$ 回の処理で済みます。kd木の作成には計算量が必要となりますが、参照スキャンに対してkd木を適用するので、ICPの反復の最中にkd木を作り直す必要はありません。

kd木を用いた最近傍の探索は以下のように行います。

ある点$\vx$がどの領域に含まれるかをkd木により計算
その領域に含まれる全ての点と$\vx$との距離を計算し、最短となるものを$\vq$とする
$d = |\vx - \vq|$として、$\vx$を中心とする半径$d$の円が領域からはみ出ていないかを調べる
はみ出ている場合には、隣の領域（２分木において同じ親を持つもう一つの領域）の全ての点との距離を計算
3,4を繰り返していき、はみ出なくなったら終了

ステップ3~5を行う理由は、点$\vx$がkd木によって求めた領域の端にいる場合、隣の領域に最近傍となる点が存在する可能性があるためです。
[1] によると、kd木の作成時に各領域に１点のみが含まれるように分割することでステップ2の処理を早く行うことができますが、ステップ3~5の処理を行う回数が増えてしまう問題があります。そのため、データ数にもよりますが各領域に含まれる点の数が10~20あたりになるようにするのが良いとのことです。また、現在の最近傍同士を対応させるのが必ずしも最良とは限らない＆計算量削減のためステップ3において半径を $d-\epsilon$ に緩和する方法や、現在スキャンの各点が含まれる領域を求めて記録しておくことで、次の反復以降は記録していた領域から探索を始めることで計算量を削減するcached kd木という手法なども[1] に紹介されていました。

3. 外れ値除去と最適化

現在スキャンと参照スキャンは必ずしも1対1対応しているとは限らない、というか多くの場合では対応していないため、実際には対応点が存在しない点は誤った対応点を与えられることになります。また、Lidarによる観測には当然ノイズが乗ることもあります。これらの問題の単純な解決策としては、距離の閾値を決めて、それよりも現在の距離が大きい場合には、その点は除去してしまうという方法があります。[1] では、この閾値を最初は大きくして、徐々に小さくしていくという手法が紹介されていました。

外れ値除去が終了したら、あとは以下の式で与えられる対応する点間のEuclid距離の2乗の和を最小化するようにNewton法などを用いて$\vp$を更新していくだけです。
$$ \sum_{i=1}^{N_c} || \vx_i - \vq_i ||^2 $$

ここで、$\vx_i$は現在スキャンの$i$番目の点、$\vq_i$は$\vx_i$の最近傍の点を、$N_c$は外れ値を除去した後の現在スキャンの点の数を表しています。

1~3の処理を更新量が閾値以下になるまで、または指定した回数だけ繰り返し行います。

ICPの問題点

最近傍探索の計算量

上で説明したようにICPにおいては反復のたびに最近傍を探索する必要があり、計算量の点で大きな問題となっています。
この問題を改善するために、Normal Distribution Transform (NDT)や、Voxelized Generalized ICP (VGICP) と言うvoxelを用いた手法が提案されています。

縮退

例えば、トンネルなどのように点群に特徴があまり存在しないような環境においてICPを用いると、参照スキャンを取得した位置からほとんど動いていない場所をロボット姿勢の推定値としても、多くの点について最近傍点との距離を小さくすることができてしまいます。トンネル内にくぼみがあったとしても、この部分の誤差は大きいままですが、大半の点では距離が小さくなってしまっているため正しい方向に移動できないという問題が起こります。これによって実際よりも移動量が小さく推定されてしまうのが縮退です。

IMUなどの他のセンサと統合する方法や、今まで説明してきたpoint-to-point ICPをpoint-to-plane ICPに拡張することである程度改善することができます。point-to-plane ICPでは、各点がある平面上にあると仮定して、その平面に垂直な方向の距離のみを最小化するため、以下のコスト関数を用います。

$$\sum_{i=1}^{N_c} | \vv_i^T (\vx_i - \vq_i) | ^2 $$
ここで、$\vv_i$は、$\vx_i$の最近傍点$\vq_i$における単位法線ベクトルを表します ( $||\vv_i||=1$ )。
$\vx_i - \vq_i$と$\vv_i$の内積は、$\vx_i - \vq_i$を$\vv_i$に射影したときの長さを表すため、平面に垂直な方向の距離を表しています。

法線ベクトルの推定方法の１つとして、点$\vq_i$の近傍のいくつかの点を用いて分散共分散行列を計算し、その行列の固有値最小の固有ベクトルを用いるという手法がよく使われているようです。これはもし周囲の点群が同じ平面上に分布しているとするならば、主成分分析の考え方からわかるように第１固有ベクトルと第２固有ベクトルはこの平面上の向きになり、第３固有ベクトルは第１固有ベクトル、第２固有ベクトルと直行しているという性質から法線の方向に対応すると考えられるためです。

次以降の記事で紹介するNDTやGICP、それらの拡張手法もpoint-to-plane（やplane-to-plane）の考え方に基づいています。

精度行列の計算

前回のGraph-based SLAMの記事において、スキャンマッチングを使うと姿勢の変化量と精度行列（分散共分散行列の逆行列）が求められると説明していました。ここではラプラス近似に基づく精度行列の計算方法について説明します。

例えば先程説明したpoint-to-plane ICPにおいては以下のコスト関数を最小化するように姿勢の変化量 $\vp$ を計算していました。
$$\sum_{i=1}^{N_c} | \vv_i^T (\vx_i - \vq_i) | ^2 $$
このコスト関数を $L(\vp)$ とし、$L(\vp)$ を最小化する $\vp$ を $\tilde{\vp}$ と表すことにします。$L(\vp)$ をテイラー展開を用いて$\tilde{\vp}$のまわりで2次までで近似します。

\begin{align} 
L(\vp) &\approx L(\tilde{\vp}) + \left.\frac{\partial L}{\partial \vp}\right|_{\vp=\tilde{\vp}}(\vp - \tilde{\vp}) + \frac{1}{2} (\vp - \tilde{\vp})^T \mH (\vp - \tilde{\vp}) \\
&= L(\tilde{\vp}) + \frac{1}{2} (\vp - \tilde{\vp})^T \mH (\vp - \tilde{\vp}) 
\end{align}

ここで、$\mH$ は$L(\vp)$ の $\vp$ についてのヘッセ行列です。2行目では、$L(\vp)$ の微分が $\vp = \tilde{\vp}$ において0になることを用いました。

今度は（少々唐突ですが）平均 $\tilde{\vp}$ 、精度行列 $\mLambda$ であるガウス分布に$\vp$ が従うとすると、以下が成り立ちます。

\begin{align} 
- \log \mathcal{N}(\vp | \tilde{\vp}, \mLambda) = \frac{1}{2} (\vp - \tilde{\vp})^T \mLambda (\vp - \tilde{\vp}) + \text{cons}t
\end{align}

先程の式と見比べると全く同じ形になっていることがわかります。このことから、$L(\vp)$ を最小化するように $\vp$ を求めるということは、ヘッセ行列 $\mH$ を精度行列とするガウス分布に $\vp$ が従うとみなしたときの最尤推定と考えることができます。よって、$\mH$ を $\vp$ の精度行列として利用することができます。ヘッセ行列の計算は複雑であることが多いため、ガウス・ニュートン法で用いた近似を適用することもあります。

この精度行列の計算方法はICPでも次回以降説明するNDTやGICPでも用いることができますが、point-to-point ICPの場合には点の対応関係は固定したまま精度行列を考えるため、あまり有用なものにはなりません。