はじめに

　さくっと原理を勉強したいときにpythonで試せると効率がよいと思う。今回は、Graph Optimizationで有名なオープンソースフレームワークであるg2oをpythonで使う方法についてまとめてみる。

　g2oは、ICRA2011で発表されたGraph Optimizationを行うためのフレームワークで、SLAMで有名なORB SLAMで使われていたりする。本家はこちら。

　c++実装で実際に組み込む際にはこちらを用いるのが良いと思われる。ただ勉強目的に手軽に試したいとなった時はC++だとやや環境構築等が面倒だったりする。他の処理と組み合わせる時もpythonでサクっと試せないものかと思ったのが今回この記事を書いている動機。

　ググって調べて見ると、g2o-pythonなるものを見付けた。pybindを使って本家C++実装のg2oをpythonからよびだせるようにしたwrapperがある。pypiで容易にinstallもできてしまうので、セットアップではまることなく手早く原理の勉強に集中できる。

github.com

　今回はこのg2o-pythonを実際に使って、楕円フィッティングのサンプルコードを書いてみたので紹介しようと思う。サンプルコードは下記。正確性が欠けているとかもっと良い解法はきっとあると思うのだが、とりあえずg2o-pythonの使い方の解説に重きをおいて書いたので、その辺りご容赦願いたい。

github.com

　g2oがフレームワークとして提供してくれているFactor Graph Optimizationそのものについての理解・整理は別のエントリーでまた紹介したいと思う。今回はあくまで表面的にFactor Graph Optimizationを理解したつもりになって、非線形最適化ならこんな感じでとりあえず出来そう、と読んで頂いた方に思って頂けたら幸いである。

環境構築：

　pythonのパッケージ管理ツールは色々あるけれども個人的にpoetryが気に入っているので、poetryを使って今回構築した環境及びサンプルコードを下記に公開する。基本的にはpypiからg2o-pythonを入れるだけで使うことができた。上記repositoryのREADME.mdに記載したとおり、poetry installして頂ければ基本的に直ぐ使えるようになっている。

　個別にinstallしたい方はpipで普通にinstallして頂ければと使える思う。

pip install g2o-python

楕円フィッティング：

　ある楕円上の任意の点群が与えられたときに、その楕円を求める楕円フィッティング問題を考えてみる。長軸や短軸などの楕円形状を決めるパラメータを用いてどのような問題設定になるかまず記述して、それをGraph Optimizationのフレームワークで捉え直して、g2oで扱えるようにしていく。

問題設定：

　楕円の媒介変数表示を使って任意の形状を持った楕円上にある点のモデルを考える(図１)。楕円の形状は、中心座標 $(c_x, c_y)$ と長軸 $a$ ・短軸 $b$ および長軸の $x$ 軸正方向から測った傾き角 $\theta$ の5つのパラメータによって決まる。

　楕円上のN個の点は、媒介変数 $\xi_i (i = 1,2, ... N)$ を用いて以下で表すことができる。　

よって実際に得られたサンプル点の座標を $(\bar{x_i}, \bar{y_i})$ とすると楕円の形状パラメータ及び各点の媒介変数 $\xi_i$ はN+5個の変数を持つ最小二乗問題として下記の最適値として与えられる。

Graph Optimizationのフレームワークで捉える：

　式(1),(2)で与えられる楕円フィッティングの問題をgraph optimizationのフレームワークで書き下す。graphの構築において最適化のパラメータとしてのVertexとパラメータ間の制約としてのEdgeの二つを考える。楕円形状のパラメータ $(a,b,c_x,c_y, \theta)$ 、および楕円上のN個の点 $\xi_i (i = 1,2...N)$ をVertexとしたgraph構造は図２のようなN＋１個のVertexとN個のEdgeを持つGraph構造となる。各Edgeは楕円形状パラメータに基づくi番目の点の予測座標 $(x_i,y_i)$ と観測座標 $(\bar{x_i}, \bar{y_i})$ の2乗誤差となる。

　g2oはバックエンドで、ユーザが定義した最小二乗問題をGauss Newton(GN)法（それを改良したLevenberg Marquardt(LM)法）によって解いている。

g2oでの実装：

全体の流れ：

　graph構造で解きたい問題を書き下せたら、後はg2oを使って実装に落とし込むだけである。実装に落とし込む時の大雑把な作業ステップは下記。以下で順に見ていく。

Vertexをcalssとして記述
Edgeをclassとして記述
Solver/Optimizer定義・graph構築
反復計算

1. Vertexをclassとして記述

