$$\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) の拡張であるNDTの解説を行いました。この記事ではGICPと呼ばれる拡張について説明します。GICPは名前の通りICPを一般化したものであり、その特殊系としてpoint-to-point ICPやpoint-to-plane ICPを含んでいると（ほぼ）みなすことができます。[2]によると、NDTのほうが計算量は少ないですが、NDTはvoxelサイズの設定に性能が大きく影響されるという問題もあり、性能はGICPのほうが高いようです。

参考文献

[1] Generalized ICP, V. Aleksandr et al., Robotics: Science and Systems, 2009.
[2] Voxelized GICP for Fast and Accurate 3D Point Cloud Registration, K. Koide et al., IEEE ICRA, 2021.

数式表記

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

Generalized Iterative Closest Point (GICP)

NDTでは、voxelごとに１つのガウス分布を考えましたが、GICPでは各点がそれぞれ異なるガウス分布から生成されていると考えます。GICPではICPと同様に各点について最近傍を見つける必要があります。既に最近傍探索や外れ値除去が終わっていると仮定して、現在スキャンの点を$A=\{\va_1, \cdots, \va_N\}$、参照スキャンの点を$B=\{\vb_1, \cdots, \vb_N\}$として、$\va_i$と$\vb_i$が全ての$i$について対応しているとします。この各点がそれぞれ$\va_i \sim \mathcal{N}\vmu_i^A, \mSigma_i^A)$、$\vb_i \sim \mathcal{N}(\vmu_i^B, \mSigma_i^B)$のようにガウス分布から生成されたものだと仮定します。そして現在スキャンを真の姿勢$(\mR, \vt)$によって変換したものを$\hat{\va}_i = \mR \va_i + \vt$とします。このとき、$\vmu_i^B - (\mR \vmu_i^A + \vt) = 0$ となっていると仮定します。
誤差を$\vd_i = \vb_i - \hat{\va}_i$と定義すると、誤差は以下のガウス分布に従うことになります。

\begin{align}
\vd_i &\sim \mathcal{N}(\vmu_i^B - (\mR \vmu_i^A + \vt) , \mSigma_i^B + \mR \mSigma_i^A \mR^T) \\
&= \mathcal{N}(\mathbf{0}, \mSigma_i^B + \mR \mSigma_i^A \mR^T)
\end{align}

いつものように対数を取って整理すると、

\begin{align}
\log p(\vd_i) &= \log \mathcal{N}(\mathbf{0}, \mSigma_i^B + \mR \mSigma_i^A \mR^T) \\
&= - \frac{1}{2} \vd_i^T (\mSigma_i^B + \mR \mSigma_i^A \mR^T)^{-1} \vd_i + \text{const}
\end{align}

（ガウス分布の正規化係数の部分も$\mR$に依存するので考慮すべきだと思うのですが、なぜ考慮していないのかまだわかっていません。教えてくださると助かります。扱いやすくするため？）
この対数尤度を最大化するように姿勢を求めるのがGICPです。

どのような時に尤度が大きくなるかを考えてみます。前回の記事のマハラノビス距離の部分と同様の考え方をするため、前回の記事を読んでからのほうがわかりやすいと思います。$\mSigma_i^B$を固有値分解により$\mSigma_i^B = \mU \mLambda_i^B \mU^T$と表すとして、これがある平面を表しているとして$\mLambda_i^B = \text{Diag}(10, 10, 0.01)$と仮定します。一方、$\hat{\mSigma}_i^A = \mR \mSigma_i^A \mR^T$と定義して、例えば$\mSigma_i^B$と同じ固有ベクトルを用いて$\hat{\mSigma}_i^A = \mU \mLambda_i^A \mU^T$と表せると仮定します（一般には成り立ちません）。もし$\mLambda_i^A = \text{Diag}(0.01, 0.01, 10)$のように$\mSigma_i^B$が張る平面と直行する方向に広がっていたとすると、$\mSigma_i^B + \hat{\mSigma}_i^A = \mU\ \text{Diag}(10.01, 10.01, 10.01) \mU^T$となり、$\vd_i$は球状のぼんやりした分布に従うため、$\vd_i$がどのような値をとっても確率密度は大きくなりません。一方、$\mLambda_i^A = \text{Diag}(10, 10, 0.01)$のように$\mSigma_i^B$が張る平面と全く同じ平面を表している場合、$\mSigma_i^B + \hat{\mSigma}_i^A = \mU \ \text{Diag}(20, 20, 0.02) \mU^T$ であり、平面方向にのみ広がる分布となり、$\vd_i$が同じ平面上にある場合には確率密度が大きくなります。従って、尤度が大きくなるように姿勢を求めると、$\hat{\mSigma}_i^A$が表す平面が$\mSigma_i^B$が表す平面となるべく重なるようになり、$\vd_i$はその平面上に来るようになるはずです。これはpoint-to-planeを更に拡張したplane-to-planeのようになっています。実際に使う際には、各点について２０個程度の近傍の点から分散共分散行列を計算し、３つの固有ベクトルのうち固有値が最も小さい固有ベクトルが法線方向であると仮定して、その固有値を小さい値で置き換え、残りの固有値を大きくすることで分散共分散行列が平面を表すよう修正するようです。

