SLAM中のMarginalization処理内容を明らかにしたい。

Visual-Inertial odometryなど調べると登場するMarginalization。実際どんな処理をするか、どんな意味があるか、など理解できずいろいろと調べてみた。この記事では調べてわかった事実を書きたい。いろいろと間違っていそうですが、メモ代わりに残しておく。基本は[1]の文献に沿って進める。したがってSidinig window filter / Fixed-lag smootherの話になる。本記事では、Visual odometryの問題設定とSystem matrixの計算まで記載する。

推定する状態の定義

推定したい状態量は、Sliding window内のカメラPose（6自由度）とSiding window内で観測したランドマークの位置（3自由度）。それぞれの記号は、Map中のランドマークを示す ${\boldsymbol{x}_m}$ とロボットのPoseを示す ${\boldsymbol{x}_p}$ としている。まとめて、 ${\boldsymbol{x}}$ とする。

$\boldsymbol{x} = \begin{bmatrix} \boldsymbol{x}_m\\ \boldsymbol{x}_p \end{bmatrix}$

詳しく書くと、 ${\boldsymbol{x}_m}$ はランドマーク(LM)の位置。Siding window中にはn個のLMが存在する。

$\boldsymbol{x}_m = \begin{bmatrix} \boldsymbol{x}_{m_1}\\ \boldsymbol{x}_{m_2}\\ ...\\ \boldsymbol{x}_{m_n} \end{bmatrix}$

${\boldsymbol{x}_p}$ は今までのPose。Siding window中にはm個のPoseが存在する。

$\boldsymbol{x}_p = \begin{bmatrix} \boldsymbol{x}_{p_1}\\ \boldsymbol{x}_{p_2}\\ ...\\ \boldsymbol{x}_{p_m} \end{bmatrix}$

VIsual odometryの問題設定

Prior information、Process model、Sensor modelの３種から構成する。それぞれのモデルについて確率密度関数(PDF)を設定、すべてのPDFをスタックすることでVisual odometryを形作る。また、ここではガウス分布を仮定しているので、それぞれのモデルについての平均、分散が設定できればOK。

Prior information

状態変数のうちいくつかは、ある時点での分布が求まっていると主張するのでPrior information。もしPrior informationがなければこの項は無視される。Photo metric BAなどがPiror informationが無い場合に相当する([1]のFig. 6[d]参照)。

Sliding window中の一番古いPoseと、その時点で観測されているLandmarkにPrior informationが設定されていると設定。（あくまで例として設定するだけ。いろいろなパターンが存在するはず。）平均値を ${\hat{\boldsymbol{x}}_\Pi}$ 、分散を ${\Pi}$ としてすすめる。

$\hat{\boldsymbol{x}}_\Pi = \begin{bmatrix} \hat{\boldsymbol{x}}_{m}\\ \hat{\boldsymbol{x}}_{p_1} \end{bmatrix}$

$\Pi = \begin{bmatrix} \Pi_{m} & \Pi_{mp} \\ {\Pi_{mp}}^T & \Pi_{p} \end{bmatrix}$

このとき、観測されているLandmarkの平均値:

$\hat{\boldsymbol{x}}_{m} = \begin{bmatrix} \hat{\boldsymbol{x}}_{m_1}\\ \hat{\boldsymbol{x}}_{m_2}\\ ...\\ \hat{\boldsymbol{x}}_{m_n} \end{bmatrix}$

Sliding window内の最も古いPoseの平均値: ${\hat{\boldsymbol{x}}_{p_1}}$

自分の考え

Visual odometryでは、前ステップ推定で求めた平均値と分散値を利用することになると予想している。前回求めた値を利用することの妥当性について、Siding wndowに含まれる一番古いPoseの値を再利用することから考えてみる。新たな観測値が得られたとしても、一番古いPoseの情報は大きく影響は受けないように思える。小さな誤差要因となる可能性はあるが大きな影響はなさそう。

疑問:

${\hat{\boldsymbol{x}}_\Pi}$ といった平均値はどのタイミングのものを利用するのか？
Prior informationを持つPoseはSiding windowのうちどこまで利用するのか？

Process information

現在の状態と制御入力から次のタイムステップの状態を推定するモーションモデルが相当する。ここでは次の式でモーションモデルを定義している。この式からPDFを作り出す。

