## 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にそろえる。

- 姿勢数は `poses-per-lap * laps + 1`。
- 隣接する姿勢間に移動観測の辺を作る。
- 空間的に近く、時刻が十分離れた姿勢間にループ閉合の辺を作る。
- 空間の格子で過去の姿勢を整理し、距離条件を満たす候補から近い順に選ぶ。
  1つの新しい姿勢から追加する辺数には上限がある。混雑した格子では候補数は増える。
- 相対移動は辺の始点のローカル座標系で表し、並進と角度に独立な正規ノイズを加える。
- 隣接移動の標準偏差は並進各成分0.02 m、角度0.005 rad。
  ループ閉合では並進各成分0.03 m、角度0.01 rad。
- 重みはこれらの公称分散の逆数とする。`noise-scale`は生成するノイズに共通の倍率を
  掛けるが、重みは変えない。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法で、完全な信頼領域法の実装ではない。

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

- 対角でスケーリングした勾配の最大絶対値が1e-8未満。
- 受理されたステップで、各姿勢の並進修正量が最大1e-7 m未満、角度修正量が最大1e-8 rad未満。
- 受理されたステップで、目的関数の減少量を `max(1, 更新前の目的関数)` で割った値が1e-10未満。
- 非線形反復の上限に到達。

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

## 5. 結果の確認

- 観測に対する目的関数の最適化前後の変化。
- 正解に対する位置RMSE（2次元の位置誤差の二乗平均平方根）と角度RMSE。
  座標系は最初の姿勢で固定しているので、評価時の追加の位置合わせは行わない。
- 実際に解いた減衰付き方程式について、残差ノルムを右辺ノルムで割った相対残差。
  1e-6を超える場合、または有限値でない場合はエラーとする。
- 画像：緑が正解、赤が初期推定、青が最適化後、黒い十字が固定した姿勢。

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

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

# Where is parallelism

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

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

- `optimization_total`は非線形反復全体（初期コスト評価、検証、反復ログ出力等を含む）。
- `backend_with_setup`はそれにブロック構造・記号解析の時間を加えたもの。
- 入力生成と画像・CSVの書き出しはバックエンド時間の外。
- 各内訳は総時間に含まれるため、総時間と内訳を重複して加算しない。
- 分解回数、受理・棄却回数も出力する。時間の変化と反復回数の変化を区別する。
- `A_lower_nnz`と`L_nnz`は下三角の構造上の非ゼロ数で、対角とブロック内の明示的ゼロを含む。
  `fill_ratio`は `L_nnz / A_lower_nnz`。

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

# Build and run

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


## 小規模検証と実行

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

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

既定サイズの例（901姿勢、実行時間はMiyabiで測定する）：

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

画像変換セル：

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

画像表示セル：

```python
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`や不要な大きい出力・一時ファイルを取り除き、必要なファイルだけにする。
