移動するロボットの「位置と向き」を、複数の移動観測から推定する。 2次元の姿勢(pose)は、x座標・y座標・向きの3つの値で表す。 姿勢グラフの頂点は各時刻のロボットの姿勢、辺は2つの姿勢間で観測された 相対的な移動と回転である。頂点はLiDARが測定した物体表面の点ではない。
隣接時刻の移動観測を積み上げるだけでは、観測ノイズが蓄積して軌跡がずれる。 以前いた場所を再訪したことによる観測(ループ閉合)も使って、軌跡全体の 観測との矛盾を小さくする。この処理を姿勢グラフ最適化(PGO)と呼ぶ。
本プログラムは、SLAMのうちこのバックエンド処理を対象にする。 点群処理やICP、ループ検出の代わりに、正解の軌跡とノイズ付き相対姿勢観測を プログラム内で生成する。大規模な疎連立一次方程式の求解には、疎コレスキー分解を使う。
滑らかな8の字の軌跡を複数周する。軌跡のパラメータtに対して、回転前の位置は
x = scale * sin(t)、y = 0.65 * scale * sin(2*t)
とし、接線方向を向きとする。
全姿勢を同じ座標変換で移し、最初の姿勢を原点・向き0にそろえる。
poses-per-lap * laps + 1。noise-scaleは生成するノイズに共通の倍率を
掛けるが、重みは変えない。0ならノイズなしの観測を生成できる。初期推定は、隣接移動の観測を順に積み上げて作る。 正解軌跡は観測生成と評価に使い、最適化の残差はノイズ付き観測と現在の推定値だけから作る。 固定シードを使う。同じ実行環境・設定で再現可能だが、標準ライブラリが異なる場合には 正規乱数の列が一致するとは限らない。
各辺について、現在の推定から予測する相対姿勢と観測値との差を計算する。 残差は、ローカル座標系の並進2成分と角度差1成分の計3成分である。 角度は約 -pi から pi の範囲に折り返す。目的関数は、重み付き残差二乗の総和の半分。 この実装は、この座標表現による残差を用い、SE(2)の対数写像による残差は用いない。
解析的なヤコビアンを使い、Gauss–Newton法の近似行列Hと右辺bを組み立てる。
全体を平行移動・回転できる不定性をなくすため、最初の姿勢は未知数から除く。
姿勢数Nに対し、未知数は 3*(N-1) 個になる。
1本の辺は2つの姿勢に依存するため、最大4個の3×3ブロックに寄与する。 Hを上下両側を持つブロックCSRで保存し、各辺が加算する場所は初期化時に求めておく。 毎反復では数値配列をゼロにし、辺を順に処理してHとbを組み立てる。
組み立て用のブロックCSRから、下三角部分をスカラーCSCの格納領域へ反映する。 各3×3ブロックの中も、個々のスカラー要素として分解する。
初版の消去順序は、時刻順・各姿勢内でx,y,角度の自然順序である。 記号解析では、変数の消去に伴って残りの隣接変数を互いに接続し、fill-inを求める。 集合を使った単純な実装なので、fill-inが多いと記号解析自体も重くなる。 因子Lの非ゼロ構造と、各列が参照する以前の列の一覧を作る。
グラフ構造は固定なので、記号解析と格納位置の対応付けは最初の1回だけ行う。 数値分解は列単位のleft-looking型で、既に計算した列の寄与を引き、 対角の平方根とその下の値を求める。密な全体行列には変換しない。 分解後は、前進・後退代入で修正量を求める。
行列の各対角に lambda * max(Hの対角値, 1e-12)
を加えて求解する。
修正後の目的関数が減ればステップを受理してlambdaを3分の1にする。
減らない場合や分解に失敗した場合はlambdaを10倍にし、同じ線形化を使って再求解する。
これは単純な減衰付きGauss–Newton法で、完全な信頼領域法の実装ではない。
停止条件は次のいずれかである。
max(1, 更新前の目的関数) で割った値が1e-10未満。修正量・目的関数による停止はlambdaが1e-3以下のときだけ使う。
再試行の上限まで改善できなければ no_acceptable_step
として終了し、終了コード2を返す。 反復上限は
iteration_limit と表示し、収束した場合と区別する。
ノイズがあるため、観測誤差の減少が毎回の真値RMSEの減少を保証するわけではない。 また、軌跡が重なる部分では画像の線も重なる。
--self-testでは、ヤコビアンの数値微分との比較、fill-inを持つ小さな行列での
既知の解・密行列解法との比較、因子からの行列再構成、数値分解の再利用、
非正定値の検出、PGOの組み立て・格納対応、ノイズなしの軌跡復元、
固定シードのノイズあり問題の改善を検証する。
密行列を使うのはこの小規模な検証だけである。
| 処理 | 将来のCPU/GPU並列化 | 制限や検討事項 |
|---|---|---|
| 残差・ヤコビアン評価 | 辺ごとに分担 | 不規則な姿勢参照 |
| Hとbの組み立て | 辺ごとの寄与の計算と集約 | 同じ格納位置への加算競合 |
| 数値分解 | 依存関係上独立な列や部分木、列をまとめたブロック更新 | 逐次依存、fill-in、仕事量の偏り |
| 三角求解 | 依存しない行群の同時処理 | 同期と長い依存関係 |
| 線形残差の確認 | ブロック行ごとの疎行列ベクトル積 | メモリ帯域 |
| 疎コレスキー分解 | ピボット要素確定後の各列更新 | 並列化可能量の動的な変化 |
std::chrono::steady_clockで、入力生成、ブロック構造、記号解析と対応付け、
行列組み立て、数値反映と減衰、数値分解、三角求解、線形残差確認、候補評価、
最適化全体をそれぞれ測る。残差・ヤコビアン評価は組み立てと同じ辺ループで行う。
optimization_totalは非線形反復全体(初期コスト評価、検証、反復ログ出力等を含む)。backend_with_setupはそれにブロック構造・記号解析の時間を加えたもの。A_lower_nnzとL_nnzは下三角の構造上の非ゼロ数で、対角とブロック内の明示的ゼロを含む。
fill_ratioは L_nnz / A_lower_nnz。まず小さい問題で動作を確認し、Miyabiの計算ノードで姿勢数とグラフ構造を変えながら、
数秒から数分程度の問題サイズを探す。自然順序ではfill-inが急増することがあるので、
姿勢数だけから所要時間を決めつけない。--max-factor-nnzは因子の構造サイズの上限であり、
バイト単位のメモリ制限ではない。
ファイルは 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や不要な大きい出力・一時ファイルを取り除き、必要なファイルだけにする。