$\boldsymbol{x}_{j+1} = f_j (\boldsymbol{x}_{j}, \boldsymbol{u}_{j+1}) + \boldsymbol{w}_{j+1}$

このとき、 ${\boldsymbol{w}_{j+1} \sim N(0, Q_{j+1})}$ である。したがってモーションモデルのPDFは次の通り。

$\boldsymbol{x}_{j+1} \sim N(f_j (\boldsymbol{x}_{j}, \boldsymbol{u}_{j+1}), Q_{j+1})$

このモーションモデルはSliding window内のPoseに対して定義できるので、状態 ${\boldsymbol{x}_{p}}$ に適用すると以下の通り。

$\boldsymbol{x}_{f} \sim N\left( \begin{bmatrix} \boldsymbol{x}_{p_2} = f(\boldsymbol{x}_{1}, \boldsymbol{u}_{2})\\ \boldsymbol{x}_{p_3} = f(\boldsymbol{x}_{2}, \boldsymbol{u}_{3})\\ ...\\ \boldsymbol{x}_{p_m} = f(\boldsymbol{x}_{m-1}, \boldsymbol{u}_{m}) \end{bmatrix} , Q=\begin{bmatrix} Q_2 & ~ & ~ \\ ~ & \ddots & ~ \\ ~ & ~ & Q_m \end{bmatrix} \right)$

Sensor information

システムの状態 ${\boldsymbol{x}}$ から対応する観測値を作り出すセンサモデルが相当する。Visual odometryではカメラに写ったランドマークの画像中座標系位置が観測値になる。つまりランドマーク ${\boldsymbol{x}_{m_i}}$ をカメラPose ${\boldsymbol{x}_{p_j}}$ から観測した時のカメラ座標系位置 ${\boldsymbol{z}_{ij}}$ を作り出すProjectionモデルが相当する。ここではステレオカメラを想定しているので、画像二枚分で ${\dim({\boldsymbol{z}_{ij})} = 4}$ 。

${\boldsymbol{z}_{ij}}$ について以下が成り立つとする。 ${h_{ij}}$ はProjection関数。

$\boldsymbol{z}_{ij} = h_{ij} (\boldsymbol{x}_{m_i}, \boldsymbol{x}_{p_j}) + \boldsymbol{v}_{ij}$

このとき、 ${\boldsymbol{v}_{ij} \sim N(0, R_{ij})}$ である。したがってセンサモデルのPDFは次の通り。

$\boldsymbol{z}_{ij} \sim N(h_{ij} (\boldsymbol{x}_{m_i}, \boldsymbol{x}_{p_j}), R_{jj})$

これを観測値全てについてスタックして以下のようなPDFにして利用する。観測値はランドマーク番号をメジャーとしてスタックする。

$\boldsymbol{z} \sim N\left( \begin{bmatrix} \boldsymbol{z}_{10}\\ \boldsymbol{z}_{10}\\ ...\\ \boldsymbol{z}_{nm} \end{bmatrix} , R=\begin{bmatrix} R_{00} & ~ & ~ \\ ~ & \ddots & ~ \\ ~ & ~ & R_{nm} \end{bmatrix} \right)$

また、System全体の基準Poseとスケール不定を回避するために、 ${\boldsymbol{x}_{p_0}}$ を準備している。これは定数として扱うため、後述するJacobianの計算には出現しない。ステレオカメラなのでPoseが一つあればスケール不定性を回避できる。

統合

上記３つのPDFをまとめると ${p(\boldsymbol{z} | \boldsymbol{x})p(\boldsymbol{x})}$ は以下の通り。

$p(\boldsymbol{z} | \boldsymbol{x})p(\boldsymbol{x}) = N\left( \begin{bmatrix} \boldsymbol{x}_{\Pi}\\ \boldsymbol{x}_{f}\\ \boldsymbol{z} \end{bmatrix} , C=\begin{bmatrix} \Pi^{-1} & ~ & ~ \\ ~ & Q & ~ \\ ~ & ~ & R \end{bmatrix} \right)$

System matrixの計算

最小２乗法を使って上記で求めた観測後の事後確率を最大にする状態を推定する。以下のコスト関数 ${g(\boldsymbol{x})}$ を設定して非線形最小二乗法を適用する。