　上述したようにVertexは楕円形状Vertexと楕円上の点Vertexの二つがある。この2つのVertexをclassとして記述する。まずは、楕円形状Vertexの実装。

class VertexEllipse(g2o.VectorXVertex):
    """A circle parameterized by position x,y with radius a,b """

    def __init__(self) -> None:
        g2o.VectorXVertex.__init__(self)
        self.set_dimension(5)
        self.set_estimate([0] * 5)

    def oplus_impl(self, update) -> None:
        self.set_estimate(self.estimate() + update)

g2o.VectorXVertexは任意の次元ベクトルの状態をもつVertexを定義することができて、今回の場合は $(a,b,c_x,c_y, \theta)$ の5次元なのでself.set_dimension(5)で定義する。
self.set_estimate([0] * 5)で一旦状態の初期値をすべて0で与えておく。（乱暴な初期値だとGN法は往々にしてうまくいかないので、実際後でより良い初期値を与える。）
oplus_impl関数にはGauss Newton法によって求まった微小更新が過去の推定状態に対してどう作用するかを記述する。今回は単純に足し算で更新されるので、self.estimate() + updateとする。

続いて、楕円上の点Vertexの実装

class VertexTheta(g2o.VectorXVertex):
    """A angle of point on ellipse parameterized by theta """

    def __init__(self) -> None:
        g2o.VectorXVertex.__init__(self)
        self.set_dimension(1)
        self.set_estimate([0])

    def oplus_impl(self, update) -> None:
        self.set_estimate(self.estimate() + update)

ほとんど楕円形状Vertexと同様、 $\xi$ の一次元しかないので次元は1、更新式も単純加算で同様。

2. Edgeをclassとして記述

　続いて、Edgeを実装する。EdgeにはどのVertex同士を繋いでいるのかと、観測が与えられた時の誤差式（図２における赤四角）の2つを実装する。

class EdgePointOnEllipse(g2o.VariableVectorXEdge):
    def __init__(self) -> None:
        g2o.VariableVectorXEdge.__init__(self)
        self.set_dimension(1)  # dimension of the error function
        self.information()
        self.resize(2)  # number of vertices
        self.set_measurement([0, 0])  # initial measurement

    def compute_error(self):
        ellipse = self.vertex(0).estimate()
        theta = self.vertex(1).estimate()
        
        cx = ellipse[0]
        cy = ellipse[1]
        
        a = ellipse[2]
        b = ellipse[3]
        phi = ellipse[4]
        
        R = get_rot_mat_from_angle(phi)
        
        estimate = R @ np.array([a * np.cos(theta), b * np.sin(theta)]).squeeze() + np.array([cx,cy])
        
        error = np.linalg.norm(self.measurement() -  estimate)
        return [error]

N時点の単純なベクトルVertex同士を繋ぐg2o.VariableVectorXEdgeを利用する。
誤差関数の戻り値の次元を指定する。今回は単純のために1次元とした。(分散をちゃんと使ったモデルをちゃんと作るならここを2次元にしたりした方が良い。)
後程graph構築の際にこのedgeに2.で定義した楕円形状VertexとN個の楕円上の点Vertexを繋げる。Edge内ではself.vertex(idx)という形でつながっているVertexがindexで管理されており、self.vertex(0).estimate()といった形で、その時のVertexの状態を得ることができる。
compute_error関数内では式(1),(2)の各楕円上の点とその観測に基づく誤差関数を実装している。

3. Solver/Optimizer定義・graph構築

　solverとoptimizerを定義して、上記で定義したVertexとEdgeを使ってgraphを構築する。

optimizer = g2o.SparseOptimizer()
solver = g2o.BlockSolverX(g2o.LinearSolverDenseX())
solver = g2o.OptimizationAlgorithmLevenberg(solver)
optimizer.set_algorithm(solver)

GN法を実行するoptimizerと、最適化のためのアルゴリズムをsolverで定義する。
この辺りは問題の構造に基づいて適切な設定を与えることによって高速な演算が可能になる。
正直この辺り、私自身の理解がかなり怪しいところもあり一旦ほかのg2o公式サンプルプログラムを参考に上記BlockSolverX, LinearSolverDenseXアルゴリズムを設定した。

　続いてgraphを構築する。

ellipse: VertexEllipse = VertexEllipse()
ellipse.set_id(0)
ellipse.set_estimate([init_cx,init_cy,init_A,init_B,init_phi])  # some initial value for the circle
optimizer.add_vertex(ellipse)


