目標：なぜリー代数を用いた位置姿勢のパラメータの最適化が便利なのか理解しよう

この記事は、ロボット技術者向けリー群の速習(4) リー群を用いた最適化を踏襲して、small_gicpのソースコードを用いながら解説する記事となります。

1. ICP registrationについて

Source点群（赤色）の一点一点をTarget点群（黒色）における最近傍点と対応付け、点と点の距離の総和を最小二乗法で最小化する。

1.1 デモ

Source code: (icp_registration)

1.2 フローチャート

2. 最小二乗法について

残差関数を最小化するために、パラメータを用意してどれくらいの量を動かせばよいかを考える。

2.1 残差関数

Target点群を $\mathcal{M} = \lbrace \boldsymbol{m}_n \in \mathbb{R}^3 | _{n=1\ldots N} \rbrace$、Source点群を $\mathcal{S} = \lbrace \boldsymbol{s}_n \in \mathbb{R}^3 | _{n=1\ldots N} \rbrace$とする。

また、$\boldsymbol{R} \in SO(3)$、$\boldsymbol{t} \in \mathbb{R}^3$として、Source点群座標系からTarget点群座標系への変換行列を$T = [\boldsymbol{R} | \boldsymbol{t}] \in SE(3)$とする。

点群のマッチングを行うには、変換行列$T$をパラメータとして、以下の残差関数の最小化問題を考える。

\boldsymbol{e}_i(\boldsymbol{T}) = \boldsymbol{m}_i - (\boldsymbol{R} \boldsymbol{s}_i + \boldsymbol{t})

\arg\min_{\boldsymbol{T}} \sum_{i} \| \boldsymbol{e}_i(\boldsymbol{T}) \|^2

2.2 リー代数を用いたパラメータ化

実際に最小二乗法を解くときには、リー代数$\mathfrak{se}(3)$を用いることでパラメータの数を最小限の6つにする。

