Visual OdometryをCUDAで高速化した話

はじめに

自動運転を支える技術の一つに、Visual Odometryというものがあります。これはカメラに映った景色の変化からカメラ自身の動きを推定するというものです。 私は3週間フィックスターズのインターンとして参加し、このVisual Odometryの処理の高速化に取り組みました。

本記事ではVisual Odometryで用いた手法の概要と、その高速化について書いていこうと思います。

Visual Odometryで用いたアルゴリズム

今回用いたアルゴリズムは大まかに以下の3つのステップからなります。3次元復元を行うため、入力としてステレオカメラで撮像した左右の画像を使用します。

1. 基準フレーム(≒前回フレーム)の画像に対して特徴点を抽出し、各特徴点の位置と輝度を記録
2. それぞれの特徴点について、視差情報から3次元位置を復元
3. 「1.で記録した輝度」と「2.の結果を現在フレーム上に投影して得られる輝度」との二乗誤差が最小になるような現在フレームのカメラの姿勢を推定

今回は、3.の姿勢推定の部分をCUDAで実装し、高速化を行いました。

特徴点抽出

特徴点とは、簡単に言うと画像中の目印のことです。抽出される特徴点の例として色の境界、壁の模様、建物の角などがあり、テーブルの面など同じ色が連続している領域では特徴点が抽出されにくい傾向にあります。

今回は 特徴点抽出アルゴリズムとしてFASTアルゴリズムを使用しており、これによってコーナーを高速に検出することができます。

ステレオマッチングによる3次元復元

左右のカメラで撮像した画像から3次元的な位置情報を得ることは、一般にステレオマッチングと呼ばれています。近くにある物体ほど左右で大きくずれて見えることを利用して奥行きの推定を行うことができ、これによって特徴点の3次元的な位置を計算することができます。

以下は特徴点に対して奥行き推定を行った結果です。小さな丸が特徴点に対応し、カメラとの距離が近いものは緑色で、遠いものは青色で描かれています。

問題の数学的な定式化

それぞれの特徴点において、記録しておいた輝度と観測される輝度の二乗誤差が最小になるようなカメラの姿勢を求めるには、どのように計算すれば良いでしょうか。

現在フレームのカメラの姿勢を ${\bf T}_c$ とすると、 ある ${\bf T}_c$ が与えられた際に3次元上の特徴点 ${\bf x}_i$ が現在フレームのどこに映るかが分かります。これを  $ {\bf u}_{c,i} = \pi_{{\bf T}_c} ({\bf x}_i) $  のように書きます。$ \pi_{{\bf T}_c}$ は3次元上の特徴点を画像上に投影する関数を表します。

${\bf u}_{c,i}$ における現在フレームの輝度 $I_c({\bf u}_{c,i})$ と、あらかじめ記録しておいた基準フレームの輝度 $I_r({\bf u}_{r,i})$ との誤差を $r_i$とします。$r_i$ の二乗を全ての特徴点について足し合わせたものを評価値 $E$ とすると、これは ${\bf T}_c$ によって決まるため $E({\bf T}_c)$ と書け、結局 $E({\bf T}_c)$ を最小化するような $ {\bf T}_c $ を求めるという問題に帰着されます。

$ {\bf u}_{c,i} = \pi_{{\bf T}_c} ({\bf x}_i) $

$ r_i = I_c({\bf u}_{c,i}) – I_r({\bf u}_{r,i}) $

$ {\rm minimize} \ E({\bf T}_c) = \sum_i r^2_i $

これはカメラの姿勢を表す6つのパラメータを変数とした、非線形最小二乗問題となります。

ガウス・ニュートン法

ガウス・ニュートン法は、非線形最小二乗問題を繰り返し計算で解くアルゴリズムです。

計算において必要となるのは、輝度差 $r_i$ を縦に並べた残差ベクトル ${\bf r}$ と、各パラメータに対する変化量の割合を並べた勾配(ヤコビアン) ${\bf J}$ の2つであり、以下の方程式

${\bf H} = {\bf J}^{\rm T}{\bf J} $

${\bf b} = {\bf J}^{\rm T}{\bf r} $

${\bf H}{\delta {\bf t}} = -{\bf b} $

を解くことでパラメータの更新量 $\delta {\bf t}$ を求めカメラ姿勢を更新、評価値が改善されなくなるまでこれ繰り返します。

なお、初期値の決定については「カメラは等速度運動する」という仮定のもとで、 前フレームの姿勢に前フレームのカメラの移動量をそのまま足した結果を初期値としています。

TukeyのBiweight推定法

推定におけるロバスト性(外れ値に対する頑健さ)を高めるために、TukeyのBiweight推定法を使用します。

これは誤差の二乗を足し合わせる際に、誤差の大きなデータに対してその重みを小さくすることによって外れ値の影響を減らす方法です。

この際、あらかじめ残差ベクトルに係数をかけてスケーリングする必要があり、係数は残差ベクトルの絶対値の中央値(Median absolute deviation)をもとに計算されます。

CUDA高速化の方針と実装

カメラの姿勢推定において、計算回数が多い処理はどこなのかを考えます。

