CNNとdepth推定を用いたオドメトリの算出 前編(CNN SLAM #1)

CNN SLAMというSLAMに深層学習を組み合わせた手法が登場して、私自身大変興味があるので、実装を通して理解しようと思い、この記事をしたためることにしました。

deep learningを使ってend-to-endでやっちゃうよ！　というよりかはあくまでシステムの一部としてdeep learningを取り入れているという印象です。

処理の概要

RGB画像のみを入力とし、depth推定とセグメンテーションの2つをCNNを用いて推定しています。depth推定ができれば、あとはRGB-Dカメラを用いたSLAMと同様の手法が使えます。どんな手法を採用するにせよ、カメラの移動量(回転行列と並進ベクトル)すなわちオドメトリを求めることが必須となります。

ただし、このdepth推定の処理に時間がかかるということと、真値と一致する保証がない（不確実性が存在する）という問題があります。

前者の処理時間の問題は、key-frame Initializationという手法で解決を図ります。流れてくるRGBの画像すべてにCNNを適用するのではなく、キーフレームごとにCNNを適用させようというやり方です。間引きみたいな感じですね。

後者の推定誤差が残る問題は、Frame-wise Depth Refinementで解決を図ります。ここは少し難しいので後の章で述べます。

いきなりすべてを実装すると上手くいかないので、今回はCNN SLAMにおけるカメラのオドメトリの算出をRGB画像とdepth画像を用いて試してみたいと思います。なので、depth推定も今回はしておりません。これもいずれ加えていくという感じです。

夏休みが終わる頃にはCNN SLAMをマスターできてるっていう計画ですよ。

オドメトリの算出

r({\bf u}, {\bf T}) = I_{k_i}({\bf u}) - I_t(\pi({\bf K}{\bf T}_{t}^{k_i} V_{ki}({\bf u})))

時刻tにおいて画像が得られたとき、時刻$t$に最も近いキーフレーム取得時の時刻を$k_i$からのオドメトリ（移動量）$T$を算出します。$u$は画像の各ピクセルの座標値を指しており、各ピクセルにおける輝度値をもとに、変化前と変化後で、どう画像が変化したかをピクセルごとに求めます。なお、$K$はカメラの内部パラメータで、$V$関数は画像座標系(2次元)からカメラ座標系(3次元)への変換を行います。$\pi$関数はその逆処理になります。

この$r$関数は1ピクセルにおける処理なので、各ピクセルごとの$r$関数の総和をとり、その誤差の最小化を図ります。

$\rho$関数や$\sigma$関数がついてますね。ここらへんのコスト関数の設計については私もまだよくわかっておりません。あると精度が上がるのかしらん。（要更新）

E[{\bf T}_{t}^{k_i}] = \sum_{u \in \Omega}{\rho(\frac{r({\bf u}, {\bf T}_{t}^{k_i})}{\sigma(r({\bf u}, {\bf T}_{t}^{k_i}))})}

今回はシンプルに$r({\bf u}, {\bf T}_{t}^{ki})$の$T$に対する最適化を考えます。すなわち、自乗和($\sum{r^2}$)の最小化を狙います。最小化の手法としては最急降下法が有名ですが、論文ではガウス・ニュートン法を用いているらしいので、こちらを採用してみます。

上記のwikipediaへのリンク内の記事における$r$はこれまで説明した通りです。なので、あとはヤコビアンさえ求まれば最適化ができるということになります。

ヤコビアンというのはこういう行列です。

J = \left(
  \begin{array}{c}
    \frac{\partial {\bf r}_1}{\partial {\bf x}_1} & \ldots & \frac{\partial {\bf r}_1}{\partial {\bf x}_n} \\
    \vdots & \ddots & \vdots \\
    \frac{\partial {\bf r}_m}{\partial {\bf x}_1} & \ldots & \frac{\partial {\bf r}_m}{\partial {\bf x}_n}
  \end{array}
\right)

各ピクセルにおける$r$関数による残差をn個$x$のパラメータで偏微分したものを並べたものになっています。今回はパラメータ$x$は最適化すべき角度$(\alpha, \beta, \gamma)$と並進$(x, y, z)$の6個ありますので、$n=6$となります。残差を求めるときのIは画像の輝度値を取得しているので、ここは数学的に微分ができません。したがって、数値微分をとります。以下のような式で微分を近似します。$h$は微小な値であることを前提とします。1e-04とか1e-07とかそういう値です。

\frac{\partial r}{\partial x} = \frac{r(x + h) - r(x- h)}{2h}

これでヤコビアンも求められるので、あとはガウス・ニュートン法に書いてあるように反復計算をしていくのみです。

Depth画像の補正

先述した通り、depth"推定"をしているので、当然のことながら不確かさが残ります。論文ではこの不確かさを数式で定義しています。画像は時系列に沿って与えられるものであり、強い相関があるのでこれを利用して推定精度の向上を図ろうというわけです。

具体的には現在のkey-frameにおけるdepth推定と不確かさのマップと過去のkey-frameにおけるそれらを有するとき、不確かさを重みとして、不確かさが小さいほうの推定結果に重みを置いて、現在のdepth推定に補正をかけていくという手法をとります。

現在のkey-frameのインデックスを$k_i$、それより1つ前のkey-frameのインデックスを$k_j$とすると、時刻$k_i$における不確かさのマップ$U_{k_i}$は以下のように求まります。

U_{k_i}({\bf u}) = D_{k_i}({\bf u}) - D_{k_j}(\pi({\bf K}{\bf T}_{k_j}^{k_i} V_{ki}({\bf u})))

depth画像がちゃんと求められていて、移動量$T$もちゃんと計算できていると仮定すれば、不確かさはゼロですね。
また、時刻$k_j$における不確かさのマップも以下のように更新します。

\tilde{U}_{k_j}({\bf v}) = \frac{D_{k_j}({\bf v})}{D_{k_i}({\bf u})}U_{k_j}({\bf v}) + \sigma_p^2 \\
{\bf v} = \pi({\bf K}{\bf T}_{k_j}^{k_i} V_{ki}({\bf u}))

$\sigma_p^2$はノイズを付与することを意味しています。
不確かさのマップは、値が大きいほど不確かであることを意味するので、値が小さければ小さいほど、その推定結果をより強く信頼するということになります。

これらを用いた更新式は論文中の式(8),(9)に書いてある通りです。

論文中の3.4節ではステレオマッチングを用いることで、各時刻tに対するdepth画像を推定し、これを用いて時刻tにおけるkey-frameのdepth画像を補正しています（key-frame取得時の時刻は$k_i$）。ステレオマッチングはカメラ2台を用いて両眼視差を求めることにより、景色の奥行きを推定するものですが、今回は時系列に沿って流れてくる画像2枚をもとにして、奥行きを推定していることになります。

今後の予定

実際にPythonコードで実装し試してみましたので、コードも含めて今後アップしていきます。