GraphSLAMバックエンドの2次元姿勢グラフ最適化

Problem

移動するロボットの「位置と向き」を、複数の移動観測から推定する。 2次元の姿勢(pose)は、x座標・y座標・向きの3つの値で表す。 姿勢グラフの頂点は各時刻のロボットの姿勢、辺は2つの姿勢間で観測された 相対的な移動と回転である。頂点はLiDARが測定した物体表面の点ではない。

隣接時刻の移動観測を積み上げるだけでは、観測ノイズが蓄積して軌跡がずれる。 以前いた場所を再訪したことによる観測(ループ閉合)も使って、軌跡全体の 観測との矛盾を小さくする。この処理を姿勢グラフ最適化(PGO)と呼ぶ。

本プログラムは、SLAMのうちこのバックエンド処理を対象にする。 点群処理やICP、ループ検出の代わりに、正解の軌跡とノイズ付き相対姿勢観測を プログラム内で生成する。大規模な疎連立一次方程式の求解には、疎コレスキー分解を使う。

How the algorithm/baseline code works

1. 入力と初期推定

滑らかな8の字の軌跡を複数周する。軌跡のパラメータtに対して、回転前の位置は x = scale * sin(t)、y = 0.65 * scale * sin(2*t) とし、接線方向を向きとする。 全姿勢を同じ座標変換で移し、最初の姿勢を原点・向き0にそろえる。

初期推定は、隣接移動の観測を順に積み上げて作る。 正解軌跡は観測生成と評価に使い、最適化の残差はノイズ付き観測と現在の推定値だけから作る。 固定シードを使う。同じ実行環境・設定で再現可能だが、標準ライブラリが異なる場合には 正規乱数の列が一致するとは限らない。

2. 最小二乗問題と行列の組み立て

各辺について、現在の推定から予測する相対姿勢と観測値との差を計算する。 残差は、ローカル座標系の並進2成分と角度差1成分の計3成分である。 角度は約 -pi から pi の範囲に折り返す。目的関数は、重み付き残差二乗の総和の半分。 この実装は、この座標表現による残差を用い、SE(2)の対数写像による残差は用いない。

解析的なヤコビアンを使い、Gauss–Newton法の近似行列Hと右辺bを組み立てる。 全体を平行移動・回転できる不定性をなくすため、最初の姿勢は未知数から除く。 姿勢数Nに対し、未知数は 3*(N-1) 個になる。

1本の辺は2つの姿勢に依存するため、最大4個の3×3ブロックに寄与する。 Hを上下両側を持つブロックCSRで保存し、各辺が加算する場所は初期化時に求めておく。 毎反復では数値配列をゼロにし、辺を順に処理してHとbを組み立てる。

3. 疎コレスキー分解

組み立て用のブロックCSRから、下三角部分をスカラーCSCの格納領域へ反映する。 各3×3ブロックの中も、個々のスカラー要素として分解する。

初版の消去順序は、時刻順・各姿勢内でx,y,角度の自然順序である。 記号解析では、変数の消去に伴って残りの隣接変数を互いに接続し、fill-inを求める。 集合を使った単純な実装なので、fill-inが多いと記号解析自体も重くなる。 因子Lの非ゼロ構造と、各列が参照する以前の列の一覧を作る。

グラフ構造は固定なので、記号解析と格納位置の対応付けは最初の1回だけ行う。 数値分解は列単位のleft-looking型で、既に計算した列の寄与を引き、 対角の平方根とその下の値を求める。密な全体行列には変換しない。 分解後は、前進・後退代入で修正量を求める。

4. 減衰付き非線形反復

行列の各対角に lambda * max(Hの対角値, 1e-12) を加えて求解する。 修正後の目的関数が減ればステップを受理してlambdaを3分の1にする。 減らない場合や分解に失敗した場合はlambdaを10倍にし、同じ線形化を使って再求解する。 これは単純な減衰付きGauss–Newton法で、完全な信頼領域法の実装ではない。

