1次元カルマンフィルター

初めに

　本エントリーでは、カルマンフィルターの入門として1次元のカルマンフィルタ、即ち状態量が1つのみの場合をまとめてみたいと思います。別エントリーで、一般の任意の数の状態量を持つカルマンフィルターについても説明しようと思いますが、式の意味を理解する上で行列の知識が必要になるのでまずは単純な一次式でモデルが表される1次元の場合でカルマンフィルターについての考え方について整理します。 前提知識として条件付き確率分布についての知識が必要となるので、その辺りこちらのエントリーにも記載しているので理解して貰ってから読んで頂けるとよいかと思います。 biocv.hateblo.jp

　カルマンフィルターの概要を理解する上では、数値解析ソフトMatlabを開発しているMathWorks社が提供している以下の教材が非常に分かりやすかったのでお薦めです。

jp.mathworks.com

カルマンフィルターの目的

カルマンフィルターの目的は、直接観測が出来ない量について過去に得られている情報と現在得られている観測の情報を元に尤もらしい値を推定することです。具体的な例を挙げると、上記MathWorks社の教材内でも取り上げられているエンジンの内部温度を予測する例を考えます（図１）。

図１：エンジンの内部温度の予測

<

ある時刻$t$のエンジン内部温度:$y_t$を知りたいのですが、エンジンの内部は激しい化学反応が起きており直接温度計を仕掛けて測ることが出来ません。そこでエンジン容器を伝わって来た熱を容器の外側に取り付けた温度計によって観測した温度$Z_t$でエンジンの内部温度を予測する事を考えます。エンジン内部の温度は、時系列的に変化するので時刻$t$以前の温度の情報${y_1,y_2,...,y_{t-1}}$および温度を上げたり下げたり制御するための制御入力:$u_t$を予測に用いることができます。 既に説明したとおり、カルマンフィルターでは直接値を知ることが出来ない以上、具体的な確定値ではなくある程度不確からさをもった予測値をモデルによって求めるというスタンスをとります。具体的には、どの値がどのくらいの確からしさであり得そうなのか、つまり”確率分布”をモデルによって求めます。上記のエンジンの例では、内部温度を確率変数として扱うことで時刻$t$の$y_t$がどのような確率分布になっているかをモデルによって求めます。$y_t$が既知の分布であればそのパラメータ、例えば正規分布の場合は時刻$t$の平均と分散が求まれば良いです。

図２：確率変数としての状態量

<

カルマンフィルターのモデル

一般的な場合1次元の場合のカルマンフィルタをモデル化し、モデルを用いて状態量を推定するステップについて述べて、その際重要となるカルマンゲインについても導出します。 1次元の状態量：$y_t$が一つ前の時刻$t-1$の状態量：$y_{t-1}$ と制御入力：$u_t$によって決まる状態遷移モデルを考えます。状態量とは一般に直接観測できない情報であり、カルマンフィルターの目的は以前の情報と現在得られている観測情報:$z_t$から状態量$y_t$を推定することです。グラフで表すと図３のようになります。

図３：状態遷移モデル

<

1次元のカルマンフィルターでは$y_t$が$y_{t-1}$と$u_t$の1次の式、および$z_t$が$y_t$の1次の式で表される以下のような式の関係で状態遷移が表される場合を考えます。式(1)を1つ前の状態から今の状態を”予測する”モデルとして「予測モデル:Prediction Model」といい、図３において赤い矢印で表されるエッジです。式(2)を得られている状態と観測情報の関係を表すモデルとして「観測モデル：Observation Model」といい、図３において緑の矢印で表されるエッジです。また、それぞれのモデルには誤差$\epsilon_t$と$\eta_t$がありそれぞれ平均が0で分散が$R^2,Q^2$の正規分布であるとします。 $y_t$,$u_t$,$z_t$と$\epsilon_t$,$\eta_t$はtによらず独立であるとします。

$$ y_t=a_t y_{t-1} + b_t u_t + \epsilon_t \tag{1} $$

$$ z_t = h_t y_t + c_t + \eta_t \tag{2} $$

$$ \epsilon_t \sim N(0, R^2) \tag{3} $$

$$ \eta_t \sim N(0, Q^2) \tag{4} $$

