正規分布と最適化について

内部学生向けの勉強資料
確率ロボティクスの前提となる正規分布の表現方法・利用方法と最適化とのつながりについての話です。

正規分布について
・共分散行列表現
・情報行列表現
最小二乗法と最尤推定について
・最小二乗法（ガウスニュートン法）
・最尤推定法と最小二乗法の関わり
グラフ最適化について
・ファクタグラフ最適化

Transcript

1. 正規分布と最適化について 産業技術総合研究所 小出健司 2024/10/04 ※内部学生向け（一部カット版）
2. 本日のお話 正規分布について • 共分散行列表現 • 情報行列表現 最小二乗法と最尤推定について • 最小二乗法（ガウスニュートン法） • 最尤推定法と最小二乗法の関わり グラフ最適化について • ファクタグラフ最適化 • Gaussian Belief Propagation
3. 本日のお話 正規分布について • 共分散行列表現 • 情報行列表現 最小二乗法と最尤推定について • 最小二乗法（ガウスニュートン法） • 最尤推定法と最小二乗法の関わり グラフ最適化について • ファクタグラフ最適化 • Gaussian Belief Propagation
4. SfM Visual SLAM LiDAR SLAM Gaussian Splatting Scene Graph Estimation State Estimation & Control 世の中のクールなものは全て、多かれ少なかれ正規分布に依って作られている 正規分布は大事
5. 正規分布（確率密度関数） 𝒙 ∼ 𝒩 𝝁, 𝚺 = 1 2𝜋 𝐷|Σ| exp − 1 2 𝒙 − 𝝁 𝑇𝚺−𝟏 𝒙 − 𝝁 変数 平均 : ℝ6 共分散行列 : ℝ6×6
6. 正規分布（確率密度関数） 𝒙 ∼ 𝒩 𝝁, 𝚺 = 1 2𝜋 𝐷|Σ| exp − 1 2 𝒙 − 𝝁 𝑇𝚺−𝟏 𝒙 − 𝝁 変数 平均 : ℝ6 共分散行列 : ℝ6×6 平均：山のピークがどこにあるか 共分散行列：山がどのように広がっているか
7. 共分散行列 𝚺 = 𝝈𝒙 𝟐 𝝈𝒙𝒚 𝝈𝒙𝒚 𝝈𝒚 𝟐 ：X軸方向の分散（広がり） ：X軸方向の分散（広がり） ：XYの共分散（相関 or 傾き） 𝜎𝑥 2 𝜎𝑦 2 𝜎𝑥𝑦 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟏 𝟎 𝟎 𝟏
8. 共分散行列 𝚺 = 𝝈𝒙 𝟐 𝝈𝒙𝒚 𝝈𝒙𝒚 𝝈𝒚 𝟐 ：X軸方向の分散（広がり） ：X軸方向の分散（広がり） ：XYの共分散（相関 or 傾き） 𝜎𝑥 2 𝜎𝑦 2 𝜎𝑥𝑦 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟐 𝟎 𝟎 𝟏
9. 共分散行列 𝚺 = 𝝈𝒙 𝟐 𝝈𝒙𝒚 𝝈𝒙𝒚 𝝈𝒚 𝟐 ：X軸方向の分散（広がり） ：X軸方向の分散（広がり） ：XYの共分散（相関 or 傾き） 𝜎𝑥 2 𝜎𝑦 2 𝜎𝑥𝑦 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟏 𝟎 𝟎 𝟐
10. 共分散行列 𝚺 = 𝝈𝒙 𝟐 𝝈𝒙𝒚 𝝈𝒙𝒚 𝝈𝒚 𝟐 ：X軸方向の分散（広がり） ：X軸方向の分散（広がり） ：XYの共分散（相関 or 傾き） 𝜎𝑥 2 𝜎𝑦 2 𝜎𝑥𝑦 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟏 𝟎. 𝟓 𝟎. 𝟓 𝟏
11. 共分散行列 𝚺 = 𝝈𝒙 𝟐 𝝈𝒙𝒚 𝝈𝒙𝒚 𝝈𝒚 𝟐 ：X軸方向の分散（広がり） ：X軸方向の分散（広がり） ：XYの共分散（相関 or 傾き） 𝜎𝑥 2 𝜎𝑦 2 𝜎𝑥𝑦 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟏 𝟎. 𝟗 𝟎. 𝟗 𝟏
12. 共分散行列 𝚺 = 𝝈𝒙 𝟐 𝝈𝒙𝒚 𝝈𝒙𝒚 𝝈𝒚 𝟐 ：X軸方向の分散（広がり） ：X軸方向の分散（広がり） ：XYの共分散（相関 or 傾き） 𝜎𝑥 2 𝜎𝑦 2 𝜎𝑥𝑦 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟏 −𝟎. 𝟗 −𝟎. 𝟗 𝟏
13. 共分散行列の固有値分解 𝚺 = 𝑸𝚲𝐐−𝟏 固有値 固有ベクトル 𝑸 = 𝒒𝟏𝟏 𝒒𝟏𝟐 𝒒𝟐𝟏 𝒒𝟐𝟐 第一軸 第二軸 共分散行列の固有ベクトル・固有値は主成分軸の方向とその軸方向の分散を表す 𝚲 = 𝝀𝟏 𝟎 𝟎 𝝀𝟐 第一軸・第二軸方向の分散 ※通常、ノルム=1に正規化
14. 共分散行列の固有値分解 𝚺 = 𝑸𝚲𝐐−𝟏 固有値 固有ベクトル 𝑸 = 𝒒𝟏𝟏 𝒒𝟏𝟐 𝒒𝟐𝟏 𝒒𝟐𝟐 第一軸 第二軸 共分散行列の固有ベクトル・固有値は主成分軸の方向とその軸方向の分散を表す 𝚲 = 𝝀𝟏 𝟎 𝟎 𝝀𝟐 第一軸・第二軸方向の分散 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟐 𝟎. 𝟓 𝟎. 𝟓 𝟏 𝐐 = 𝟎. 𝟗𝟐 −𝟎. 𝟑𝟖 𝟎. 𝟑𝟖 𝟎. 𝟗𝟐 𝚲 = 𝟐. 𝟐𝟏 𝟎 𝟎 𝟎. 𝟕𝟗 方向：[0.92, 0.38] 長さ： 2.21 方向：[-0.38, 0.92] 長さ： 0.79 ※通常、ノルム=1に正規化
15. 共分散行列の固有値分解 𝚺 = 𝑸𝚲𝐐−𝟏 固有値 固有ベクトル 𝑸 = 𝒒𝟏𝟏 𝒒𝟏𝟐 𝒒𝟐𝟏 𝒒𝟐𝟐 𝚲 = 𝝀𝟏 𝟎 𝟎 𝝀𝟐 第一軸 第二軸 共分散行列の固有ベクトル・固有値は主成分軸の方向とその軸方向の分散を表す 第一軸・第二軸方向の分散 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟐 𝟎. 𝟓 𝟎. 𝟓 𝟏 𝐐 = 𝟎. 𝟗𝟐 −𝟎. 𝟑𝟖 𝟎. 𝟑𝟖 𝟎. 𝟗𝟐 𝚲 = 𝟐. 𝟐𝟏 𝟎 𝟎 𝟎. 𝟕𝟗 方向：[0.92, 0.38] 長さ： 2.21 方向：[-0.38, 0.92] 長さ： 0.79 利用例：カルマンフィルタの誤差楕円の表示
16. 共分散行列の固有値分解 共分散行列を固有値分解することで縮退判定や正則化を行うことができる 𝚺 = 𝑸𝚲𝐐−𝟏 固有値 固有ベクトル 共分散行列の固有値は ある軸方向の分散を表す 固有値が負の場合、 その軸方向の分散が定義できない！
17. 共分散行列の固有値分解 共分散行列を固有値分解することで縮退判定や正則化を行うことができる 𝚺 = 𝑸𝚲𝐐−𝟏 固有値 固有ベクトル 共分散行列の固有値は ある軸方向の分散を表す 固有値が負の場合、 その軸方向の分散が定義できない！ 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟐 𝟎. 𝟓 𝟎. 𝟓 𝟏 𝐐 = 𝟎. 𝟗𝟐 −𝟎. 𝟑𝟖 𝟎. 𝟑𝟖 𝟎. 𝟗𝟐 𝚲 = 𝟐. 𝟐𝟏 𝟎 𝟎 𝟎. 𝟕𝟗 程よい広がり
18. 共分散行列の固有値分解 𝚺 = 𝑸𝚲𝐐−𝟏 固有値 固有ベクトル 共分散行列を固有値分解することで縮退判定や正則化を行うことができる 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟐 𝟏. 𝟒 𝟏. 𝟒 𝟏 𝐐 = 𝟎. 𝟖𝟏 −𝟎. 𝟓𝟖 𝟎. 𝟓𝟕 𝟎. 𝟖𝟐 𝚲 = 𝟐. 𝟗𝟗 𝟎 𝟎 𝟎. 𝟎𝟏𝟑 縮退寸前 共分散行列の固有値は ある軸方向の分散を表す 固有値が負の場合、 その軸方向の分散が定義できない！
19. 共分散行列の固有値分解 𝚺 = 𝑸𝚲𝐐−𝟏 固有値 固有ベクトル 共分散行列を固有値分解することで縮退判定や正則化を行うことができる 最小固有値が小さいとき（正定値行列でないとき） その共分散行列は縮退していると判定できる 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟐 𝟏. 𝟓 𝟏. 𝟓 𝟏 𝐐 = 𝟎. 𝟖𝟏 −𝟎. 𝟓𝟖 𝟎. 𝟓𝟖 𝟎. 𝟖𝟏 𝚲 = 𝟑. 𝟎𝟏 𝟎 𝟎 −𝟎. 𝟎𝟖 縮退して計算不可 共分散行列の固有値は ある軸方向の分散を表す 固有値が負の場合、 その軸方向の分散が定義できない！
20. 共分散行列の固有値分解 𝚺 = 𝑸𝚲𝐐−𝟏 固有値 固有ベクトル 共分散行列を固有値分解することで縮退判定や正則化を行うことができる 共分散行列の固有値は ある軸方向の分散を表す 逆説的に固有値の値を操作して 縮退を防ぐこと（正則化）ができる！
21. 共分散行列の固有値分解 𝚺 = 𝑸𝚲𝐐−𝟏 固有値 固有ベクトル 共分散行列を固有値分解することで縮退判定や正則化を行うことができる 共分散行列の固有値は ある軸方向の分散を表す 逆説的に固有値の値を操作して 縮退を防ぐこと（正則化）ができる！ 例えば： -5 5 5 -5 0 0 𝑦 𝑥 𝚺 = 𝟐 𝟏. 𝟓 𝟏. 𝟓 𝟏 𝐐 = 𝟎. 𝟖𝟏 −𝟎. 𝟓𝟖 𝟎. 𝟓𝟖 𝟎. 𝟖𝟏 𝚲 = 𝟑. 𝟎𝟏 𝟎 𝟎 −𝟎. 𝟎𝟖 軸方向と第一軸分散を維持して まともな分布に変換できた！ 0.1で置き換える！ 𝚺′ = 𝑸𝚲′𝐐−𝟏 = 𝟐. 𝟎𝟔 𝟏. 𝟒𝟏 𝟏. 𝟒𝟏 𝟏. 𝟏𝟐
22. 本日のお話 正規分布について • 共分散行列表現 • 情報行列表現 最小二乗法と最尤推定について • 最小二乗法（ガウスニュートン法） • 最尤推定法と最小二乗法の関わり グラフ最適化について • ファクタグラフ最適化 • Gaussian Belief Propagation
23. 正規分布の積 確率ロボティクスの根幹は分布を掛け合わせること カルマンフィルタでもファクタグラフでも形は違えど 分布を掛け合わせて尤もらしい状態を推定する 正規分布の再生性： 正規分布を掛け合わせた結果は正規分布 𝝁 = 𝚺2 𝚺1 + 𝚺2 −1𝝁1 + 𝚺1 𝚺1 + 𝚺2 −1𝝁2 𝚺 = 𝚺1 𝚺1 + 𝚺2 −1𝚺2 𝒩 𝝁, 𝚺 = 𝒩 𝝁1 , 𝚺1 × 𝒩(𝝁2 , 𝚺2 ) 複雑でややこしい… 計算量も多い… 計算安定性も悪い… もっと簡単に正規分布を混ぜ合わせたい！！ → 情報行列形式 (Information form) ※定数倍は無視している
24. 情報行列表現 共分散行列形式 (Covariance form) 情報行列形式(Information form) 𝒙 ∼ 𝒩(𝚺, 𝝁) 𝝁 𝚺 ：共分散行列 ℝ6×6 ：平均ベクトル ℝ6 𝚲 = 𝚺−𝟏 𝜼 = 𝚺−𝟏𝝁 情報行列 ℝ𝟔×𝟔 情報ベクトル ℝ𝟔 ※Λと𝜂は状況や解釈によって様々な呼び方がある。 それぞれ違う意味を持つがいったん全て同じと考えて良い。 精度行列、ヘッセ行列、誤差ベクトル、ポテンシャルベクトル 表現が変わっただけでどちらも同じ正規分布を表している 情報を失うことなく相互変換可能
25. 情報行列表現の利点１：分布積がとっても簡単 共分散行列表現 (Covariance form) 情報行列表現 (Information form) 足すだけでいい！！（数式的には共分散形式と等価） 𝝁 = 𝚺2 𝚺1 + 𝚺2 −1𝝁1 + 𝚺1 𝚺1 + 𝚺2 −1𝝁2 𝚺 = 𝚺1 𝚺1 + 𝚺2 −1𝚺2 𝒩 𝚺, 𝝁 = 𝒩 𝚺1 , 𝝁1 × 𝒩(𝚺2 , 𝝁2 ) 𝒩 𝚲, 𝜼 = 𝒩 𝚲1 , 𝜼1 × 𝒩(𝚲2 , 𝜼2 ) 𝜼 = 𝜼𝟏 + 𝜼𝟐 𝚲 = 𝚲𝟏 + 𝚲𝟐
26. 情報行列表現の利点１：分布積がとっても簡単 共分散行列表現 (Covariance form) 情報行列表現 (Information form) 足すだけでいい！！（数式的には共分散形式と等価） 𝝁 = 𝚺2 𝚺1 + 𝚺2 −1𝝁1 + 𝚺1 𝚺1 + 𝚺2 −1𝝁2 𝚺 = 𝚺1 𝚺1 + 𝚺2 −1𝚺2 𝒩 𝚺, 𝝁 = 𝒩 𝚺1 , 𝝁1 × 𝒩(𝚺2 , 𝝁2 ) 𝒩 𝚲, 𝜼 = 𝒩 𝚲1 , 𝜼1 × 𝒩(𝚲2 , 𝜼2 ) 𝜼 = 𝜼𝟏 + 𝜼𝟐 𝚲 = 𝚲𝟏 + 𝚲𝟐 𝒩 𝚲, 𝜼 = ෑ 𝑖 𝑁 𝒩 𝚲𝑖 , 𝜼𝑖 𝜼 = ෍ 𝒊 𝑵 𝜼𝒊 𝚲 = ෍ 𝒊 𝐍 𝚲𝐢 何個掛け合わせても足すだけ！！ ※この特性は後で出てくるガウスニュートン法と密接に関わってくる
27. 情報行列表現の利点２：情報が存在しないこと(縮退)を表現可能 共分散行列 𝚺 情報行列は共分散行列の逆行列なので、特性が逆になる 情報行列 𝚲 = 𝚺−𝟏 要素 分散 (大きいほど不確か) 精度 (大きいほど正確) 固有値 大きいほど不確か 大きいほど正確 例えば、物体の二次元位置(XY)のXだけ観測できたとき（Xの分散=10、Yは情報がない） 𝚺 = 10 0 0 ∞ 𝚲 = 0.1 0 0 0 共分散行列： 無限大の不確かさ→計算が破綻 情報行列： 情報がゼロ→そのまま表現・計算可能 一部の自由度に情報がない制約（e.g., 点群縮退）も利用してそのまま最適化ができる ※逆に、誤差がゼロ（精度が無限大）の観測は表現できないがこれはあまり問題にならない。何故か答えよ。
28. 情報行列表現の利点２：情報が存在しないこと(縮退)を表現可能 共分散行列 𝚺 情報行列は共分散行列の逆行列なので、特性が逆になる 情報行列 𝚲 = 𝚺−𝟏 要素 分散 (大きいほど不確か) 精度 (大きいほど正確) 固有値 大きいほど不確か 大きいほど正確 例えば、二次元位置(XY)のXだけ観測できたとき（Xの分散=10、Yは情報がない） 𝚺 = 10 0 0 ∞ 𝚲 = 0.1 0 0 0 共分散行列： 無限大の不確かさ→計算が破綻 情報行列： 情報がゼロ→そのまま表現・計算可能 全ての自由度に情報がない制約（e.g., 点群縮退）も利用してそのまま最適化ができる ※逆に、誤差がゼロ（精度が無限大）の観測は表現できないがこれはあまり問題にならない。何故か答えよ。 タイトカップリングで縮退した点群をそのまま5自由度制約として用いる https://youtu.be/uEy7GI7mhlw?feature=shared
29. 情報行列表現の弱点：周辺分布を取り出すのが面倒 共分散行列表現 (Covariance form) 情報行列表現 (Information form) 良くある状況：カルマンフィルタの位置・速度状態分布から、位置のみの周辺分布を取り出したい 𝚺 = 𝚺𝑝𝑝 𝚺𝑝𝑣 𝚺𝑣𝑝 𝚺𝑣𝑣 位置に対応するブロックをそのまま抜き出すだけ 𝚲 = 𝚲𝛼𝛼 𝚲𝛼𝛽 𝚲𝛽𝛼 𝚲𝛽𝛽 𝝁 = 𝝁𝑝 𝝁𝑣 𝜼 = 𝜼𝛼 𝜼𝛽 𝜼𝑴𝜶 = 𝜼𝛼 − 𝚲𝛼𝛽 𝚲𝛽𝛽 −1 𝜼𝛽 𝚲𝑴𝜶 = 𝚲𝛼𝛼 − 𝚲𝛼𝛽 𝚲𝛽𝛽 −1𝚲𝛽𝛼 𝛼に対応する周辺分布： (Schur の補行列) 情報行列表現において、特定の変数の情報のみを取り出す周辺化は結構面倒な処理 特に変数が多くなると計算量・計算精度が悪化するので、バンドル調整などでは 特殊な周辺化手法 (Left null-space marginalization) が提案されている
30. 正規分布表現のおさらい 𝒙 ∼ 𝒩(𝚺, 𝝁) 変数 平均ベクトル 共分散行列 𝚲 = 𝚺−𝟏 𝜼 = 𝚺−𝟏𝝁 情報行列 情報ベクトル 共分散行列形式 情報行列形式 分布の掛け算が簡単 情報ゼロを表現可能 周辺化が面倒 最後に欲しいのは結局平均ベクトル
31. 本日のお話 正規分布について • 共分散行列表現 • 情報行列表現 最小二乗法と最尤推定について • 最小二乗法（ガウスニュートン法） • 最尤推定法と最小二乗法の関わり グラフ最適化について • ファクタグラフ最適化 • Gaussian Belief Propagation
32. 最適化 https://www.slideshare.net/slideshow/ss-42149384/42149384 低次元＆非リアルタイムのプロトタイピング リアルタイム処理全般 我々が一番よく使うクラス 経路計画や行動制御のような 厳密制約のある問題 非リアルタイムの全体最適化（ ORなど） よくある用途 最適化とは：関数 𝑓 𝑥 を最小化する変数 𝑥 を求めること
33. （非線形）最小二乗法 ෥ 𝒙 = argmin 𝒙 ෍ 𝒆𝑖𝑗 𝑇 𝒆𝑖𝑗 目的関数が誤差の二乗和で表される最適化問題 観測 変数 𝒙𝑖 と𝒙𝑗 による観測予測関数 𝒆𝒊𝒋 = 𝒛𝒊𝒋 − 𝒇𝒊𝒋 𝒙𝒊𝒋 誤差＝残差の二乗： 𝒆𝒊𝒋 𝑻 𝒆𝒊𝒋 = 𝒆𝒊𝒋 𝟐 残差 (符号付) 任意関数最小化に比べると要件が厳しい 関数が必ず二乗誤差でなくてはならない 変数の数より観測のほうが多くなくてはならない 関数の非線形性が強すぎてはならない ෥ 𝒙 = argmin 𝒙 𝑓 𝒙 任意関数最小化： しかし、 最適化が爆速 → 大規模リアルタイム処理ならほぼ最小二乗法の一択 重み付きの場合： 𝒆𝒊𝒋 𝑻 𝛀𝐢𝐣 𝒆𝒊𝒋 便宜上、𝒙𝑖𝑗 = 𝒙𝑖 𝑇, 𝒙𝑗 𝑇 𝑇と連結して表す ※ 例えば、三次元相対位置なら：位置推定二つ (𝒙𝑖 , 𝒙𝑗 )を受け取り、 その相対位置と観測の差 𝒛𝑖𝑗 − 𝒙𝑗 − 𝒙𝑖 を残差として計算する
34. ガウスニュートン法 非線形最小二乗法はガウスニュートン法が基本 • 最適化の安定性を高めた → Levenberg Marquardt (LM) 法、Dogleg 法 • インクリメンタル最適化を取り入れた → incremental smoothing and mapping (iSAM) 最小二乗法≒ガウスニュートンは広範な問題に適用できる ガウスニュートンを習得して君も状態推定マスター！！ 点群マッチング バンドル調整 (SfM) GNSS測位 行動制御・計画
35. ガウスニュートン法の流れ 1. Linearize (線形化) ガウスニュートン法は逐次最適化：適当な初期値から始めて変数を繰り返し更新する 2. Solve (求解) 現在の推定値で非線形目的関数を二次関数に変換する 線形化された二次関数を線形代数で解いて推定値を更新する 収束するまで線形化と求解を繰り返す
36. ガウスニュートン法（線形化） 𝐹𝑖𝑗 𝒙𝑖𝑗 = 𝒆𝑖𝑗 𝑇 𝒆𝑖𝑗 𝒆𝒊𝒋 = 𝑓𝑖𝑗 𝒙𝒊𝒋 ※簡単のため観測 𝒚𝑖𝑗 は関数に含める 目的関数： 現在の推定値 ෭ 𝒙𝑖𝑗 で非線形誤差関数 𝑓𝑖𝑗 を線形近似する (テイラー展開)： 𝐹𝑖𝑗 ෭ 𝒙𝑖𝑗 + 𝚫𝒙𝒊𝒋 = 𝑓𝑖𝑗 ෭ 𝒙𝑖𝑗 + 𝚫𝒙𝒊𝒋 𝑇 𝑓𝑖𝑗 ෭ 𝒙𝑖𝑗 + 𝚫𝒙𝒊𝒋 ෥ 𝒙 = argmin 𝒙 ෍ 𝐹𝑖𝑗 𝒙𝑖𝑗 ≈ 𝒆𝒊𝒋 + 𝑱𝒊𝒋 𝚫𝒙𝒊𝒋 𝑇 𝒆𝒊𝒋 + 𝑱𝒊𝒋 𝚫𝒙𝒊𝒋 = 𝚫𝒙𝒊𝒋 𝑻 𝑱𝒊𝒋 𝑻 𝑱𝒊𝒋 𝚫𝒙𝒊𝒋 + 𝟐𝒆𝒊𝒋 𝑻 𝑱𝒊𝒋 𝚫𝒙𝒊𝒋 + 𝒆𝒊𝒋 𝑻 𝒆𝒊𝒋 𝑯𝒊𝒋 𝒃𝒊𝒋 𝑐𝑖𝑗 = 𝚫𝒙𝒊𝒋 𝑻 𝑯𝒊𝒋 𝚫𝒙𝒊𝒋 + 2𝒃𝒊𝒋 𝚫𝒙𝑖𝑗 + 𝑐𝑖𝑗 全ての観測誤差の総和 ある観測の二乗誤差 (スカラ) 観測誤差関数 (残差ベクトル) 線形化とは：非線形誤差関数の現在の残差とヤコビ行列から二次関数パラメータ𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を求めること 𝑓𝑖𝑗 ෭ 𝒙𝑖𝑗 + 𝚫𝒙𝒊𝒋 ≈ 𝒆𝒊𝒋 + 𝑱𝒊𝒋 𝚫𝒙𝒊𝒋 線形変化 ヤコビ行列 (偏微分) 現在の残差 現在の推定値 さっきの相対位置観測の 例でいうと 𝒙𝑖𝑗 ∈ ℝ6、𝒆𝑖𝑗 ∈ ℝ3
37. ガウスニュートン法（求解） 目的関数： 𝑔 ෭ 𝒙 + 𝚫𝒙 = ෍ 𝐹𝑖𝑗 (෭ 𝒙𝑖𝑗 + 𝚫𝒙𝒊𝒋 ) ≈ ෍ 𝚫𝒙𝒊𝒋 𝑻 𝑯𝒊𝒋 𝚫𝒙𝒊𝒋 + 2𝒃𝒊𝒋 𝚫𝒙𝑖𝑗 + 𝑐𝑖𝑗 各変数の二次関数パラメータ 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝒄𝒊𝒋 から全変数を含んだ二次関数パラメータ 𝑯, 𝒃, 𝒄 を構築する 例えば、観測関数 𝑓 𝒙𝟐 , 𝒙𝟑 なら 𝒙𝒊𝒋 = 𝒙𝟐 𝑻, 𝒙𝟑 𝑻 𝑇 として 𝑯𝒊𝒋 = 𝑯𝟐𝟐 𝑯𝟐𝟑 𝑯𝟑𝟐 𝑯𝟑𝟑 𝒃𝒊𝒋 = 𝒃𝟐 𝒃𝟑 を求め、 全体 𝑯, 𝒃 の対応するブロックに加算する 線形化された各観測関数の総和 全変数に対する誤差 各観測の二乗誤差 𝒃 = 𝒃1 𝒃2 𝒃3 ⋮ 𝒃𝑵 𝑯 = 𝑯𝟏𝟏 𝑯𝟏𝟐 𝑯𝟏𝟑 𝑯𝟐𝟏 𝑯𝟐𝟐 𝑯𝟐𝟑 𝑯𝟑𝟏 𝑯𝟑𝟐 𝑯𝟑𝟑 ⋯ 𝑯𝟏𝑵 𝑯𝟐𝑵 𝑯𝟑𝑵 ⋮ ⋱ ⋮ 𝑯𝑵𝟏 𝑯𝑵𝟐 𝑯𝑵𝟑 ⋯ 𝑯𝑵𝑵
38. ガウスニュートン法（求解） 現在の推定値 ෭ 𝒙 で線形化された目的関数が、局所変化量 𝚫𝒙 でどのように変化するか： 𝑔 ෭ 𝒙 + 𝚫𝒙 ≈ 𝚫𝒙𝑯𝚫𝒙 + 2𝒃𝚫𝒙 + 𝑐 線形方程式を解いて目的関数を最小化する更新量 𝚫𝒙∗ を求める： 𝑯𝚫𝒙∗ = −𝒃 𝒙∗= ෭ 𝒙 + 𝚫𝐱∗ 更新量ベクトル 𝚫𝒙∗ を現在の推定値 ෭ 𝒙 に加算して、更新後の変数 𝒙∗ を得る： ※ 二次関数の極は 微分=0 となる点 𝚫𝒙∗ = −𝑯−𝟏𝒃 ※ 実際は逆行列は直接求めずに、行列分解で効率的に方程式を解く 𝝏 𝝏𝚫𝐱 𝚫𝒙𝑯𝚫𝒙 + 2𝒃𝚫𝒙 + 𝑐 = 0 2𝑯𝚫𝒙 + 2𝒃 = 0 ※ 関数の相対的な増減だけが重要なので定数 𝑐 は消える
39. ガウスニュートン法（まとめ） 1. 各観測誤差関数 𝑓 𝒙𝒊𝒋 から二次関数パラメータ 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を求める 2. 各関数の 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を足し合わせて全変数を含む 𝑯, 𝒃, 𝑐 を求める Linearize (線形化) Solve (求解) 1. 線形方程式 𝑯𝚫𝒙∗ = −𝒃 を解いて最適更新量 𝚫𝒙∗ を求める 2. 現在の推定値 ෭ 𝒙 に更新量を加算する：𝒙∗ = ෭ 𝒙 + 𝚫𝒙∗ 収束するまで線形化と求解を繰り返す
40. 最小二乗法 ＝ 最尤推定 1. 各観測誤差関数 𝑓 𝒙𝒊𝒋 Linearize (線形化) 非線形誤差関数→任意の誤差分布 二次目的関数→局所的な正規分布近似(情報行列形式) 全観測誤差の正規分布(情報行列形式)を掛け合わせる 2. 各関数の 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を足し合わせて全変数を含む 𝑯, 𝒃, 𝑐 を求める から二次関数パラメータ 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を求める
41. 最小二乗法 ＝ 最尤推定 1. 各観測誤差関数 𝑓 𝒙𝒊𝒋 から二次関数パラメータ 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を求める 2. 各関数の 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を足し合わせて全変数を含む 𝑯, 𝒃, 𝑐 を求める Linearize (線形化) 非線形誤差関数→任意の誤差分布 二次目的関数→局所的な正規分布近似(情報行列形式) 全観測誤差の正規分布(情報行列形式)を掛け合わせる 観測分布A 観測分布B 正規分布A 正規分布B 全体正規分布
42. 最小二乗法 ＝ 最尤推定 1. 各観測誤差関数 𝑓 𝒙𝒊𝒋 から二次関数パラメータ 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を求める 2. 各関数の 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を足し合わせて全変数を含む 𝑯, 𝒃, 𝑐 を求める Linearize (線形化) Solve (求解) 1. 線形方程式 𝚫𝒙∗ = −𝑯−𝟏𝒃 を解いて最適更新量 𝚫𝒙∗ を求める 2. 現在の推定値 ෭ 𝒙 に更新量を加算する：𝒙∗ = ෭ 𝒙 + 𝚫𝒙∗ 非線形誤差関数→任意の誤差分布 二次目的関数→局所的な正規分布近似(情報行列形式) 全観測誤差の正規分布(情報行列形式)を掛け合わせる ガウスニュートン法は各観測を局所正規分布として情報行列形式で全て掛け合わせることと等価 最小二乗法を解くこと ＝ 最尤推定を解くこと ※実際は重み付き最小二乗法＝最尤推定 (重み≒分散) 掛け合わせた結果の情報行列・情報ベクトルを、共分散・平均形式に変換する (𝝁 = 𝚲−1𝜼) ※ヘッセ行列の逆行列＝共分散行列とするのはこれによる
43. カルマンフィルタ = 最尤推定 = 最小二乗法 カルマンフィルタは予測分布と観測分布を正規分布として 掛け合わせる最尤推定＝ 実は最小二乗法をオンラインで解く方法 線形カルマンフィルタの基礎 https://www.jstage.jst.go.jp/article/sicejl/56/9/56_632/_pdf “フィルタの状態変数と雑音などの正規性，状態方程式の線形性という仮定のもとでは， 推定誤差の二乗和を最小化するという意味で最適なフィルタはカルマンフィルタである ことが理論的に示されている．” 情報行列表現を使えば、カルマンフィルタは観測の情報行列・ベクトルを足すだけで 簡単に導出できる (Information filter) Covariance form Information form ※ 実際は状態遷移誤差の 加算が必要
44. 最尤推定＝最小二乗法（別解） 𝒙 ∼ 𝒩 𝝁, 𝚺 = 1 2𝜋 𝐷|Σ| exp − 1 2 𝒙 − 𝝁 𝑇𝚺−1 𝒙 − 𝝁 𝒩 𝝁𝟏 , 𝚺𝟏 𝒩 𝝁𝟐 , 𝚺𝟐 = 𝐶𝐴 exp − 1 2 𝒙 − 𝝁𝟏 𝑇𝚺𝟏 −𝟏 𝒙 − 𝝁𝟏 exp − 1 2 𝒙 − 𝝁𝟐 𝑇𝚺𝟐 −𝟏 𝒙 − 𝝁𝟐 −log 𝒩 𝝁𝟏 , 𝚺𝟏 𝒩 𝝁𝟐 , 𝚺𝟐 = 1 2 𝒙 − 𝝁𝟏 𝑇𝚺𝟏 −𝟏 𝒙 − 𝝁𝟏 + 1 2 𝒙 − 𝝁𝟐 𝑇𝚺𝟐 −𝟏 𝒙 − 𝝁𝟐 + CB 正規分布の定義 二つの正規分布の積をとる 負の対数をとる (極は変わらない) 情報行列を重みとした平均からの二乗誤差 あらためて、正規分布の積 = 重み付き最小二乗法 ということが確かめられる
45. Levenberg-Marquardt法（安定化Gauss-Newton） 1. 各観測誤差関数 𝑓 𝒙𝒊𝒋 から二次関数パラメータ 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を求める 2. 各関数の 𝑯𝒊𝒋 , 𝒃𝒊𝒋 , 𝑐𝑖𝑗 を足し合わせて全変数を含む 𝑯, 𝒃, 𝑐 を求める Linearize (線形化) Solve (求解) 1. 線形方程式 (𝑯 + 𝜆𝑰) 𝚫𝒙∗ = −𝒃 を解いて更新量 𝚫𝒙∗ を求める Verify (検証) 1. 現在の推定値 ෭ 𝒙 に更新量を加算した 𝒙∗ = ෭ 𝒙 + 𝚫𝒙∗ が元の非線形目的関数を減少させるか確認 2. 減少していれば 𝒙∗ を採用して 𝜆 を小さくする（ブレーキを緩める） 3. 減少していなければ 𝒙∗ を棄却して 𝜆 を大きくする（ブレーキを強くする） ダンピング係数：大きいと変数の更新量が小さくなる＆再急降下方向に近づく 目的関数の非線形性が強いとGN法は発散することがあるので、検証ステップを入れて発散を防ぐ
46. 本日のお話 正規分布について • 共分散行列表現 • 情報行列表現 最小二乗法と最尤推定について • 最小二乗法（ガウスニュートン法） • 最尤推定法と最小二乗法の関わり グラフ最適化について • ファクタグラフ最適化 • Gaussian Belief Propagation
47. 大規模な問題におけるガウスニュートン法 A B C D E 各時刻におけるセンサ姿勢を変数として軌跡全体を最適化する 姿勢数を𝑁として 6𝑁 × 6𝑁 の情報行列を解く必要がある 𝑯 = 𝑯𝟏𝟏 ⋯ 𝑯𝟏𝑵 ⋮ ⋱ ⋮ 𝑯𝑵𝟏 ⋯ 𝑯𝑵𝑵 6𝑁 6𝑁 𝒃 = 𝒃𝟏 ⋮ 𝒃𝑵 6𝑁 線形方程式を解く処理量 ∝ 情報行列の要素数 姿勢数の二乗で処理が重くなる → すぐに処理が追い付かなるので大規模な最適化ができない どうする？ 最適化問題の局所性・疎性を利用する
48. 大規模な問題におけるガウスニュートン法 A B C D E 隣接変数間にしか直接の関連性はない （A-B間は関連がある A-C間はない） A B C D E A B C D E F F 情報行列を構築するときに観測が存在しない (直接関連しない) 変数間は何も足されない ＝ 値がゼロのまま 値が０の場所が多い 疎行列計算で効率的に解く 変数が増えても近隣分しか要素が増えない 処理量∝地図規模 F
49. ファクタグラフ最適化 局所性・疎性を持つ最適化問題をモデリングする強力な枠組み 変数間の関係性(制約)をグラフとして表現 最適化対象となる変数 変数間の二乗誤差関数 (非線形) → ファクタの存在しない変数間は情報ゼロ 変数 : 𝑥1 , 𝑥2 , 𝑥3 ファクタ : 𝑓0 𝑥1 , 𝑓1 𝑥1 , 𝑥2 ; 𝑜1 , 𝑓2 𝑥2 , 𝑥3 ; 𝑜2 グラフ内の全てのファクタを足し合わせたもの 𝑔 𝑥 = 𝑓0 𝑥1 + 𝑓1 𝑥1 , 𝑥2 ; 𝑜1 + 𝑓2 𝑥2 , 𝑥3 ; 𝑜2 目的関数 : 𝑔 𝑥 𝑥1 𝑥2 𝑓0 (𝑥1 ) 𝑓1 (𝑥1 , 𝑥2 ; 𝑜1 ) 𝑥3 𝑓2 (𝑥2 , 𝑥3 ; 𝑜2 )
50. ファクタグラフの利点 大規模・高速な最適化 モジュラーな設計 広範囲の応用 • 疎なグラフ表現のおかげで大規模問題へのスケール性が良い • 数千～数万の変数・制約を含む問題も十分に解ける • 最小二乗法でほとんどの状態推定問題は記述できる • 加えてハード制約や非最小二乗誤差も導入して行動計画なども • 変数と制約が高度に抽象化・部品化されている • 変数の型が共通であれば、レゴブロックのように繋ぎ合わせて 多様なセンサ観測に対する制約を融合できる GNSS IMU ホイールオドメトリ ループ 𝑙1 ランドマーク カメラ 投影誤差 GLIM Building Rome in a Day A Factor-Graph Approach for Optimization Problems with Dynamics Constraints
51. ファクタグラフの苦手な部分 ファクタグラフで大体のことはできる、が、いつも最適な選択という訳ではない（No Silver Bullet） データ対応付け • 一般に最小二乗法の枠組みではデータ対応付けを解くのは難しい • Visual Odometry などは画像上での特徴点追跡が確定した上で バンドル調整を行うのが一般的 • 点群ICPやVOの直接法（輝度誤差最小化）は最小二乗の枠組みで 自然と対応付けを解けるが初期値依存性が強い 広範囲に及ぶ多変数を同時最適化するとき（e.g., SLAM）がファクタグラフ最適化の真骨頂 密な最適化問題 • グラフ最適化が高速なのは問題の疎性を利用できるとき • 密な最適化問題に対してはグラフ表現の利点が活きないため 単一変数の最適化（1フレームスキャンマッチング）や 密なモデル最適化（ニューラルネットワーク）には向かない
52. ファクタグラフの逐次最適化 (iSAM2) インクリメンタル最適化 𝑥1 𝑥2 𝑥3 𝑥4 𝑥5 𝑥6 𝑥7 𝑥8 オンライン推定問題ではグラフ構造は徐々に変化していく 新しい変数・ファクタが追加されても一部しか影響を受けない 𝑥7 が追加されたとき：𝑥6 以前は影響を受けない 𝑥8 が追加されたとき：ループを閉じても 𝑥3 までしか影響を受けない GN法で毎回全変数を最適化すると無駄が多い ファクタグラフを完全部分グラフ（クリーク）の連なりとして モデル化する（Bayes Tree） 影響を受ける範囲を自動判別して連鎖的にそこだけ最適化する 地図がどれほど大きくなっても一度に影響を受ける変数分の最適化コストしかかからない iSAM2 (incremental Smoothing and Mapping)
53. 今日話せなかったけれど大事なトピック 最適化 • 多様体表現とリー代数 • ロバスト推定（M推定 / Graduated Non-Convexity） • 行列分解（コレスキー・LU・QR分解） • 周辺化（変数消去） • 制約付き最適化 (Quadratic programming / Augmented Lagrangian Factor Graph) 状態推定 • ノンパラメトリック推定（パーティクルフィルタ・ヒストグラムフィルタ） • フィルタリング vs 最適化 • 変数消去を使った Smart Factor