\boldsymbol{x}^\wedge = \begin{bmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{\upsilon} \\ \boldsymbol{0} & 0 \end{bmatrix} \in \mathfrak{se}(3)

ここで、$\mathfrak{se}(3)$のベクトル$\boldsymbol{x} = \begin{bmatrix} \boldsymbol{\omega} & \boldsymbol{\upsilon} \end{bmatrix}^T \in \mathbb{R}^6$のうち

$\boldsymbol{\omega} \in \mathbb{R}^3$: 回転を表すパラメータ
$\boldsymbol{\upsilon} \in \mathbb{R}^3$: 並進を表すパラメータ

である。
ベクトル$\boldsymbol{x}$は指数写像によって$\boldsymbol{T}$に変換する。

\boldsymbol{T} = \exp\left(\begin{bmatrix} [\boldsymbol{\omega}]_\times & \boldsymbol{\upsilon} \\ \boldsymbol{0} & 0 \end{bmatrix}\right)

2.3 Gauss Newton法

Gauss-Newton法は、ニュートン法において、Hessianをヤコビアンの行列積で近似する手法である。

更新

パラメータはその微小変位$\boldsymbol{\delta} \in \mathbb{R}^6$で更新される。

\boldsymbol{x}_{k+1} = \boldsymbol{x}_k + \boldsymbol{\delta}

パラメータの更新量

更新量はヘシアンの逆行列と勾配で計算される。

\boldsymbol{\delta} = -\boldsymbol{H}^{-1} \boldsymbol{g}

Hessian (ヘシアン)

Gauss-Newton法では、全点群ペアのヤコビアンを用いてヘシアンを次のように近似する。

\boldsymbol{H} \approx \sum_{i} \boldsymbol{J}_i^T \boldsymbol{J}_i

Gradient (勾配)

勾配も同様に全点群ペアの寄与を累積する。

\boldsymbol{g} = \sum_{i} \boldsymbol{J}_i^T \boldsymbol{e}_i

各点ペア $i$ について残差 $\boldsymbol{e}_i$ とヤコビアン $\boldsymbol{J}_i$ を計算し、それらを全点群数分加算することで、パラメータの更新量が計算できる。

ガウス・ニュートン法の詳細はこちらを参照
最適化とガウス・ニュートン法の導出について

3. ヤコビアンの導出

同次座標で表現された点を $\tilde{\boldsymbol{m}}_i = [\boldsymbol{m}_i^T, 1]^T$、$\tilde{\boldsymbol{s}}_i = [\boldsymbol{s}_i^T, 1]^T$として、残差を

\boldsymbol{e}_i(\boldsymbol{T}) = \tilde{\boldsymbol{m}}_i - (\boldsymbol{T} \exp(\boldsymbol{\delta}^\wedge) \tilde{\boldsymbol{s}}_i)

とする。ヤコビアンは残差 $\boldsymbol{e}_i$を$\boldsymbol{\delta}$で微分して求める：

\boldsymbol{J}_i = \frac{\partial \boldsymbol{e}_i}{\partial \boldsymbol{\delta}} = -\frac{\partial (\boldsymbol{T} \exp(\boldsymbol{\delta}^\wedge) \tilde{\boldsymbol{s}}_i)}{\partial \boldsymbol{\delta}} \bigg|_{\boldsymbol{\delta}=\boldsymbol{0}}

以下の順番でヤコビアンの導出過程を説明する

3.1 指数写像の一次近似について
3.2 分子の計算について
3.3 微分について

3.1 指数写像の一次近似について

指数写像

\exp(\boldsymbol{\delta}^\wedge) = \boldsymbol{I} + \boldsymbol{\delta}^\wedge + \frac{1}{2!}(\boldsymbol{\delta}^\wedge)^2 + \cdots

において、$\boldsymbol{\delta}$ が微小量であるとき、$\boldsymbol{\delta}$ の二乗以上の高次の項は無視できるほど小さくなるため、以下の一次近似が成立する。

\exp(\boldsymbol{\delta}^\wedge) \approx \boldsymbol{I} + \boldsymbol{\delta}^\wedge

3.2 分子の計算について

この一次近似を用いて、微小変位後に位置する点$ \boldsymbol{T} \exp(\boldsymbol{\delta}^\wedge) \tilde{\boldsymbol{s}}_i$ を計算する。

まず、微小変換 $\exp(\boldsymbol{\delta}^\wedge)$ が点 $\boldsymbol{s}_i$ に作用する部分を見てみる。

\exp(\boldsymbol{\delta}^\wedge) \tilde{\boldsymbol{s}}_i \approx (\boldsymbol{I} + \boldsymbol{\delta}^\wedge) \tilde{\boldsymbol{s}}_i

これを展開すると：

\begin{bmatrix} \boldsymbol{I} + [\boldsymbol{\omega}]_\times & \boldsymbol{\upsilon} \\ \boldsymbol{0}^T & 1 \end{bmatrix} \begin{bmatrix} \boldsymbol{s}_i \\ 1 \end{bmatrix} = \begin{bmatrix} \boldsymbol{s}_i + [\boldsymbol{\omega}]_\times \boldsymbol{s}_i + \boldsymbol{\upsilon} \\ 1 \end{bmatrix}

次に、変換行列

\boldsymbol{T} = \begin{bmatrix} \boldsymbol{R} & \boldsymbol{t} \\ \boldsymbol{0}^T & 1 \end{bmatrix}

を左からかけると：

\boldsymbol{T}(\exp(\boldsymbol{\delta}^\wedge) \boldsymbol{s}_i) \approx \boldsymbol{R}(\boldsymbol{s}_i + [\boldsymbol{\omega}]_\times \boldsymbol{s}_i + \boldsymbol{\upsilon}) + \boldsymbol{t}

= \boldsymbol{R}\boldsymbol{s}_i + \boldsymbol{R}[\boldsymbol{\omega}]_\times \boldsymbol{s}_i + \boldsymbol{R}\boldsymbol{\upsilon} + \boldsymbol{t}

3.3 微分について

この式を微小変位ベクトル $\boldsymbol{\delta}$ で偏微分する。
ここで、ヤコビアン $\boldsymbol{J}_i$ は、3×6行列 である：

\boldsymbol{J}_i = \frac{\partial \boldsymbol{e}_i}{\partial \boldsymbol{\delta}} = \begin{bmatrix}
\frac{\partial \boldsymbol{e}_i}{\partial \boldsymbol{\omega}} & \frac{\partial \boldsymbol{e}_i}{\partial \boldsymbol{\upsilon}}
\end{bmatrix}

そのため、残差ベクトル $\boldsymbol{e}_i$ を $\boldsymbol{\omega}, \boldsymbol{\upsilon}$ でそれぞれ偏微分する。

回転成分 $\boldsymbol{\omega}$ で偏微分する。
外積行列の性質 $[\boldsymbol{\omega}]_\times \boldsymbol{s}_i = -[\boldsymbol{s}_i]_\times \boldsymbol{\omega}$ を用いると：

\frac{\partial}{\partial \boldsymbol{\omega}} (\boldsymbol{R}[\boldsymbol{\omega}]_\times \boldsymbol{s}_i) = \frac{\partial}{\partial \boldsymbol{\omega}} (\boldsymbol{R}(-[\boldsymbol{s}_i]_\times \boldsymbol{\omega})) = -\boldsymbol{R} [\boldsymbol{s}_i]_\times

並進成分 $\boldsymbol{\upsilon}$ で偏微分すると：

\frac{\partial}{\partial \boldsymbol{\upsilon}} (\boldsymbol{R}\boldsymbol{\upsilon}) = \boldsymbol{R}

したがって、ヤコビアンは以下のようになる。

\boldsymbol{J}_i = \frac{\partial \boldsymbol{e}_i}{\partial \boldsymbol{\delta}} = -\frac{\partial (\boldsymbol{T} \exp(\boldsymbol{\delta}^\wedge) \boldsymbol{s}_i)}{\partial \boldsymbol{\delta}} = \begin{bmatrix} \boldsymbol{R} [\boldsymbol{s}_i]_\times & -\boldsymbol{R} \end{bmatrix}

コードを見てみよう
ヤコビアンの計算：
small_gicp/icp_factor.hpp#L46