ここで時刻$t$において得られている情報:$I_t$を以下の様に定義します。 $$ I_t \equiv \{Z_1,Z_2,Z_3,...,Z_t,u_1,u_2,...,u_t\} \tag{5} $$ つまり$I_t$は時刻$t$の時点で得られている情報の全てで、具体的には$t$以前の全ての観測と制御入力です。この情報:$I_t$を用いることによって図３のグラフは図４のように単純化することができ、$y_t$は一つ前の時刻$t_1$までに得た情報：$I_{t-1}$とその時刻の制御入力$u_t$によって決まることを表しています。

図４：情報量を使った簡略化

<

カルマンフィルターでは状態量$y_t$を確率変数として扱い、ここでは正規分布に従うとします。

予測モデル

以上で述べたモデルを用いて$t-1$までに得ている情報から、$t$の状態を予測する予測のモデルを書き下します。具体的には、$y_{t-1}$の分布から$y_t$の分布を求めるが正規分布を仮定しているので、時刻$t$の平均と分散が時刻$t-1$の平均と分散によってどう表されるかを考えれば良いです。

$$ \hat{y}_{t|t-1}=E[y_t|I_{t-1}, u_t] \tag{6} $$

$$ \Sigma_{t|t-1} = Var[y_t|I_{t-1},u_t] \tag{7} $$

式(1)-(4)を使って計算すると平均は、

$$ \begin{align} \hat{y}_{t|t-1}&=E[a_t y_{t-1} + b_t u_t + \epsilon_t|I_{t-1},u_t] \\\ &= E[a_t y_{t-1}|I_{t-1}] + E[b_t u_t |I_{t-1}]+ E[\epsilon_t|I_{t-1},u_t] \\\ &= a_tE[ y_{t-1}|I_{t-1}] + b_t u_t \\\ &= a_t \hat{y}_{t-1|t-1} + b_t u_t \end{align} \tag{8} $$

分散は、

$$ \begin{align} \Sigma_{t|t-1}&=Var[a_t y_{t-1} + b_t u_t + \epsilon_t|I_{t-1},u_t] \\\ &= Var[a_t y_{t-1}|I_{t-1}] + Var[b_t u_t |I_{t-1}]+ Var[\epsilon_t|I_{t-1},u_t] \\\ &= {a_t}^2Var[ y_{t-1}|I_{t-1}] + R^2\\\ &= {a_t}^2 \Sigma_{t-1|t-1} + R^2 \end{align} \tag{9} $$

まとめると予測モデルは式(10)になります。

$$ \begin{align} \hat{y}_{t|t-1} &= a_t \hat{y}_{t-1|t-1} + b_t u_t \\\ \Sigma_{t|t-1} &= {a_t}^2 \Sigma_{t-1|t-1} + R^2 \end{align} \tag{10} $$

更新モデル

予測モデルでは、時刻$t-1$までの情報$I_{t-1}$を使って１つ先の時刻：$t$の状態$y_t$の予測値：$y_{t|t-1}$の分布を求めました。更新モデルでは、時刻$t$に得られている情報の全て：$I_t$を使って更に精度を高めるべく、予測値：$y_{t|t-1}$の分布から更新値：$y_{t|t}$の分布を求めます。 下記の式(11),(12)によって$y_{t|t}$の平均と分散を更新値を得ることが出来ます。

$$ \hat{y}_{t|t}=E[y_t|I_{t}] \tag{11} $$

$$ \Sigma_{t|t} = Var[y_t|I_t] \tag{12} $$ これらを展開して、予測値$y_{t|t-1}$から$y_{t|t}$を求める更新式を導き出します。それには冒頭で述べた別エントリーにて紹介した条件付き確率の平均・分散の関係を用います。 具体的には、2変数正規分布に従う確率変数$\bf{X}=(X_1,X_2)$について、$X_2$の実現値$x_2$が得られた時の条件付確率分布: $X_1|X_2$は以下で表されるのでこの関係式を用います。ここで$\rho$は$X_1$と$X_2$の相関係数です。

$$ X_1|X_2 \sim N(\mu_1 + \frac{Cov(X_1,X_2)}{Var[X_2]}(x_2-\mu_2), Var[X_1](1-{\rho}^2)) \tag{13} $$

式(13)の関係式を用いて式(11)と式(12)を展開します。 まず平均$\hat{y}_{t|t}$は、

$$ \begin{align} \hat{y}_{t|t}&=E[y_t|I_{t-1},u_t,z_t] \\\ &= E[y_{t|t-1}|z_t] \\\ &= \hat{y}_{t|t-1} + \frac{Cov(y_{t|t-1},z_t)}{Var[z_t]}(z_t - (h_t \hat{y}_{t|t-1} + c_t)) \\\ &=\hat{y}_{t|t-1} + K_t(z_t - (h_t \hat{y}_{t|t-1} + c_t)) \end{align} \tag{14} $$