停止条件は次のいずれかである。

修正量・目的関数による停止はlambdaが1e-3以下のときだけ使う。 再試行の上限まで改善できなければ no_acceptable_step として終了し、終了コード2を返す。 反復上限は iteration_limit と表示し、収束した場合と区別する。

5. 結果の確認

ノイズがあるため、観測誤差の減少が毎回の真値RMSEの減少を保証するわけではない。 また、軌跡が重なる部分では画像の線も重なる。

--self-testでは、ヤコビアンの数値微分との比較、fill-inを持つ小さな行列での 既知の解・密行列解法との比較、因子からの行列再構成、数値分解の再利用、 非正定値の検出、PGOの組み立て・格納対応、ノイズなしの軌跡復元、 固定シードのノイズあり問題の改善を検証する。 密行列を使うのはこの小規模な検証だけである。

Where is parallelism

処理 将来のCPU/GPU並列化 制限や検討事項
残差・ヤコビアン評価 辺ごとに分担 不規則な姿勢参照
Hとbの組み立て 辺ごとの寄与の計算と集約 同じ格納位置への加算競合
数値分解 依存関係上独立な列や部分木、列をまとめたブロック更新 逐次依存、fill-in、仕事量の偏り
三角求解 依存しない行群の同時処理 同期と長い依存関係
線形残差の確認 ブロック行ごとの疎行列ベクトル積 メモリ帯域
疎コレスキー分解 ピボット要素確定後の各列更新  並列化可能量の動的な変化 

性能測定

std::chrono::steady_clockで、入力生成、ブロック構造、記号解析と対応付け、 行列組み立て、数値反映と減衰、数値分解、三角求解、線形残差確認、候補評価、 最適化全体をそれぞれ測る。残差・ヤコビアン評価は組み立てと同じ辺ループで行う。

まず小さい問題で動作を確認し、Miyabiの計算ノードで姿勢数とグラフ構造を変えながら、 数秒から数分程度の問題サイズを探す。自然順序ではfill-inが急増することがあるので、 姿勢数だけから所要時間を決めつけない。--max-factor-nnzは因子の構造サイズの上限であり、 バイト単位のメモリ制限ではない。

Build and run

ファイルは pgo.cc(PGO本体)と sparse_cholesky.h(疎直接法)。 コマンドラインの一覧と既定値は ./pgo --help で表示できる。

小規模検証と実行

カーネルごとに一度 %load_ext miyabi を実行し、実行セルには例えば次を書く。

%%sub
cd code
./pgo --self-test && ./pgo --poses-per-lap 64 --laps 2 --iterations 20 --seed 1 --image trajectory.ppm

既定サイズの例(901姿勢、実行時間はMiyabiで測定する):

%%sub
cd code
./pgo --poses-per-lap 300 --laps 3 --loop-neighbors 2 --iterations 20 --seed 1 --image trajectory.ppm --csv trajectory.csv

画像変換セル:

%%bash
cd code
convert trajectory.ppm trajectory.png

画像表示セル:

from IPython.display import Image, display
display(Image("code/trajectory.png"))

--loop-neighbors 0でループ閉合なしの比較ができる。 この場合、初期軌跡は隣接観測を積み上げているため、初めから目的関数がほぼゼロでも、 正解に対するずれは残る。--noise-scale 0も初期軌跡がほぼ正解になる。 ノイズなしで非線形更新を実際に試す検証は、初期姿勢に摂動を加える--self-testで行う。

ローカル検証と提出

小規模なローカル確認は g++ -O2 -Wall -Wextra -std=c++17 pgo.cc -o pgo でビルドできる。 大きな計算や性能測定はMiyabiの計算ノードで行う。 ノートブックのコンパイル・実行セルは自分で記入し、そのまま実行できることを確認する。 提出前に実行ファイルpgoや不要な大きい出力・一時ファイルを取り除き、必要なファイルだけにする。