$g(\boldsymbol{x}) = \begin{bmatrix} g_{\Pi}(\boldsymbol{x})\\ g_{f}(\boldsymbol{x})\\ g_{z}(\boldsymbol{x}) \end{bmatrix} = \begin{bmatrix} \boldsymbol{x}_{\Pi} - \hat{\boldsymbol{x}}_{\Pi}\\ \boldsymbol{x}_{f} - f(\boldsymbol{x})\\ \boldsymbol{z} - h(\boldsymbol{x}) \end{bmatrix} , C=\begin{bmatrix} \Pi^{-1} & ~ & ~ \\ ~ & Q & ~ \\ ~ & ~ & R \end{bmatrix}$

最小二乗法はGauss-newton法を利用して解く。各イテレーション中では、以下の式で求まる ${\delta \boldsymbol{x}_i}$ を使って初期状態 ${\boldsymbol{x}_0}$ から繰り返し修正することで最も観測値を説明できる状態まで持っていく。 ${i}$ はGauss-newton イテレーションにおけるイテレーション回数。

$G_i^T C^{-1} G_i \delta \boldsymbol{x}_i = - G_i^T C^{-1} g(\boldsymbol{x}_{i})$

このとき、

$G_i = \begin{bmatrix} \frac{\partial g_{\Pi}(\boldsymbol{x})}{\partial \boldsymbol{x}}(\boldsymbol{x}_i) = L\\ \frac{\partial g_{f}(\boldsymbol{x})}{\partial \boldsymbol{x}}(\boldsymbol{x}_i) = D\\ \frac{\partial g_{z}(\boldsymbol{x})}{\partial \boldsymbol{x}}(\boldsymbol{x}_i) = -H \end{bmatrix}$

としてコスト関数の状態変数についてJacobianになる。

System matrix

System matrixとはGauss newton時に出現する ${G_i^T C^{-1} G_i}$ の部分。この部分は状態間の関係を示していて逆行列が状態変数の分散になる。 ${G_i^T C^{-1} G_i}$ は以下のように分解でき、Prior information, Process model, Sensor model要素ごとの和となる。

$G^T C^{-1} G = \begin{bmatrix} L^T D^T -H^T\\ \end{bmatrix} \begin{bmatrix} \Pi & ~ & ~ \\ ~ & Q^{-1} & ~ \\ ~ & ~ & R^{-1} \end{bmatrix} \begin{bmatrix} L\\ D\\ -H \end{bmatrix} \\= L^T \Pi L + D^T Q^{-1} D + H^T R^{-1} H$

MarginalizationとSystem matrix

Sliding windowからPoseやランドマークの変数を除去する場合にMarginalizationという操作が行われる。ただ単に状態変数やSystem matrixから状態を除去(Drop)するとSystemから除去した状態に関する情報が失われ推定精度に影響する。このため、MarginalizationをSystem matrixに行うことで除去した変数に関する情報を埋め込むことを行う。ここで除去した変数の情報を埋め込んだSystem matrixは次回のPrior informationとして利用されるはず？？？？？（この部分が一番わからん。）次の記事では、Marginalizeを実施したときにSystem matrixにどういった変化が生まれるか調べる。

laala-kami.hatenablog.com

Memo

Prior informationの部分がMarginalizeの中核だと思う。前ステップでMarginalizeした結果がこの部分に貯められていくはず。ここの処理を明確にしたい。あと、ここに関係する平均値がなんの値かわかりにくい気がする。明快に ${\hat{\boldsymbol{x}}_{p_1}}$ と書いてある。けど、二回目以降は何になるの？増えていくの？それとも、 ${\hat{\boldsymbol{x}}_{p_2}}$ にすり替わるの？これがわからん。おそらくすり替わると思う。もし増えてしまったらSiding windowが広がっていることになるから。初回はゼロか、初期値としてダミーが入るか。次回以降は、Schur complementされたデカイ方の行列がここにはいると思っている。明日以降、続きかきます。詳しい人いましたらコメントなどで教えてほしいです。

参考文献

[1] Gabe Sibley, Gaurav Sukhatme and Larry Matthies. Visual Sliding Window SLAM with Application to Planetary Landers.

いろいろ日記

2020 11/3

今日考えたこと

Basaltで使っているHessianの計算方法

今後

Marginalizationについてわかっていること２（Marginalize時のSystem matrixの振る舞い）

System information matrix

Perform marginalization

Fill-in details

Marginalizationについてわかっていること１（System matrixの計算まで）