最初に述べたようにGICPはpoint-to-point ICPやpoint-to-plane ICPをその特殊系に含んでいます。$\mSigma_i^A = \mathbf{0}$、$\mSigma_i^B = \mI$とすると、$\vd_i \sim \mathcal{N}(\mathbf{0}, \mI)$となり、ガウス分布の$\exp$の中身がユークリッド距離となるため、尤度の最大化はユークリット距離の最小化となり、point-to-point ICPと確かに一致しています（$\mSigma_i^A = \mathbf{0}$は実際にはありえませんが）。

point-to-plane ICPでは、前々回の記事で示したように、$\vb_i$での法線方向の単位ベクトルを$\vv_i$として以下の距離を最小化します。
$$L = \sum_{i=1}^{N} | \vv_i^T (\vb_i - (\mR \va_i + \vt)) | ^2 = \sum_{i=1}^{N} | \vv_i^T \vd_i | ^2$$

ここで、$\mV_i = \vv_i \vv_i^T$とすると、$\mV_i \vd_i = \vv_i (\vv_i^T \vd_i)$は $\vd_i$を$\vv_i$ 上に射影したベクトルを表すので、上の式は以下のように書き換えることもできます。
$$L = \sum_{n=1}^{N_c} || \mV_i^T \vd_i ||^2 = \sum_{n=1}^{N_c} (\mV_i^T \vd_i)^T (\mV_i^T \vd_i) = \sum_{n=1}^{N_c} \vd_i^T \mV_i \vd_i$$
最後の部分では、$||\vv||^2 = 1$ と $\mV \mV^T = \vv \vv^T \vv \vv^T = \vv ||\vv||^2 \vv^T = \mV$であることを用いています。これをGICPの対数尤度と比較してみると、$\mSigma_i^A = \mathbf{0}$、$\mSigma_i^B = \mV^{-1}$としたときにGICPと一致することがわかります。実際には$\mV$はランク１行列であるため逆行列は計算できませんが、$(\mSigma_i^B)^{-1}$が$\mV$に近づいていく時にGICPはpoint-to-plane ICPに近づいていく、と言えます。

余談ですが、$\mV$のように$\mV^T = \mV = \mV^2$という性質を持つ行列を直交射影行列と呼びます。$\mV \vx$と$(\mI - \mV)\vx$という２つの$\vx$の射影を考えると、$\mV \vx + (\mI - \mV)\vx = \vx$であり、$(\mV \vx)^T ((\mI - \mV)\vx) = \vx^T \mV \vx - \vx^T \mV \mV \vx = \mathbf{0}$となることから、$\vx$を直交する２つの成分に分解できていることがわかります。$\mV = \vv \vv^T$以外にも列直交行列を$\mU$ とすると、$\mV = \mU \mU^T$も直交射影行列となります。また、$\mQ = (\mI - 2 \mV)$とすると$\mQ$による変換はハウスホルダー変換と呼ばれます。

まとめ

この記事ではICPの一般形であるGICPについて説明しました。[2]の実験結果などを見てみると、NDTよりもかなり精度がよくなっています。また、NDTではvoxelサイズが性能に大きな影響を与えているため状況に応じたサイズ設定が必要となりますが、GICPではその問題がないのも利点と言えそうです。ただし、各点についての分散共分散行列の計算にかなりの計算量が必要となり、またICPと同様に毎回最近傍点の探索が必要であるためNDTやICPよりも遅いという問題点があります。

とりあえず今回の記事でこのシリーズは最後になります。見てくださった方ありがとうございます。間違いなどありましたら、教えて頂けると助かります。よろしくお願いします。