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

Last updated at 2024-02-27Posted at 2023-11-21

はじめに

グラフ最適化（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)