はじめに
グラフ最適化（Graph Optimization）は、パラメータをグラフ構造で表現し、最適化問題を解決する手法です。特にロボティクスなどの領域で広く活用されています。
以下に、グラフ最適化の応用例をいくつか挙げます。
- Visual SLAMやSFMのバンドル調整（Bundle Adjustment）→「解説記事」
- Graph SLAMのループ閉じ込み問題→「解説記事」
- 経路計画問題(TEB, ebandなど)→(coming soon)
実際のアプリケーションでは、ceresやgtsam、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つのランドマーク（ドア）があり、ロボットには自分とドアの距離を測るセンサが搭載されています。また、自分の走行距離も測ることができます。
上記の図にロボット移動する様子を示しています。
この例を、グラフで表現すると、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$
下記のように、このグラフを可視化することができます。
解き方
すぐにお気づきかと思いますが、先ほどの情報はお互いに矛盾していますね。それは、ロボットの移動量やセンサーの値が真値ではなく、それぞれに独立した誤差が存在する測量値からです。エッジ$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)をベースの解き方を説明します。
以前の記事の復習になりますが、ガウス・ニュートン法は式(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はエッジのサイズの合計です。
H = np.zeros([self.psize, self.psize])
g = np.zeros([self.psize])
次は、(6)と(7)を使って、実際のグラフ化ライブラリを以下のように実装できます。
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)の反復更新を利用し、収束させることができます。
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のループクロージング
データ:manhattanOlson3500 (E. Olson 2006)
左：ループクロージングする前、右：ループクロージングした後
3Dのループクロージング
データ:sphere2500 (M. Kaess 2012)
左：ループクロージングする前、右：ループクロージングした後
経路計画問題
障害物（青いポリゴン）との距離を考慮しながら、ロボット(緑の物体)が安全に走行できる最適な経路を生成するデモ
これらの実問題は回転など非線形の情報を含んでおり、リー群シリーズの内容を理解した上で、リー群ベースの解法を利用することをおすすめします。詳細については長くなってしまうため、次回ではこれらのデモを解説しながら、非線形問題におけるグラフ最適化について説明します。
リー群のシリーズ:
私の独自のグラフ最適化ライブラリはgithubに公開していますので、ご自由に参考にしてください。
- グラフ最適化ライブラリ: graph_optimization/graph_solver.py