$$ \begin{align} \Sigma_{t|t}&=Var[y_t|I_{t-1},u_t,z_t] \\\ &= Var[y_{t|t-1}|z_t] \\\ &= Var[y_{t|t-1}] (1-{\rho}^2) \end{align} \tag{15} $$

式(15)における相関係数$\rho$は以下になります。 $$ \rho = \frac{Cov(y_{t|t-1},z_t)}{\sqrt{Var[y_{t|t-1}]} \sqrt{Var[z_t]}} \tag{16} $$

式(14)の第二項の係数がカルマンゲイン:$K_t$でありこれを具体的に計算します。 まず分母となる観測$z_t$の分散$Var[z_t]$は、

$$ \begin{align} Var[z_t]&= Var[h_t y_{t|t-1} + c_t + \eta_t] \\\ &={h_t}^2Var[y_{t|t-1}] + Q^2 \\\ &={h_t}^2\Sigma_{t_t-1} + Q^2 \end{align} \tag{17} $$

となります。 よって$K_t$は、

$$ \begin{align} K_t&=\frac{Cov(y_{t|t-1},z_t)}{Var[z_t]} \\\ &= \frac{Cov(y_{t|t-1},h_t y_{t|t-1} + c_t + \eta_t)}{Var[z_t]} \\\ &= \frac{h_tCov(y_{t|t-1},y_{t|t-1})}{Var[z_t]} \\\ &= \frac{h_tVar[y_{t|t-1}]}{Var[z_t]} \\\ &= \frac{h_t\Sigma_{t|t-1}}{{h_t}^2\Sigma_{t|t-1} + Q^2} \end{align} \tag{18} $$

となります。 分散$\Sigma_{t|t-1}$は、式(16)と式(18)を用いるとカルマンゲイン:$K_t$を用いて

$$ \begin{align} \Sigma_{t|t}&= Var[y_{t|t-1}] (1 -\frac{{Cov(y_{t|t-1},z_t)}^2}{Var[y_{t|t-1}] Var[z_t]} ) \\\ &=Var[y_{t|t-1}] (1 - \frac{{K_t}^2 {Var[z_t]}^2}{Var[y_{t|t-1}] Var[z_t]}) \\\ &=Var[y_{t|t-1}] (1 - h_t K_t) \\\ &=\Sigma_{t|t-1}(1-h_tK_t) \end{align} \tag{19} $$

まとめると更新モデルは式(20)になります。

$$ \begin{align} \hat{y}_{t|t}&=\hat{y}_{t|t-1} + K_t(z_t - (h_t \hat{y}_{t|t-1} + c_t)) \\\ \Sigma_{t|t}&＝\Sigma_{t|t-1}(1-h_tK_t) \\\ K_t&=\frac{h_t\Sigma_{t|t-1}}{{h_t}^2\Sigma_{t|t-1} + Q^2} \end{align} \tag{20} $$

更新モデルの直感的理解

式(16)と式(18)からカルマンゲイン$K_t$は以下のようにも書ける。 $$ K_t = \rho \frac{\sqrt{Var[y_{t|t-1}]}}{\sqrt{Var[z_t]}} \tag{21} $$ これは、予測と観測の相関係数に対して互いの不確かさの標準偏差の比を重みにかけた値が、フィルタリングの係数になっていることを表しています。 $\rho$は時刻$t$までの観測$z_t$と予測$y_t$の相関であり、何ら相関のない観測が得られている場合は0に近く極めて相関の高い観測が得られている場合は1に近づきます。 標準偏差の比はそれぞれの分布の確からしさで正規化する役割をしていて、観測の標準偏差が分母なので観測があまり確からしくなければ更新は小さくなり、確かであれば大きくなることを表しています。 逆に分子の予測の標準偏差が小さい、即ち十分に確度が高く予測できている状態においてはあまり観測は反映されず、更新は小さくなることを表しています。

まとめ

カルマンフィルターの目的について確認し、さらに1次元のカルマンフィルターつまり状態量が1つのみの場合のカルマンフィルターについて正規性を仮定した下で予測と更新のモデル式(10)と(20)を導出しました。カルマンフィルターを何となくではなく、実感をもって理解するにはこの式の展開を自分で一度やってみるところは避けて通れないところなのかなと思っています。本エントリーでは正規性を仮定して導出を行いましたが、より緩い制約としての不偏性さえ満たせばカルマンフィルターとしては成立します。そちらの導出に関しては、下記に上げる参考書等をご確認下さい。 実用上の問題では、複数の値を持つ多次元の状態量を扱う必要がある場合が殆どなので、別エントリーにて一般の多次元のカルマンフィルターについても解説したいと思います。

