グラフ最適化をマスターしよう！

# はじめに

グラフ最適化（Graph Optimization）は、パラメータをグラフ構造で表現し、最適化問題を解決する手法です。特にロボティクスなどの領域で広く活用されています。

以下に、グラフ最適化の応用例をいくつか挙げます。

* Visual SLAMやSFMのバンドル調整（Bundle Adjustment）→[「解説記事」](https://qiita.com/scomup/items/f8632151712828e9625d)
* Graph SLAMのループ閉じ込み問題→[「解説記事」](https://qiita.com/scomup/items/d5999f38a34b0d76f864)
* 経路計画問題(TEB, ebandなど)→(<b>coming soon</b>)

実際のアプリケーションでは、[ceres](http://ceres-solver.org/)や[gtsam](https://gtsam.org/)、[g2o](https://github.com/RainerKuemmerle/g2o)などのグラフ最適化ライブラリを利用することで、グラフ最適化問題を解決することができます。しかし、グラフ最適化の内部原理を理解していないと、性能の向上や課題の解決が困難になることが多いです。

筆者自身は、グラフ最適化の理解を深めるため、独自のグラフ最適化ライブラリをPythonで実装したことがあります。g2oなどの大規模なOSSと比較して、行数が少ないですが、可読性が高く、様々な実際の問題に対応できます。学習に最適なツールだと思います。

この記事では、初心者向けに基礎から始めて、最終的には独自の最適化ライブラリの実装を解説します。グラフ最適化の完全な理解を目指したいと考えております。

$$
\newcommand{\pdv}[2]{\frac{\partial #1}{\partial #2}} %partial differentiation
$$

# グラフとは？

グラフ$G = (V, E)$は、頂点(Vertex)※とそれらを結ぶエッジ(Edge)からなるデータ構造です。頂点は最適化したいオブジェクトや要素を表し、エッジは頂点間の関係や接続を表します。エッジが結ぶ頂点の数は任意です。$V$と$E$はそれぞれグラフに含まれる頂点とエッジの集合です。

_※: 頂点(Vertex)をノード(Node)と呼ぶこともあります。_

## グラフの例

私はロボットの仕事をしており、今回は最も簡単なロボット移動問題をグラフで表現します。

あるロボットが1Dの世界で移動します。
この世界には1つのランドマーク（ドア）があり、ロボットには自分とドアの距離を測るセンサが搭載されています。また、自分の走行距離も測ることができます。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/149168/6caba1d7-d377-6e05-6737-f4b65802afa7.png" width="800"/>

上記の図にロボット移動する様子を示しています。

この例を、グラフで表現すると、3つの頂点があります。

* $x_1$: 　ロボットが移動する前の自己位置
* $x_2$: 　ロボットが移動した後の自己位置
* $x_3$: 　ランドマークの位置

頂点間の関係を示すエッジが4つあります。

* $e_{1}$: 　$x_3$と$x_1$の関係、ロボットが移動する前に測ったランドマークとの距離 (2m)
* $e_{2}$: 　$x_3$と$x_2$の関係、ロボットが移動した後に測ったランドマークとの距離 (-1m)
* $e_{3}$: 　$x_1$と$x_2$の関係、ロボットの移動距離 (3.1m)
* $e_{4}$: 　$x_1$の事前分布(piror)、ロボットが移動する前に、おおよその位置 (0m)

数式で書くと：
* $e_{1}$: 　$x_3 - x_1 = 2$
* $e_{2}$: 　$x_3 - x_2 = -1$
* $e_{3}$: 　$x_2 - x_1 = 3.1$
* $e_{4}$: 　$x_1 = 0$


下記のように、このグラフを可視化することができます。

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/149168/00d5720b-c8df-92f2-f108-94e427a6ec8a.png" width="300"/>

# 解き方

すぐにお気づきかと思いますが、先ほどの情報はお互いに矛盾していますね。それは、ロボットの移動量やセンサーの値が真値ではなく、それぞれに独立した誤差が存在する測量値からです。エッジ$e_{k}$の誤差の大きさは、分散$\sigma_{k}$で示します。

矛盾がありますが、全ての情報の残差を考慮し、最小二乗法を用いて矛盾を最小限に抑えることができます。式(1)は最小化したい目的関数を示しています。

$$
\underset{x}{\textrm{argmin}} \quad F(x) = \sum_{e_{k}\in E} ( r_{k} )^2 {\sigma_k}^{-1} \\\\
\tag{1}
$$



$r_{k}$はエッジ$e_{k}$の残差関数です。例えば、$e_{3}$の残差は$x_2$ - $x_1$ - 3.1となります。誤差がない場合、残差は0になります。上記のロボット問題では、以下のようにFを展開できます:

$$
  F(x)  =(\sigma_{1}^{-1}(x_3 - x_1 - 2)^2 + \sigma_{2}^{-1}(x_3 - x_2 + 1)^2+ \sigma_{3}^{-1}(x_2 - x_1 - 3.1)^2 + \sigma_{4}^{-1} x_1^2)
$$

## ガウス・ニュートン法

本文はガウス・ニュートン法(Gauss–Newton method)をベースの解き方を説明します。
[以前の記事](https://qiita.com/scomup/items/a9c09d57101583c58619)の復習になりますが、ガウス・ニュートン法は式(2)(3)のように、反復法を用いて$x$を求めることができます。

$$ 
\Delta x = -H^{-1}g
\tag{2}
$$

$$ 
x_{new} = x \boxplus \Delta x 
\tag{3}
$$

式(2)の$g$と$H$はそれぞれ$r$の勾配ベクトルとヘッセ行列であり、式(4)(5)のように計算できます。
$$ 
g = J^T \Sigma^{-1} r \tag{4}
$$

$$ 
H  \approx  J^T \Sigma^{-1} J  \tag{5}
$$

ここで、$\Sigma$はの対角線成分は各エッジの分散$\sigma$から構成されたシステム全体の偏微分行列です。$r$と$J$が計算できれば、反復法を用いて$x$を求めることができます。

ロボット移動の問題の場合、4つのエッジがありますので、全体の残差$r$は4x1のベクトルとなります。

$$
r = 
\begin{bmatrix}
r_1  \\\\
r_2  \\\\
r_3  \\\\
r_4 
\end{bmatrix} 
= \begin{bmatrix}
x_3 - x_1 - 2  \\\\
x_3 - x_2 + 1  \\\\
x_2 - x_1 - 3.1  \\\\
x_1
\end{bmatrix} 
$$


また、ヤコビ行列の$i$列目は$x_i$に対する偏微分を表します。頂点は合計3つありますので、$J$は4x3の行列です。

$$
J = \begin{bmatrix}
\pdv{r_1}{x_1} & 0 & \pdv{r_1}{x_3} \\\\
0 & \pdv{r_2}{x_2} & \pdv{r_2}{x_3} \\\\
\pdv{r_3}{x_1} & \pdv{r_3}{x_2} & 0 \\\\
\pdv{r_4}{x_1} & 0 & 0 \\
\end{bmatrix}
= \begin{bmatrix}
-1 & 0 & 1 \\\\
0 & -1 & 1 \\\\
-1 & 1 & 0 \\\\
1 & 0 & 0 
\end{bmatrix}
$$


$x$の初期値を0、式(2)(3)(4)(5)に代入して$x$を簡単に計算できます。測量誤差が同じであれば($\Sigma$を単位行列とする)、最適な解は$x_1=0$、$x_2=3.067$、$x_3=2.03$となります。


$$
x =-(J^T \Sigma^{-1} J) (J^T \Sigma^{-1} r) = \begin{bmatrix}
0  \\\\
3.067  \\\\
2.033  
\end{bmatrix} 
$$

## 解き方の一般化

先ほど、簡単な最適化問題を解きましたが、任意の問題にも適用できるように、解法を一般化します。

まず、各行列の形状を考えましょう。グラフの頂点数をn、エッジ数をmとすると、ヤコビ行列$J$のブロックサイズはm x nです。$r$のブロックサイズはm x 1になります。式(4)(5)に代入すると、Hとgのブロックサイズはそれぞれn x nおよびn x 1となることがわかります。

注意していただきたいのは、ここのブロックサイズは行列のサイズではないことです。なぜなら、各頂点やエッジは多次元のデータである場合もあります。


多次元のデータに対応するため、式(1)を式(6)のように、マハラノビス平方距離(Mahalanobis squared distance)の最小化の形に書き直します。ここで、$\Sigma_{k}$は測量$e_k$の誤差を示す共分散行列です。

$$
\begin{aligned}
\underset{x}{\textrm{argmin}} \quad F(x) 
&= \sum_{e_{k}\in E} \Vert r_{k} \Vert^2_{\Sigma_k^{-1}} \\\\
&= \sum_{e_{k}\in E} r_{k}^T \Sigma_{k}^{-1} r_{k} 
\end{aligned}
\tag{6}
$$

実際の最適化問題では、エッジの数は頂点の数よりも遥かに多いことがほとんどです。
例えば、n個の頂点があり、お互いにつなぐエッジが1本あると、エッジの数mは${}_n \mathrm{C}_2 $となります。n=1000の場合、mは499500となり、Jは499500 x 1000という非常に巨大な行列になります。計算がかなり複雑になりますね。

そのため、グラフ最適化のライブラリを設計する際には、直接$J$や$r$を管理するよりも、$H$と$g$をメモリに格納する方が有利です。なぜなら、$H$と$g$のサイズはmとは無関係だからです。

Hとgは式(7)(8)のように計算することができます。

$$ 
H  =  J^T \Sigma^{-1} J
= \begin{bmatrix}
\ddots & \vdots & \vdots \\\\ 
 \vdots & \sum_{e_k \in E} {J_{i}^k}^T \Sigma^{-1}_{k} J_j^k & \vdots \\\\ 
\vdots & \vdots & \ddots
\end{bmatrix}
 \tag{7}
$$

$$ 
g  =  J^T \Sigma^{-1} r = 
\begin{bmatrix}
\vdots \\\\
 \sum_{e_k \in E} {J_{i}^k}^T \Sigma^{-1}_{k} r_k  \\\\
\vdots 
\end{bmatrix}
\tag{8}
$$

$i$、$j$は頂点の番号であり、ヘッセ行列内で行と列の番号も示しています。$k$はエッジの番号です。$J_{i}^k$は$x_i$に関する$r_k$の偏微分行列です。

# グラフ最適化ライブラリの実装

上記の内容を完全に理解した後、ceres、gtsam、g2oではなく、独自のグラフ最適化ライブラリを実装できると思います。学習目的であるため、可読性が高く、Pythonで実装してみましょう。

以下に、独自のグラフ最適化ライブラリを要約して説明します。まず、Hとgを定義します。self.psizeはエッジのサイズの合計です。
``` python
H = np.zeros([self.psize, self.psize])
g = np.zeros([self.psize])
``` 
次は、(6)と(7)を使って、実際のグラフ化ライブラリを以下のように実装できます。

``` python
for edge in self.edges:  # すべてのエッジを処理する
    omega = edge.omega  # 共分散の逆　
    link = edge.link  # エッジのつながり情報
    r, jacobian = edge.residual(self.vertices)  # エッジごとにrとJを計算
    e2 = r @ omega @ r
    for i, v_i in enumerate(link):
        s_i = self.loc[v_i]
        e_i = self.loc[v_i] + self.vertices[v_i].size
        jacobian_i = jacobian[i] 
        g[s_i:e_i] += rho[1] * jacobian_i.T @ omega @ r  # ブログg_iを更新 式(7)
        for j, v_j in enumerate(link):
            s_i = self.loc[v_i]  # 頂点iのブロック開始番号
            e_i = self.loc[v_i] + self.vertices[v_i].size  # 頂点iのブロック終了番号
            s_j = self.loc[v_j]  # 頂点jのブロック開始番号
            e_j = self.loc[v_j] + self.vertices[v_j].size  # 頂点jのブロック終了番号
            jacobian_j = jacobian[j]
            H[s_i:e_i, s_j:e_j] += jacobian_i.T @ omega @ jacobian_j  # ブロックH_ijを更新 式(6)
```

最後には、式(2)を使用してdxを計算することができます。
そして、dxを式(3)の反復更新を利用し、収束させることができます。

``` python
if (self.use_sparse):
    dx = spsolve(csc_matrix(H, dtype=float), csc_matrix(-g, dtype=float).T)  # 式(2)
else:
    dx = np.linalg.solve(H, -g)  # 式(2)
```

## 実問題のデモ


独自のグラフ最適化ライブラリはシンプルに見えますが、実はceres、gtsam、g2oなどに劣らず、それなりの完成度でさまざまな実問題に対応できます。以下には3Dのループクロージングと経路計画問題のデモがあります。

### 2Dのループクロージング

[「ループクロージングの解説」](https://qiita.com/scomup/items/d5999f38a34b0d76f864)

データ:manhattanOlson3500 (E. Olson 2006)
左：ループクロージングする前、右：ループクロージングした後

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/149168/76daa9a2-8442-09d2-4918-e8ff332b9350.png" width="800"/>


### 3Dのループクロージング
データ:sphere2500 (M. Kaess 2012)
左：ループクロージングする前、右：ループクロージングした後

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/149168/d520576f-3e6e-9904-19a4-95849e8a1b8b.gif" width="800"/>

### 経路計画問題
障害物（青いポリゴン）との距離を考慮しながら、ロボット(緑の物体)が安全に走行できる最適な経路を生成するデモ

<img src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/149168/5afa5930-6618-5b6d-bc0a-13242162c15d.gif" width="600"/>

これらの実問題は回転など非線形の情報を含んでおり、**リー群シリーズ**の内容を理解した上で、リー群ベースの解法を利用することをおすすめします。詳細については長くなってしまうため、次回ではこれらのデモを解説しながら、非線形問題におけるグラフ最適化について説明します。

---
**リー群のシリーズ:**
* [1. 3次元回転群](https://qiita.com/scomup/items/fa9aed8870585e865117)
* [2. 3次元剛体変換群](https://qiita.com/scomup/items/d82b35aeb1ecabd75e60)
* [3. 並進速度と角速度の剛体変換](https://qiita.com/scomup/items/304277af1dbcd7e12cc8)
* [4. リー群を用いた最適化](https://qiita.com/scomup/items/a9c09d57101583c58619)

私の独自のグラフ最適化ライブラリは**githubに公開**していますので、ご自由に参考にしてください。

* グラフ最適化ライブラリ: graph_optimization/graph_solver.py

https://github.com/scomup/MathematicalRobotics