技術的概要：エッジの平滑化

問題定義

本論文は、離散的かつ非同期の可能性がある測定値 $y_k$（時刻 $t_k$ において）に基づいて、時間 $t$ の連続関数としての軌道 $x(t)$ を推定することを目的とする「連続時間状態推定」の課題に取り組んでいる。離散時間推定では、すべての測定値または時間ステップに対して明示的に状態変数を定義する必要があるが、このアプローチは高レートセンサー（例：LiDAR、IMU）や非同期データ、および制御やマッピングなどの下流タスクのために任意の時刻でシステム状態を照会する必要性に対して困難をきたす。

既存の連続時間アプローチは一般的に 2 つのパラダイムに分類される：

1. パラメトリック法： 基底関数（例：スプライン）を用いて軌道を表現する。これらは過小適合または過大適合を避けるために、基底関数の慎重な選択を必要とする。
2. ノンパラメトリック法： ガウス過程（GP） 回帰を使用する。本質的な正則化と柔軟性という技術的な利点を持つが、GP ベースの連続時間推定は、支配的なファクターグラフ推定パラダイムとの直感的な統合の欠如もあって、ロボティクス分野での採用は限定的である。

手法

著者らは、特に SE(3) などのリー群を扱うロボティクス応用に特化し、GP 回帰とファクターグラフを統合するフレームワークを提案する。

1. GP 回帰に対するファクターグラフの視点

核心的な理論的貢献は、ファクターグラフの観点から GP 連続時間推定を再構成することである。

• 分解： 推定問題は 2 つの明確なステップに分解される：
  • 主要な求解： 標準的な離散時間ファクターグラフツールを用いて、離散的な測定時刻 ($t_k$) における状態を求解する。
  • 補間（照会）： 主要な求解完了後に、任意の照会時刻 ($\tau$) における状態を計算する。
• 消去アルゴリズム： 著者らは、任意の時刻 $\tau$ における軌道の照会が、照会状態 $x_\tau$ を除去するためにファクターグラフ消去アルゴリズムを適用することと同等であることを示す。これにより、照会は「セパレーター」状態（$\tau$ の直前および直後の測定状態）の周辺分布のみに依存することが明らかになる。
• 計算効率： 線形時不変（LTI）確率微分方程式（SDE）から導出されたカーネル関数（例：加速度白色雑音（WNOA） モデル）を選択することで、逆カーネル行列が正確にブロック三重対角となる。これにより、主要な求解は測定数 $K$ の $O(K^3)$ ではなく $O(K)$（線形）時間で実行可能となる。さらに、照会ステップは$O(1)$ 操作となり、隣接状態の周辺統計情報のみが必要となる。

2. リー群への拡張

回転と並進からなるロボットのポーズ（リー群 SE(3) などの多様体上に存在）を処理するために、著者らは線形 GP アプローチを適応させる：

• 局所 GP 変数： 単一のグローバル GP の代わりに、各測定時刻の周囲のリー代数における局所変数を定義する。具体的には、$\xi_k(t) = \text{Log}(T_k^{-1}T(t))$ であり、ここで $T_k$ は測定 $k$ におけるポーズである。
• 線形化： 線形運動モデル（例：WNOA）をこれらの局所変数に適用する。リー群要素と局所変数間の非線形関係は、反復ソルバー（例：ガウス・ニュートン法またはレバーバーグ・マルカート法）内の標準的な線形化技術（摂動モデル）によって処理される。
• 共分散補間： 以前の方法（例：Anderson, 2016）よりも単純で、他の方法（例：Barfoot, 2024）よりも効率的な、リー群上の共分散を補間する新規手法が導入される。

3. 補間状態における測定値の処理

明示的に最適化されていない時刻に発生する高レート測定値を処理することは、実用的な重大な課題である。

• 状態消去： 著者らは、最適化グラフから測定に関連する状態を完全に消去する方法を示す。
• 残差ファクター： 測定を伴う補間状態を消去すると、境界を挟む状態間に 2 つのファクターが生じる：運動事前ファクターと新しい測定ファクターである。
• 独立性近似： 疎なグラフ構造を維持するために、著者らは複数の補間状態間の相互共分散を破棄する近似を導入する。これにより、複数の測定値を境界状態に対する独立したファクターとして扱うことができ、最適化問題のサイズを大幅に削減する。また、さらなる高速化のために特定の共分散項を省略する「簡略化ノイズモデル」についても議論されており、精度と信頼性のトレードオフに言及している。

実装

本手法は、ファクターグラフ最適化のための人気のある C++ ライブラリであるGTSAM（Georgia Tech Smoothing and Mapping）に実装されている。

• 新しいファクター： 著者らは、運動事前分布用の WnoaMotionFactor と、補間状態上で定義されたファクターを境界状態上のファクターに変換するラッパーである WnoaInterpFactor を導入する。
• ワークフロー： ユーザーは「境界状態」（最適化対象）と「補間状態」（消去対象）を含むグラフを定義できる。ライブラリは自動的に等価なグラフを構築し、求解し、完全な連続軌道と共分散を回復するための後処理ステップ（updateInterpValues）を提供する。
• 最適化： 反復中の冗長な計算を防ぐため、補間状態の平均とヤコビアンをキャッシュする専用の WnoaFactorGraph クラスが実装されている。

結果と評価

本フレームワークは、1 次元から 3 次元にわたる 3 つのデータセットで評価された：

1. 1 次元（巨大な牛乳瓶）： レーザー測距とオドメトリを用いて直線的に移動するロボット。結果は、連続時間アプローチが疎な測定値（最大 7.5 秒間隔）であっても滑らかで正確な軌道を生成することを示し、線形ケースにおいて「縮小」求解（補間付き）は「完全」求解と同一の結果をもたらすことを示した。
2. 2 次元（森で迷う）： 車輪付きロボットとランドマークを用いた SLAM 問題。この研究は、状態の 30 分の 1 だけを最適化し（残りを補間）することで、完全な求解とほぼ同一の軌道を回復できることを実証した。定量的分析はトレードオフを示す：「簡略化ノイズモデル」は大幅な高速化をもたらすが、「完全ノイズモデル」に比べて過信した推定をもたらし、平均精度がわずかに低下する可能性がある。
3. 3 次元（星空）： ステレオカメラと IMU データを用いた SE(3) における局所化問題。非線形性が関与しているにもかかわらず、線形化ポイントを 80% 削減（5 番目の状態のみを最適化）しても、補間された解は完全な求解と同等の精度と一貫性を維持する。

意義と主張

本論文は、GP ベースの連続時間推定がロボティクスにおいて未活用である状況を是正するものであり、以下の点を提供すると主張している：

• 新たな洞察： GP 回帰とファクターグラフの明確な関連付け。軌道照会が単に消去アルゴリズムの適用であることを示す。これは共分散補間を行うための新規かつ直感的な方法を提供する。
• 実用的なアクセシビリティ： 複雑なカーネル行列や補間方程式をゼロから導出する必要なく、連続時間推定を容易に採用できる GTSAM における実装。
• 効率性： 軌道の滑らかさや精度を犠牲にすることなく最適化変数の数を削減することで、連続時間推定が高レート・非同期センサーを効率的に処理できることの証明。

著者らは、この作業をチュートリアルおよび実用的なガイドとして位置づけ、ロボティクスにおける連続時間推定への参入障壁を下げようとしている。推定時刻のヒューリスティックな選択や、リー群上の局所 GP の使用（アーティファクトを導入する可能性がある）といった限界を認めているが、このアプローチはパラメトリック法に対する堅牢で柔軟な代替手段を提供すると論じている。