参考

• 経済・ファイナンスのための カルマンフィルター入門 (統計ライブラリー, 森平 爽一郎著, 2019/2/6)
• MathWorks "カルマンフィルターとは？"(https://jp.mathworks.com/discovery/kalman-filter.html)

多変数正規分布の条件付き期待値・分散

はじめに

　ある確率変数の実現値が分かった時の別の確率変数の確率分布を条件付確率分布という。本エントリーでは、多変数正規分布においての条件付確率分布・期待値・分散について、式の導出やその意味について考察をしたので備忘録も兼ねて紹介しようと思う。別エントリーにて書く予定のカルマンフィルターを理解する上で、条件付確率分布の理解はその土台となる。条件付期待値や分散の式が持つ意味合いについてしっかりと理解すると、カルマンフィルタの更新式の理解も深まると思います。そのあたり自分と同様不安な方は、よりクリアにして頂ければ幸いです。

2変数正規分布の条件付き確率

2変数正規分布に従う確率変数$\bf{X}=(X_1,X_2)$がある。

$$ \bf{X} \sim N( \bf{\mu}, \Sigma) \tag{1} $$ $$ \bf{\mu}=(\mu_1, \mu_2) \tag{2} $$ $$ \Sigma = \begin{bmatrix} {\sigma_1}^2&\rho\sigma_1 \sigma_2 \\\ \rho\sigma_1 \sigma_2 & {\sigma_2}^2\end{bmatrix} \tag{3} $$ $$ \rho = \frac{ Cov(X_1,X_2)}{ \sqrt{Var(X_1)}\sqrt{Var(X_2)}} \tag{4} $$

$\mu_1$と$\mu_2$および$\sigma_1$と$\sigma_2$は$X_1$と$X_2$の期待値・標準偏差であり、$\rho$は相関係数である。 この時、周辺分布$X_2$の分布ついても正規分布となる。

$$ \bf{X_2} \sim N(\mu_2,{\sigma_2}^2) \tag{5} $$

以上のもとで$X_2$の実現値$x_2$が得られた時の条件付確率分布: $X_1|X_2$について考える。$X_1|X_2$の確率密度関数$f(x_1|x_2)$は以下の式によって与えられる。

$$ f(x_1|x_2)=\frac{f(x_1,x_2)}{f(x_2)} \tag{6} $$

式(1)と(5)の確率密度関数$f(x_1,x_2)$と$f(x_2)$を具体的に書き下すと、

$$ f(x_1,x_2)=\frac{1}{2\pi\sqrt{\Sigma}} e^{-\frac{1}{2}{(\bf{x}-\bf{\mu})}^T\Sigma^{-1}(\bf{x}-\bf{\mu})} \tag{7} $$ $$f(x_2)=\frac{1}{\sqrt{2\pi}\sigma_2}e^{-\frac{1}{2{\sigma_2}^2}{(x_2-\mu_2)}^2} \tag{8}$$

となる。 式(3)の成分を用いて$\sqrt{|\Sigma|}$を計算すると、

$$ \sqrt{|\Sigma|} = \sqrt{1-{\rho}^2}\sigma_1 \sigma_2 \tag{9} $$

となる。 また、式(7)の指数部分も同様に具体的に計算すると、

$$ \begin{multline} -\frac{1}{2}{(\bf{x}-\bf{\mu})}^T{\Sigma}^{-1}(\bf{x}-\bf{\mu}) \\\ ＝-\frac{1}{2{\sigma_1}^2 {\sigma_2}^2(1-{\rho}^2)} \lbrace {\sigma_2}^2{(x_1-\mu_1)}^2 + {\sigma_1}^2{(x_2-\mu_2)}^2 \\\ - 2\sigma_1 \sigma_2 \rho(x_1-\mu_1)(x_2-\mu_2) \rbrace \tag{10} \end{multline} $$

これより、式(6)は、

$$ f(x_1|x_2)=\frac{1}{\sqrt{2\pi}\sigma_1\sqrt{1-{\rho}^2}}e^{-\frac{1}{2{\sigma_1}^2(1-{\rho}^2)} \lbrack x_1 - \lbrace \mu_1 + \rho \frac{\sigma_1}{\sigma_2}(x_2-\mu_2) \rbrace \rbrack} \tag{11} $$

となる。 よって、$X_2=x_2$の実現値が得られたときの$X_1|X_2$は以下の平均と分散を持つ正規分布に従うことがわかる。

$$ X_1|X_2 \sim N(\mu_1 + \rho \frac{\sigma_1}{\sigma_2}(x_2-\mu_2), {\sigma_1}^2 (1-{\rho}^2)) \tag{12} $$

式(4)を用いると式(13)のようにも書ける。

$$ X_1|X_2 \sim N(\mu_1 +\frac{Cov(X_1,X_2)}{Var(X_2)}(x_2-\mu_2), {\sigma_1}^2 (1-{\rho}^2)) \tag{13} $$

条件付確率の期待値・分散の意味

　式(12)の結果の意味について考えてみる。まずは分散${\sigma_1}^2 (1-{\rho}^2)$について、相関係数$\rho$が$-1 \leq \rho \leq 1$であることに注意すると、$X_2=x_2$の観測が得られることによって必ず$X_1|X_2 = x_2$の分散は小さくなるか変化しないことを表している。これが意味するところを感覚的に捉えれば、$X_1$と$X_2$に何かしらの相関がある場合に、$X_2$に何かしらの情報が得られた際、それが相関係数$\rho$を介して伝搬して$X_1$の情報に影響を与え、予測の確度を高める、ということである。

　東京の気温と山梨の気温を考えたときに、これらは地理的に近いため一定の相関を有していると考えられる。つまり、ある日の東京の気温($X_1$)の期待値の確度（分散）は、山梨の気温($X_2$)の実測値を知らない場合より、知っている場合のほうが確度が高まる。より具体的には、山梨の気温が高かった場合には、それを知る前よりも東京の気温の予測値を高い方向へ更新し、さらに更新前よりもその予測に対しての自信が高くなる、ということを意味している。これは普段我々が行っている推測と何ら変わらず、極自然に感じられると思う。 　実際に式(11)が意味するところはそういうことで、図を書いてみると図１のようになる。

図１：条件付き期待値・分散の更新のイメージ

　期待値：$\mu_1 + \rho \frac{\sigma_1}{\sigma_2}(x_2-\mu_2)$の意味についても図１を眺めることによって雰囲気がつかめると思う。元々思っていた$X_2$の予測値：$\mu_2$に対して実際得られた実現値：$x_2$との差、$(x_2-\mu_2)$に$\rho \frac{\sigma_1}{\sigma_2}$の影響度合いをかけて$\mu_1$に足すことで$X_2=x_2$を知ることによる$X_1$の予測値を更新している。ここで傾き$\rho \frac{\sigma_1}{\sigma_2}$は相関係数:$\rho$に$X_1$と$X_2$の標準偏差の比をかけたものになっている。$X_1$と$X_2$に全く相関がない場合は、$\rho=0$となり、図１左側のようになる。この場合は、$X_2=x_2$を知ったところで何らこれらの変数には関係がないので$X_1$の予測は更新されない、ということになる。標準偏差の比：$\frac{\sigma_1}{\sigma_2}$については、$X_2$の標準偏差：$\sigma_2$が小さい、即ち知る情報の確度が高いほど更新が大きくなり、また$X_1$の標準偏差：$\sigma_1$が大きいほど、予測の確度が低いほど更新が大きくなることを表している。確度が高い$X_2$の情報を得るほど$X_1$の予測が更新され、$X_1$の予測の確度がもともと高ければ、$X_2$の情報が得られてもあまり更新しない、というのも一般的な感覚からも受け入れ易いと思う。

まとめ

　正規分布の条件付き期待値と分散についてまとめました。2変数の場合の式を導出し、導き出される期待値と分散についてその意味を考えてみました。式(12)・(13)が意味するところをしっかりと理解するところが様々な応用において重要かと思います。途中式(9),(10)の展開などは省略しましたが、単純な展開なので記事で長々と紹介するよりも、気になる方は手を動かして納得して頂くのがよいかと思います。

　カルマンフィルタでは時系列のデータを扱い、過去の情報からの予測を現在得られる観測によって更新し続けます。この時の予測の更新に式(13)が登場します。なのでカルマンフィルタを理解するという特定の目的においても本記事で書いた内容は重要だと思います。カルマンフィルタについてはまた別エントリーで記載する予定です。

以上、お読み頂きありがとうございました。

参考文献

1. 現代数理統計学の基礎（共立講座 数学の魅力 11）(2017/4/11),久保川 達也 (著)

pythonでg2oを利用する

はじめに

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

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

github.com

　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で扱えるようにしていく。

問題設定：

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

図１：任意形状の楕円

　楕円上のN個の点は、媒介変数を用いて以下で表すことができる。 　

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

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

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

図２

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

g2oでの実装：

全体の流れ：

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

1. Vertexをcalssとして記述

2. Edgeをclassとして記述

3. Solver/Optimizer定義・graph構築

4. 反復計算

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を定義することができて、今回の場合はの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と同様、の一次元しかないので次元は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行列とした。
  • 実際誤差の分布を考える場合はここにちゃんと誤差の分布を表したを与える。

4. 反復計算

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

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

実行結果

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

図３：推定結果

　実際上記の結果を得るためには初期値の試行錯誤が必要で、愚直に適当な値の初期値を与えた場合はうまく収束がせずラフに推定した楕円形状パラメータとを初期値に最適化のイテレーションを回すことで安定して解に到達できるようになった。コンピュータビジョンの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を与えていましたが、誤差もちゃんとモデル化するところもトライしてみたいと思います。
• 初期値を推定するアルゴリズムが点列のがに従うことを前提にしてしまっているので、楕円の一部だけの点列が得られているような状況でも対応出来るように改善したいと思います。

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

追記

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

図４：Solverの種類と収束の様子

参考文献

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

Harris Corner Detectionの解説

内容

　SLAMやsfm等をエッジデバイスで動かす際に、軽量なHarris corner detectionは今なお広く用いられている。なんとなくは理解していたが、コーナー検出のための具体的な発想や式の意味を咀嚼しきれていなかったので記事としてまとめてみた。

　最初にコーナーを検出するためのアイデアを感覚的に捉えて、それを数学的にモデルにするにはどうしたら良いかを後半に記載した。コーナーネスの式の意味がいまいち捉えきれていない初学者等の理解の助けになれば幸いです。

目次

1．コーナ検出とは

2．Harris corner detection

3．まとめ

4.   参考文献

１．コーナ検出とは

　特徴点検出は、画像検索やSLAMなどのコンピュータービジョンのタスクで用いられる基本的な処理の一つである。画像から特徴的な点を抽出し、さらに特徴量を計算して後段処理に繋げることが多い。例えば、連続する画像間の特徴点をトラッキングして姿勢推定をしたり、特徴量の類似度によって似ている画像を検索したりする(図１)。

図１：特徴点検出

　特徴点はその名の通り特徴的で何かしら意味がありそうな点で、それらを自動で抽出するアルゴリズムの一つが今回紹介するHarris corner detectionである。画像中での特徴として代表的なのは色と形で、形の特徴としてエッジ、コーナーやブロブなどの幾何的特徴がある（図２）。

　幾何的特徴の中でもコーナー点は扱いやすく、Harris corner detectionはコーナー検出の手法として広く用いられている。

図２：特徴点検出

　画像からコーナーとなる点を自動抽出する処理について具体的に考えてみる。大雑把には、画像中の全てのピクセルに対してコーナーっぽさを計算し、コーナーである点とそうでない点の区別を行う。形の特徴は、画像の輝度がどう分布しているか、すなわち輝度の分布パターンとして捉えることができる。そこでコーナー点とそうでない点を区別するために、点周囲の領域内の輝度のパターンの違いを用いる。注目する点を中心とした数ピクセル辺長の正方領域が一般的に用いられ、カーネルと呼ばれる。

　Harris corner detectionでは、画像の局所に登場する形のパターンを図３に示したように平坦、エッジ、コーナーの3つへ分類する。よって、平坦、エッジ、コーナーのそれぞれで正方形の小領域内の輝度のパターンが識別できれば、コーナーを抽出することができる。

　輝度のパターンとして具体的には、輝度が領域内でどう変化するかの変化のパターンを用いる。エッジとコーナーは、どちらも輝度が大きく変化する点だが、変化の仕方に違いがある。一方で平坦な点においては輝度が殆ど変化しない。

図３：平坦・エッジ・コーナー

　平坦、エッジ、コーナーの輝度変化のパターンを詳細に整理する。輝度の変化は、画像の横と縦方向に輝度のパターンがどう変化するかであり、より具体的には正方形の領域内の輝度の合計の変化量である。図３右に示したように、コーナーとは横方向と縦方向の両方に輝度変化がある点ということが分かる。エッジは横か縦どちらか一方に輝度変化が生じる点である。横方向のエッジは横に動かしても殆ど輝度が変化しないが、縦方向には変化がある。逆に縦方向のエッジは縦方向に動かしても殆ど輝度が変化しないが、横方向には変化がある。平坦方向はどちらに動かしても輝度に変化がない点となる。

　以上で述べたとおり、画像からコーナーを抽出するためには、ある注目する点周辺の正方領域内の輝度変化のパターンを計算し、それらを特定の基準で3つへ分類してコーナーとなる点を選べばよい。図4はHarris corner detectionのフローで、輝度変化のパターンを数学的に記述する“特徴量記述”と、基準に基づいてコーナーを判定する”分類”という２つの処理からなる。Harris corner detectionは頑健で計算効率の高いモデルであり、1988年に発表されてから今なお広く用いられている手法となっている。

図４：Harris corner detectionの処理フロー

２．Harris corner detection

　続いてHarris corner detectionの“特徴量記述”および”分類”方法について詳細に見ていく。

2.1. 局所領域の特徴量記述

　平坦・エッジ・コーナーを特徴付けるのは、輝度の変化の仕方と捉えることができる。平坦な領域では周囲と比較して輝度が殆ど変化しないし、エッジやコーナは周囲と比較して輝度の変化のある領域になっている。この周辺領域との輝度の変化を数学的に表して、分類を行う。

　変化を表すためには、微分という数学の道具が便利で画像の輝度について横と縦方向に微分することで変化の仕方が表せる。

2.1.1. 画像の微分

　画像は2次元なので、横と縦のx,y方向それぞれの輝度の微分：との値を扱う。注目する画素に対して横幅wと縦幅hのカーネルという矩形領域を定義し、このカーネルの中の輝度の合計値の変化でとを定義する。

　図５はカーネルの横幅wと縦幅hが両方3である場合の例を表しており、x方向とy方向の微分とはそれぞれの方向に1ピクセル動かしたときのカーネル内の輝度の合計値の差分を計算することで求めることができる。x方向に向けて明るくなるような画像領域であればは正の値に大きくなるし、段々と暗くなるような画像領域であればが負の値に大きくなる。

図５：３ｘ３カーネルの場合

2.1.2. 平坦・エッジ・コーナーのIxとIy

　一旦、境界がx軸とy軸に平行な場合のみを考えて、平坦・エッジ・コーナーそれぞれの場合で輝度の微分：とがどのような値になるかを考えてみる。

平坦領域の場合

　平坦領域の場合、カーネル内の輝度の合計値はx,y方向にカーネルを移動させても変化しない（図６）。よって平坦な領域における点はを満たす点となる。

図６：平坦領域

エッジ領域の場合

　続いてエッジ領域の場合を考える（図７）。エッジに関してはx,y方向のいずれかについては、カーネルを動かすと輝度の合計値が変化するが、一方は変化しない。したがって、となる領域である。実際はx,y軸方向のいずれかに平行なエッジとは限らないので開店も考慮する必要があるが、それについては後述する。

図７：エッジ

コーナ領域の場合

　コーナーの場合、x,y方向のどちらに変化させてもカーネル内の輝度の合計値に変化が生じる。したがってコーナのみとなる領域である（図８）。

図８：コーナーの場合

2.2. コーナーの判定

2.2.1. 回転の考慮

　ここまででコーナー点とは画像の輝度の変化、すなわち2次元の微分とを考えたときにとなる点である事が分かった。なので画像中の全てのピクセルに対してとを計算してで判定すれば良い気がするが話はそう単純ではない。上でも説明したとおりで実際は回転を考慮する必要がある。

　図９は、斜めの場合のエッジでとを計算した場合を表しているが、この場合IxもIyも0ではなく結果となってしまっている。

図９：斜めの場合のエッジ

　なのでエッジの方向を考えた上でとを計算してあげる必要があり、図１０はエッジの方向を何らかの方法で見付けた上でエッジ面と平行、垂直な方向にそれぞれx'とy'座標をとり直してとを計算した場合を表した図になっている。この場合、であるがとなり、既に述べたxy軸にエッジ面が平行な場合と一致する。

図１０：座標の方向を取り直した場合

　よって次の興味としては、任意の向きのエッジ面に対してどのようにしてxy座標をx'y'座標に変換する回転を求めるのか、ということとなる。2次元の変換は、一つの2x2行列で表すことができる。2次元の変換は回転と軸方向へのスケール倍の合成なので、任意の変換行列は回転行列とスケール倍の行列へ分解することができる。具体的には後で出てくるのだが、固有値分解によってこの分解を行うことができる。

2.2.2. 対角化による輝度変化方向の同定

　Harris corner detectionの具体的なアルゴリズムに関わる内容について見ていく。Harris corner detectionの大まかな流れは、コーナっぽさの指標となるコーナーネスを数学的に定義し、その大きさによってコーナかコーナでないかの判定を行うという2つのステップからなる。

　これまで見て来たとおり、ある領域に注目してその領域内の輝度の変化量が起こる方向を見る事で平坦領域なのか、エッジなのかコーナなのかが判定できる。しかし、その方向は2.2.1.で見たとおり任意の回転を考慮しなければならない。よって予めその回転方向を知らない状態では、まずはあらゆる方向に手当たり次第動かしたときの輝度の変化量を全て計算して、輝度の変化が大きい方向を後から同定して、回転を求めることが必要になる。

　そこで、harris corner detectionでは注目する点の周辺領域として横幅w、縦幅hのカーネル:を使い、この内のピクセル:における輝度:の合計値の変化量を考える。任意の方向に変化させたときの内のの変化量を定義し、これを解析することで回転とコーナーネスを計算する（図１１）。

図１１：u,v方向の輝度変化量

ここで、は輝度の合計値を内で計算するときの重み付けの関数である。注目する点に近いほど重み付けを大きくし、遠いほど小さくするのが自然でガウシアン分布が用いられることが多い。は2次の項以降を無視したテイラー展開をによる近似を用いると次のようになり、

これを、の式に代入すると、

行列を使って整理すると、

ここで、行列を定義し、これは方向への変化に対してどれだけ輝度変化が起きるかを表す行列となる。

このを対角化すると、

ここでは回転行列で、は軸方向へのスケール倍の行列である。

で回転された新たな座標を考えると、

図１２：,方向への変換

　このは輝度変化が最も起こる方向またはに対する輝度の変化度合いで、とはそれぞれの方向に対する輝度の変化度合いということになる（図１２）。したがってここで2.1.2.で述べた平坦・エッジ・コーナーのと方向の輝度変化量を考えると、とが共に大きい場合がコーナーっぽさが強く、どちらか一方のみが大きい場合はエッジっぽさが強いということになる。このとの取る値による平坦・エッジ・コーナーの分布を図示したものが図１３である。

図１３：とによる分布

　ここまでの流れを整理すると、画像中の各ピクセルに対してを計算して対角化を行い、得られたとの値を図１３の境界にあたる閾値によってコーナーか否かを判定するのがコーナー抽出の処理の流れとなる。

2.2.3. コーナーネスの定義

　2.2.2.で述べた方法はピクセル数分の対角化演算が必要で処理が重い。そこでharris corner detectionの大きな貢献として、これまで述べたとの値による判定に相当する判定を、以下の式の値で表されるコーナーネスだけで判定する事ができる点に注目し、処理を大きく軽量化することに成功した。

ここで、は行列式、はトレースで、kは調整パラメータで経験的に0.04～0.06の値が用いられる。一般に行列式は固有値の積に等しいという性質を用いることで上式はとを用いて下記で表すことができる。

　このコーナネスCを用いることで、とを計算するだけで、2.2.2.でみたピクセルごとの固有値分解の計算をしなくて済む。図１４にとに対するCの分布を示す。図１３の対応をみるとCが大きければコーナーである事が分かり、Cをコーナネスとして用いることができることが分かる。具体的には、閾値thを設定し、C > thとなるピクセルとコーナーとして抽出する。とを対角化によって求めずとも、によって直接計算したを用いれば、コーナーの判定を行うことができる。

図１４：コーナネスの分布

3. まとめ

　輝度の変化のパターンに注目し、画像中の領域を平坦・エッジ・コーナーの三つに分類してコーナーネスを定義する考え方について述べた。画像の微分とを用いて2x2の行列の成分を解析することで平坦・エッジ・コーナーが分類できるが回転を考慮するためには固有値解析を行う必要があることを確認した。

　Harris corner detectionはこの考え方に従いつつ、演算量の大きいピクセル毎の固有値解析を避けるためにコーナーネスを定義し、その単純な閾値判定によってコーナー抽出が行える手法を提案した。このコーナーネスの考え方は、SIFTにおける安定したキーポイントの絞り込みにも応用されており、特徴点検出において重要な考え方の一つである。

4. 参考文献

• 画像認識 (機械学習プロフェッショナルシリーズ), 原田達也著, 2017

• OpenCV: Harris corner detector