分析の結果、計算回数が特徴点の個数に比例する処理がボトルネックになっていることが分かりました。

具体的には、以下の計算処理が高速化の対象となります。

1. 残差ベクトル ${\bf r}$ の計算
2. 勾配(ヤコビアン) ${\bf J}$ の計算
3. Biweight推定法におけるスケーリング係数の計算
4. ${\bf H}$ と ${\bf b}$ の計算

実際の計算規模ですが、 各フレームで特徴点をおよそ2000個抽出しており、また特徴点を中心とした周囲3×3画素の輝度値を誤差計算に使用しています。したがって、 残差ベクトル ${\bf r}$ のサイズは9×2000程度、ヤコビアン ${\bf J}$ のサイズは6×9×2000程度 となります。

${\bf J}$と${\bf r}$の計算

一つ目と二つ目の計算においては、特徴点間に依存関係が無いため並列化することができます。具体的には、各特徴点に一つのスレッドを割り当てて並列化を実現しました。

Biweight推定法におけるスケーリング係数の計算

三つ目の計算はCUDA化と近似解に置き換えることで高速化を行いました。

Biweight推定法におけるスケーリング係数は誤差の絶対値の中央値から計算されます。中央値を求める際は一度ソートを施してから中央の要素を抜き出すため、配列サイズ N に対して O(NlogN) の計算量がかかってしまいます。

そこで、このソートの処理をThrustライブラリで書き直してみたところ、ソートの実行速度が4.5倍になりました。

また、この係数は外れ値の判断基準には影響しますが、厳密な中央値から計算する必要はないと考えました。そこで、いくつかの要素をランダムに抽出して近似的に中央値を求めることで、この部分の処理を高速化しようと考えました。

近似計算を試してみたところ、収束するまでのイテレーション回数や予測精度に影響を与えずに実行速度をさらに2.5倍に向上させることができ、これらの工夫によって本計算のパフォーマンスを11倍に向上させることに成功しました。

${\bf H}$と${\bf b}$の計算

四つ目の計算については、過去のインターンにて既に高速化がなされていました。以下でその実装についても触れたいと思います。

${\bf H}$ と ${\bf b}$ の計算は行列積の計算となりますが、かけ合わせる要素は特徴点ごとに独立しているため並列に実行することができます。しかし、計算結果の和をとる部分は完全に並列化することは出来ません。

和をとる際に各スレッドがアトミック(不可分)に同じ変数にアクセスする必要がありますが、「待ち状態」が大量に生まれてしまいます。そこで、計算結果をshared memoryに格納しブロック単位でまとめあげ、格納先の変数にアクセスするスレッドを極力減らそうと工夫されていました。

計算結果をブロック単位でまとめあげる処理ではインターリーブペア実装が使われていました。(以下の図参照)

この図においては、「4つ右の要素を自分に加える」→「2つ右の要素を自分に加える」→「1つ右の要素を自分に加える」と操作を行うことで全部の要素の和を計算しています。

インターリーブペア実装では長さ N の配列の要素の和を logN 回の並列計算によって求めているため、一要素ずつ足し合わせる方法よりも高速に計算することができます。

データ転送の削減

CUDA実装の基本的な流れは「データ転送」→「並列計算」→「計算結果の転送」となりますが、各処理に対してこの流れに沿って実装したところデータ転送の時間が増えてしまい、逆に遅くなってしまいました。

そこで、共通したデータをGPUに転送したらそのデータを使いまわす、細かいデータは一つの構造体にまとめて転送する等の工夫を施すことで、極力GPU-CPU間のデータ転送を削減するように改良しました。

スレッド数のチューニング

スレッドが少なすぎると並列化の恩恵を受けられませんし、多すぎる場合もスレッドが余ってしまい非効率になってしまいます。スレッド数を適切な値に調整することで、さらに高速化できる可能性があります。

実際にスレッド数のチューニングを行ったところ、全体のパフォーマンスを1割程度改善することが出来ました。

実行時間を計ってみる

GTX 1060を搭載したマシン上でプログラムを動かして実行時間を計測しました。以下は姿勢推定の処理時間を比較したものです。

高速化前と比べて、姿勢推定の処理速度を5倍に向上させることができました。

一方で、特徴点抽出やステレオマッチングも合わせた全体の処理時間を見ると、実行時間は20%程度しか短縮されませんでした。どうやら、ステレオマッチングが処理時間の大半を占めているようです。

libSGMと連携してみる

libSGM(https://github.com/fixstars/libSGM)というライブラリをご存じでしょうか。このライブラリはフィックスターズ社で開発された、ステレオマッチングをCUDA上で行うライブラリです。

先ほどの図ではステレオマッチングが大きな割合を占めていたので、このライブラリと連携することでボトルネックを解消できるかもしれません。

libSGMと連携したおかげで処理時間は全体で11.8ミリ秒程度と非常に高速になり、 リアルタイム処理にも耐えうる速度を達成できました。

おわりに

libSGMとの連携も含めると全体の処理速度は4倍に向上し、私としても非常に満足のいく結果になりました。

メンターをしてくださった高木章洋さんと、行列積計算の高速化を行ってくれた鈴木凌斗さんに感謝いたします。

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください。