Incremental smoothing（増分平滑化）

SLAMは成長する推定問題である。新しいキーフレームが追加されるたびに、変数とファクターがファクターグラフに追加されていく。ステップごとに非線形最小二乗問題全体をゼロから解く（バッチ最適化）方式は、軌跡が長くなるほど高コストになる——しかし、新しい各計測は通常、グラフのごく小さな最近傍の部分にしか影響しない。増分平滑化はこれを利用する。すべてを再計算する代わりに、新しい情報だけを使って以前の解を更新する。

線形代数の視点

線形化された最小二乗系を解くとは、計測ヤコビアン（ $J = QR$ 、続いて $R\,\Delta\mathbf{x} = \mathbf{d}$ を後退代入で解く）または情報行列（コレスキー分解による $H = J^TJ$ ）を分解することを意味する。新しい計測は $J$ に新しい行を追加する。iSAM（incremental Smoothing and Mapping、Kaess et al.）の重要な観察はこうである。既存の三角因子 $R$ は、それらの新しい行に対して、ゼロから再構築する代わりに、一連のギブンス回転——低階の変更——によって更新できる。この更新は、新しい計測の変数に接続されているエントリのみを埋めるため、 $R$ のほとんどは変更されない。しかしiSAMはそれでも定期的なバッチステップを必要とした。線形化の基準点と変数の順序（フィルインを制御する）の両方が、推定値が進化するにつれて古くなっていくためである。

iSAM2とベイズ木

iSAM2は、ベイズ木を導入することで、この定期バッチという松葉杖を取り除いた。ベイズ木は、ファクターグラフに対する変数消去から導出される木構造のデータ構造である。消去を完了まで実行すると、そのクリークが木構造（ツリー）に組織化されたベイズネットが得られる。ベイズ木は、どの変数の解がどの他の変数に依存しているかを正確に符号化する——各クリークの解は、ルートに向かう祖先にのみ依存する。

新しいファクターが到着すると、次のことが起こる。

影響を受ける変数からルートへのパス上のクリークだけが再消去される——木の残りの部分、通常はその大部分は変更されない。
流動的な再線形化（fluid relinearization）：ある変数は、前回の線形化点から推定値がある閾値を超えて移動した場合にのみ再線形化される。これにより非線形性は、それが問題になる場所（そしてその場所だけ）で扱われる。
部分的な状態更新：解の更新は変更されたクリークから下方へ伝播し、変化が閾値未満になったところで止まる。そのため、実際に動いた推定値だけが再計算される。

その結果、探索型の軌跡に対してはほぼ一定時間の更新が得られ、かつ線形化の閾値までは厳密である——これは情報を捨てる固定ラグ近似ではなく、すべての変数にわたる完全な平滑化問題が維持される。大きなループクロージングは依然として木の大部分に触れる（それは正しい振る舞いである。実際にすべてに影響を与えるからだ）。これはプロファイリング上、時折のレイテンシスパイクとして現れる。

バックエンドの方式を選ぶ

	バッチ平滑化	増分（iSAM2）	固定ラグ＋周辺化
保持する変数	すべて	すべて	直近のウィンドウのみ
更新1回あたりのコスト	軌跡とともに増大	探索中はほぼ一定、ループクロージング時にスパイク	厳密に有界
情報	厳密	全履歴、再線形化の閾値まで厳密	古い状態は凍結された線形化事前分布に要約される
典型的な用途	オフラインマッピング、SfM	ループクロージングを伴うオンラインSLAM	組み込みVIO

iSAM2はGTSAMに実装されており、リアルタイムのレートで全履歴の平滑化を望む場合の第一選択のバックエンドである。例えばKimera-VIOは、構造を持たない「smart」投影ファクターを使ったGTSAMのiSAM2を、増分的な視覚慣性推定のために実行し、LIO-SAMはそのLiDAR慣性ファクターグラフを同じソルバに渡す。主な代替のバックエンド方式——マージナライゼーションを伴う固定ラグのスライディングウィンドウ——は、古い変数を破棄することでコストを抑える。増分平滑化は、それらを保持しながら賢く更新することでコストを抑える。

実務上の注意点

再線形化の閾値は速度と厳密さのトレードオフである。緩すぎる閾値は、高曲率（速い回転）の区間で精度をこっそり悪化させる。
構造を持たない「smart」ファクター（ファクター内部でランドマークを解析的に消去する）は、多数の短命なランドマークを持つ視覚問題に対してベイズ木を小さく保つ。
プラットフォームがループクロージングのレイテンシスパイクに耐えられない場合、一般的なアーキテクチャは、高速な固定ラグのオドメトリフロントエンドと、その背後で動くiSAM2（またはポーズグラフ）平滑化器の組み合わせである。

SLAMにおける意義

増分平滑化は、「平滑化はフィルタリングに勝る」という洞察をリアルタイムのレートで実用的にするものである。軌跡全体にわたる完全な非線形最適化の精度を、フレームごとにバッチコストを払うことなく得られる。あるシステムが「GTSAM/iSAM2ベース」と説明されているのを見たとき、その計算上のストーリーがわかるはずだ。ベイズ木の更新、流動的な再線形化、そして厳密な全履歴MAP推定である。これは、ループクロージングをシームレスに組み込む必要があるVIOおよびLiDAR慣性システム（例えばLIO-SAM、Kimera）における標準的なバックエンドである。