est_rot = get_rot_mat_from_angle(init_phi)

for i,(point, angle) in enumerate(zip(points,init_angles),1):
        
    theta: VertexTheta = VertexTheta()
    theta.set_id(i)
    theta.set_estimate([angle])
    
    optimizer.add_vertex(theta)

    edge: EdgePointOnEllipse = EdgePointOnEllipse()
    edge.set_information(np.identity(1))
  
    edge.set_vertex(0, ellipse)
    edge.set_vertex(1, theta)

    edge.set_measurement(point)

    optimizer.add_edge(edge)

インスタンス化した楕円形状VertexとN個の楕円上の点Vertex及びEdgeをoptimizerに加えてgraph構造を定義する。
edgeにはedge.set_vertex(0, ellipse)といった形で、繋いているVertexをセットして内部で誤差計算に用いる。
edge.set_measurement(point)で各edgeの観測情報を与えている。
edge.set_information(np.identity(1))でinformation行列に1次元のIdentity行列を与えている。
- ここは、edgeのcompute_errorによって計算される誤差の次元がN次元の場合はN X Nのinformation行列となる。
- 今回は簡単のために、誤差の次元を1次元とし、かつ誤差に等方正規分布を仮定しているので、1次元のIdentity行列とした。
- 実際誤差の分布を考える場合はここにちゃんと誤差の分布を表した $I = \Sigma^{-1}$ を与える。

4. 反復計算

optimizer.initialize_optimization()
optimizer.set_verbose(True)
optimizer.optimize(max_iterations)

optimizerを初期化して、max_iterations回文のGN法のイテレーションを実行する。
optimizer.set_verbose(True)にすることで各itereationの収束過程が見れるので、debugしながらの行う場合はTrueにして実行すると便利。

実行結果

　上記実装で実際に $(c_x, c_y) = (4, 8), (a, b) = (12 ,2), \theta = 30°$ の場合に等方正規分布に従うランダムノイズを与えた100点のサンプル点を与えた際の推定結果が図３。点列が入力100点で、赤線が推定楕円形状パラメータに基づく推定である。初期値青線と比較して良く一致している様子が見て取れる。

　実際上記の結果を得るためには初期値の試行錯誤が必要で、愚直に適当な値の初期値を与えた場合はうまく収束がせずラフに推定した楕円形状パラメータと $\xi$ を初期値に最適化のイテレーションを回すことで安定して解に到達できるようになった。コンピュータビジョンのBundle Adjustment(BA)においても、まずはPerspctive-n-Point(PnP)でラフに6DoF姿勢とランドマーク座標を求めてからBAを実行するが、GN法においては素性の良い初期値を与えることが安定的に解に到達するうえでは重要であると考えられる。

まとめ：

　g2oを手軽に使うことのできるpython wrapper : g2o-pythonを用いてベーシックな問題の楕円フィッティング問題を解いてみました。g2oを用いる際は、Vertex, Edgeをclassとして定義してgraphを構築し、問題に適したsolverを使うことでGauss Newton法による非線形最適化問題を解くことが出来る。実用上大事なステップは、解こうとしている問題をGraph optimizationの枠組みで捉える最初の作業と、定式化した問題の構造が持つ疎行列等の特徴を把握して適切なSolverのアルゴリズムを選択することにあると思います（今回は理解も足りずサボっていますが、、）。また、g2oを用いる場合に限らず、GN法を用いて非線形最適化を行う際は適切な初期値を与えることもやはり重要であると思います。

改善点としては下記：

記事にも何度か記載した通り誤差を等方正規分布に従うとしてinformation matrixにIdentityを与えていましたが、誤差もちゃんとモデル化するところもトライしてみたいと思います。
初期値を推定するアルゴリズムが点列の $\xi$ が $U(0, 2 \pi)$ に従うことを前提にしてしまっているので、楕円の一部だけの点列が得られているような状況でも対応出来るように改善したいと思います。

　以上読んで下さりありがとうございました。本記事で何か改善できるポイントなどございましたら是非コメントにてご指南頂けると助かります。

追記

　今回の問題においてSolverと収束の早さを改めて調べて見た結果が図４。同じ楕円サンプルに対して、LinearSolverDenseX, LinearSolverEigenX, LinearSolverPCGの結果を比較した結果であるが今回の問題においては殆ど収束スピードに差がないことが見て取れる。

参考文献

コンピュータビジョン最先端ガイド 3 (CVIMチュートリアルシリーズ) 2010/12/8

BIOCV’s blog

pythonでg2oを利用する