Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit)

https://github.com/opensrcai/bundlefit

Transcript

1. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 開発者：渋谷 樹弥、大川 快 支援者：Open Source AI Association
2. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) BundleFitとは？ Bundle Fitting Library for 3D Reconstruction • 機能 • Ceres Solverを利用したバンドル調整とポーズグラフ最適化 • Ceres Solver: Googleが開発している非線形最適化用OSSのC++ライブラリ • 一般的な画像空間上の再投影誤差の解析微分を提供 • 自動微分よりも高速 • 6, 7自由度のポーズグラフ最適化に対応 • スケール変化がある場合（例：単眼Visual SLAM）は7自由の最適化が必要 • マルチカメラシステムに対応 • リグに複数のカメラが固定されている場合にも対応 • 特徴 • C++17 • Python wrapper • Tutorial and sample codes 2
3. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 目次 • 予備知識 • 座標変換とカメラの投影モデル • 回転行列と回転ベクトル • シューア補行列 • バンドル調整 • ループクロージングとポーズグラフ最適化 • ソースコード解説
4. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 予備知識： 座標変換とカメラの投影モデル 4
5. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 座標変換とカメラの投影モデル 3D点を画像平面へ投影 • ワールド座標系→カメラ座標系→画像座標系 • 座標系 • ワールド：全カメラ共通の座標系 • カメラ ：任意のカメラに固定された座標系 • 画像 ：任意の画像平面 〃 • 内部パラメータ • カメラ座標系→画像座標系の変換 • 外部パラメータ • ワールド座標系→カメラ座標系の変換 , ワールド座標系 カメラ座標系 R , R , , ワールド R , カメラ座標系と画像座標系の関係 ワールド座標系とカメラ座標系の関係
6. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 座標変換とカメラの投影モデル カメラは空間中の物体に反射した光が撮像素子に投影されることでシーン を記録 • この関係を表すのがカメラの内部パラメータK • 最も単純な透視投影モデル 𝑓𝑢 , 𝑓𝑣 ：各軸の焦点距離 𝑐𝑢 , 𝑐𝑣 ：光学中心 𝐗𝑐 = 𝑋𝑐 , 𝑌𝑐 , 𝑍𝑐 T：カメラ座標系の3D点 𝐳𝑐 = 𝑢, 𝑣 T ：画像平面上の2D点 K′ ≔ 𝑓𝑢 0 𝑐𝑢 0 𝑓𝑣 𝑐𝑣 K ≔ 𝑓𝑢 0 𝑐𝑢 0 𝑓𝑣 𝑐𝑣 0 0 1 𝐳 = 1 𝑍𝑐 K′𝐗𝑐 , ワールド座標系 カメラ座標系 R , R , , ワールド R , カメラ座標系と画像座標系の関係 ワールド座標系とカメラ座標系の関係
7. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 座標変換とカメラの投影モデル カメラ座標系がワールド座標系と異なる場合，ワールド座標系からカメラ 座標系へ変換 • 前ページの投影モデルに 𝐗𝑐 = R𝑐𝑤 𝐭𝑐𝑤 ] ഥ 𝐗𝑤 を代入 , ワールド座標系 カメラ座標系 R , R , , ワールド R , カメラ座標系と画像座標系の関係 ワールド座標系とカメラ座標系の関係 𝐳 = 1 𝑍𝑐 K′ R𝑐𝑤 𝐭𝑐𝑤 ] ഥ 𝐗𝑤 𝑢 𝑣 = 1 𝑍𝑐 𝑓𝑢 0 𝑐𝑢 0 𝑓𝑣 𝑐𝑣 𝑟𝑐𝑤 11 𝑟𝑐𝑤 12 𝑟𝑐𝑤 13 𝑡𝑐𝑤 1 𝑟𝑐𝑤 21 𝑟𝑐𝑤 22 𝑟𝑐𝑤 23 𝑡𝑐𝑤 2 𝑟𝑐𝑤 31 𝑟𝑐𝑤 32 𝑟𝑐𝑤 33 𝑡𝑐𝑤 3 𝑋𝑤 𝑌𝑤 𝑍𝑤 1 𝐗𝑐 = 𝑋𝑐 , 𝑌𝑐 , 𝑍𝑐 T ：カメラ座標系の3D点 𝐗𝑤 = 𝑋𝑤 , 𝑌𝑤 , 𝑍𝑤 T ：ワールド座標系の3D点 R𝑐𝑤 , 𝐭𝑐𝑤 ：カメラの回転行列，並進ベクトル ഥ 𝐗𝑤 = 𝑋𝑤 , 𝑌𝑤 , 𝑍𝑤 , 1 T ： 𝐗𝑤 の同次座標
8. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 予備知識： 回転行列と回転ベクトル 8
9. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 回転行列と回転ベクトル カメラ姿勢の剛体変換T（6自由度）と相似変換S（7自由度）は回転行列R ∈ SO(3) と並進 ベクトル𝐭 ∈ ℝ3，スケールパラメータ𝑠を用いてそれぞれ以下のように表現 SO 3 ： 3次の特殊直交群（Special Orthogonal group） ⇒ 3次元の回転 SE(3) ： 〃 特殊ユークリッド群（Special Euclidean group） ⇒ 〃 回転 + 並進 Sim(3)： 〃 相似変換群（Similarity transformation group） ⇒ 〃 回転 + 並進 + スケール 上記のリー群に付随するリー代数（リー環） 𝔰𝔬 3 ， 𝔰𝔢 3 ， 𝔰𝔦𝔪 3 • Rは3自由度の座標変換であるにも関わらず9つのパラメータで表現 • 回転行列の条件（直交性と行列式が1）も満たす必要性 R（3 × 3の行列表現）は最適化変数として扱うには不適 H. Strasdat et al., “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010
10. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 回転行列と回転ベクトル Rを回転ベクトル表現 𝝎 = 𝜔1 , 𝜔2 , 𝜔3 T ∈ 𝔰𝔬 3 で表現 • 𝔰𝔬 3 からSO 3 への指数写像 exp ∶ 𝔰𝔬 3 → SO 3 𝝎 → R • 𝜃 = 𝝎 2 ： 𝝎の回転角 • ∙ 2 はL2ノルム（一般的な距離の概念であるユークリッド距離） • この写像の関係はロドリゲスの回転公式 （Rodrigues’ rotation formula）により以下 の式で表現 I𝑛 は𝑛 × 𝑛の単位行列I𝑛 ， ∙ × は歪対称行列の作用素 • SO 3 から𝔰𝔬 3 への対数写像 log ∶ SO 3 → 𝔰𝔬 3 R → 𝝎 H. Strasdat et al., “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010
11. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 回転行列と回転ベクトル • 並進𝐯も含めたカメラ姿勢 𝐩 = 𝝎T, 𝒗T T ∈ 𝔰𝔢 3 からSE(3)への写像 • スケール𝜎も含めた𝐬 = 𝝎T, 𝜎, 𝒗T T ∈ 𝔰𝔦𝔪 3 からSim(3)への写像 H. Strasdat et al., “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010
12. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 予備知識： シューア補行列 12
13. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 行列A𝑝×𝑝 , B𝑝×𝑞 , C𝑞×𝑝 , D𝑞×𝑞 を用いてブロック行列 M 𝑝+𝑞 × 𝑝+𝑞 を定義 • Dが正則のとき、区分行列の区画Dに関するシューア補行列M/D𝑝×𝑝 • Aが正則のとき、区分行列の区画Aに関するシューア補行列M/A𝑞×𝑞 シューア補行列 M/D ≡ A − BD−1C M−1 = M/D −1 − M/D −1BD−1 −D−1C M/D −1 D−1 + D−1C M/D −1BD−1 M ≡ A B C D M/A ≡ D − CA−1B M−1 = A−1 + A−1B M/𝐴 −1CA−1 −A−1B M/𝐴 −1 − M/𝐴 −1CA−1 M/𝐴 −1 （導出は後述） （導出は後述）
14. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) M ≡ A B C D = ഥ A B Ο D I𝑝 Ο D−1C I𝑞 = I𝑝 BD−1 Ο I𝑞 ഥ 𝐀 Ο Ο 𝐃 I𝑝 Ο D−1C I𝑞 区分行列Mの区画Dに関するシューア補行列M/D𝑝×𝑝 をഥ Aとおく ഥ Aを用いてMをブロック対角行列を含む行列積に分解すると XYZ −1 = Z−1Y−1X−1であるから、ഥ AとDが正則（逆行列が存在）ならば シューア補行列を用いた逆行列の導出（M/D𝑝×𝑝 の場合） ഥ A ≡ M/D𝑝×𝑝 ≡ A − BD−1C A B C D −1 = I𝑝 Ο −D−1C I𝑞 ഥ 𝐀−𝟏 Ο Ο 𝐃−𝟏 I𝑝 −BD−1 Ο I𝑞 = ഥ A−1 Ο −D−1Cഥ A−1 D−1 I𝑝 −BD−1 Ο I𝑞 = ഥ A−1 −ഥ A−1BD−1 −D−1Cഥ A−1 D−1 + D−1Cഥ A−1BD−1
15. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 区分行列Mの線形方程式をシューア補行列ഥ Aを利用して行列積に分解すると これを２つの方程式に展開すると 一式目から𝐱1 を計算し、その結果を二式目に代入することで𝐱2 を計算 シューア補行列を用いた線形方程式の解法（M/D𝑝×𝑝 の場合） ഥ A𝐱1 = ҧ 𝐛1 D𝐱2 = 𝐛2 − C𝐱1 A B C D 𝐱1 𝐱2 = 𝐛1 𝐛2 I𝑝 −BD−1 Ο I𝑞 A B C D 𝐱1 𝐱2 = I𝑝 −BD−1 Ο I𝑞 𝐛1 𝐛2 ഥ A Ο C D 𝐱1 𝐱2 = ҧ 𝐛1 𝐛2 , where ҧ 𝐛1 ≡ 𝐛1 − BD−1𝐛2 𝐱1 = ഥ A−1 ҧ 𝐛1 𝐱2 = D−1 𝐛2 − C𝐱1 ഥ Aのサイズが小さければഥ A−1の計算コストは低い Dがブロック対角行列であればD−1の計算コストは低い
16. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 区分行列Mの区画Aに関するシューア補行列M/A𝑞×𝑞 をഥ Dとおく ഥ Dを用いてMをブロック対角行列を含む行列積に分解すると XYZ −1 = Z−1Y−1X−1であるから、Aとഥ Dが正則（逆行列が存在）ならば シューア補行列を用いた逆行列の導出（M/A𝑞×𝑞 の場合) M ≡ A B C D = A Ο C ഥ D I𝑝 A−1B Ο I𝑞 = I𝑝 Ο CA−1 I𝑞 𝐀 Ο Ο ഥ 𝐃 I𝑝 A−1B Ο I𝑞 A B C D −1 = I𝑝 −A−1B Ο I𝑞 𝐀−𝟏 Ο Ο ഥ 𝐃−𝟏 I𝑝 Ο −CA−1 I𝑞 = A−1 −A−1Bഥ D−1 Ο ഥ D−1 I𝑝 Ο −CA−1 I𝑞 = A−1 + A−1Bഥ D−1CA−1 −A−1Bഥ D−1 −ഥ D−1CA−1 ഥ D−1 ഥ D ≡ M/A𝑞×𝑞 ≡ D − CA−1B
17. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 区分行列Mの線形方程式をシューア補行列ഥ Dを利用して行列積に分解すると これを２つの方程式に展開すると 一式目から𝐱2 を計算し、その結果を二式目に代入することで𝐱1 を計算 シューア補行列を用いた線形方程式の解法（M/A𝑞×𝑞 の場合) ഥ D𝐱2 = ҧ 𝐛2 A𝐱1 = 𝐛1 − B𝐱2 A B C D 𝐱1 𝐱2 = 𝐛1 𝐛2 I𝑝 Ο −CA−1 I𝑞 A B C D 𝐱1 𝐱2 = I𝑝 Ο −CA−1 I𝑞 𝐛1 𝐛2 A B Ο ഥ D 𝐱1 𝐱2 = 𝐛1 ҧ 𝐛2 , where ҧ 𝐛2 ≡ 𝐛2 − CA−1𝐛1 𝐱2 = ഥ D−1 ҧ 𝐛2 𝐱1 = A−1 𝐛1 − B𝐱2 ഥ Dのサイズが小さければഥ D−1の計算コストは低い Aがブロック対角行列であればA−1の計算コストは低い
18. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) バンドル調整 18
19. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) ( ) ( ) ( ) ( , ) ( ) , ( ) ( ) ( ) ( ) ( , ) ( , ) ( , ) ( ) ( , , ) バンドル調整（Bundle adjustment (BA)） 再投影誤差が最小となるようにカメラ姿勢と3D点の位置を最適化 • イメージセンサの観測には誤差が存在 • 観測した2D点 𝐳 と3D点𝐗の間で 𝐳 = 1 𝑍𝑐 K′𝐗𝑐 が正確には満たされない • この誤差をガウス分布と仮定しMAP推定または最尤推定として解く方法論 • 𝑚個のカメラと𝑛個の3D点が与えられた時，各カメラで観測している3D点の再投影 点𝒛 𝐩, 𝐗 と対応する2D点𝒛との距離（geometric error）の総和が最小となるカメラ 姿勢𝐩 ，3D点𝐗を推定 geometric error の模式図（再掲） （再掲）
20. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) バンドル調整（Bundle adjustment (BA)） • インターネット上の画像などを用いたSfM • 正確なカメラの内部パラメーター Kも未知であるため同時にBAで最適化 • Visual SLAM • 計算速度や処理の安定性を重視するため，事前にカメラキャリブレーションを行い 既知とするのが一般的 • 最適化変数 • Motion-only BA ：カメラ姿勢 𝐩 • Structure-only BA：3D点 𝐗 • Full BA ：カメラ姿勢 𝐩， 3D点 𝐗 • 最適化範囲 • Local BA ：最新フレームから過去一定数のフレーム • Global BA：全フレーム • photometric errorを最小化する場合は photometric bundle adjustment と呼ぶ
21. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 再投影誤差の数値最小化 観測誤差を平均0，共分散行列∑のガウス分布と仮定した誤差関数を最小化 • は非線形関数のため，反復法による数値計算で最小値を与える を推定 • エピポーラ幾何や三角測量などにより推定したカメラ姿勢と3D点の座標を初期値 • として更新することで を計算 ：カメラ𝑖の姿勢 𝑖 = 1, … , 𝑚 ：3D点𝑗の位置 𝑗 = 1, … , 𝑛 ∈ 𝔰𝔢 3 B. Triggs et al., “Bundle Adjustment ― A Modern Synthesis”, International Workshop on Vision Algorithms, 1999
22. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) ニュートン法 • を現在の推定値回りで二次の項までテーラー展開 • 右辺を𝛿𝐱に関して微分すると，極小値を与える𝛿𝐱は以下の式を満たす • ガウスニュートン法 • の第二項は第一項と比較して十分小さい，つまり， と近似 • レーベンバーグ・マーカート法 • 左辺にダンピングファクタ を導入 再投影誤差の数値最小化 ヤコビ行列： 勾配（ベクトル）： ヘッセ行列： （𝑘は𝐞の要素番号） B. Triggs et al., “Bundle Adjustment ― A Modern Synthesis”, International Workshop on Vision Algorithms, 1999
23. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) BAにおけるヤコビ行列の計算 ： を全てのカメラ姿勢と3D点の座標 で微分 • 問題設定 • カメラの視点と3D点が各々三つ（𝑚 = 𝑛 = 3） • 透視投影モデル • 内部パラメータは既知 • 観測 はカメラ𝑖の姿勢 と3D点 𝑗 の座標 にのみ依存 • それ以外で偏微分している多くの要素は零行列 ( ) ( ) ( ) ( , ) ( ) , ( ) ( ) ( ) ( ) ( , ) ( , ) ( , ) ( ) ( , 問題設定の例 𝑚 = 𝑛 = 3のヤコビ行列（灰色：零行列） B. Triggs et al., “Bundle Adjustment ― A Modern Synthesis”, International Workshop on Vision Algorithms, 1999
24. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) BAにおけるヤコビ行列の計算 • ， ：カメラ座標系とワールド座標系における3D点の関係 • 連鎖律を用いて ， を以下のように展開 • 投影式 を以下のように展開 • と上式 を各々カメラ姿勢 とカメラ座標系における3D点 で微分 • は以下のように計算可能 （∵ ） B. Triggs et al., “Bundle Adjustment ― A Modern Synthesis”, International Workshop on Vision Algorithms, 1999 G. Gallego et al., “A compact formula for the derivative of a 3-d rotation in exponential coordinates”, JMIV, 2015
25. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) シューア補行列を利用したバンドル調整の高速化 • バンドル調整では一般的に以下が成り立つ • ３次元点パラメータのみの偏微分要素が構成するブロック対角行列がヘッ セ行列の大部分を占める • シューア補行列を利用した逆行列計算を利用して、ニュートン法の方程式 H ො 𝐱 𝛿𝐱 = −𝐠 ො 𝐱 におけるH ො 𝐱 の逆行列計算を高速化 m ≪ 𝑛 m：カメラ視点の数、𝑛：３次元点の数
26. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) ( ) ( ) ( ) ( ) ( ) ( ) バンドル調整におけるヤコビ行列とヘッセ行列 𝐗(1) 𝐗(2) 𝐗(3) 𝐩(1) 𝐩(3) 𝐩(2) 𝐗(1) 𝐗(2) 𝐗(3) 𝐩(1) 𝐩(3) 𝐩(2) 𝐗(1) 𝐗(2) 𝐗(3) 𝐩(1) 𝐩(3) 𝐩(2) ヤコビ行列 ヘッセ行列 カメラ： 3 3D点 ： 3 カメラ姿勢による偏微分 H𝐩𝐩 3D点 による偏微分 H𝐱𝐱 H = H𝐩𝐩 H𝐩𝐱 H𝐱𝐩 H𝐱𝐱 模式的なケース m = 𝑛
27. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) バンドル調整におけるヤコビ行列とヘッセ行列 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ヤコビ行列 ヘッセ行列 𝐗(1) 𝐗(8) 𝐗(2) 𝐗(3) 𝐗(4) 𝐗(5) 𝐗(6) 𝐗(7) 𝐗(10) 𝐗(9) 𝐗(11) 𝐗(12) 𝐗(13) 𝐗(14) 𝐗(15) 𝐩(1) 𝐩(4) 𝐩(5) 𝐩(3) 𝐩(2) 𝐗(1) 𝐗(8) 𝐗(2) 𝐗(3) 𝐗(4) 𝐗(5) 𝐗(6) 𝐗(7) 𝐗(10) 𝐗(9) 𝐗(11) 𝐗(12) 𝐗(13) 𝐗(14) 𝐗(15) 𝐩(1) 𝐩(4) 𝐩(5) 𝐩(3) 𝐩(2) 𝐗(1) 𝐗(8) 𝐗(2) 𝐗(3) 𝐗(4) 𝐗(5) 𝐗(6) 𝐗(7) 𝐗(10) 𝐗(9) 𝐗(11)𝐗(12)𝐗(13)𝐗(14)𝐗(15) 𝐩(1) 𝐩(4) 𝐩(5) 𝐩(3) 𝐩(2) カメラ： 5 3D点 ：15 H = H𝐩𝐩 H𝐩𝐱 H𝐱𝐩 H𝐱𝐱 カメラ姿勢による偏微分 H𝐩𝐩 3D点 による偏微分 H𝐱𝐱 実際のケース m ≪ 𝑛
28. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 先の例では D = H𝐱𝐱 がA = H𝐩𝐩 よりもサイズが大きいブロック対角行列であ るためシューア補行列 ഥ A ≡ ഥ H𝐩𝐩 ≡ H/H𝐗𝑿 ≡ H𝐩𝐩 − H𝐩𝐱 H𝐱𝐱 −1H𝐱𝐩 を利用 これを２つの方程式に展開すると 一式目から∆𝐱1 を計算し、その結果を二式目に代入することで∆𝐱2 を計算 シューア補行列を利用したバンドル調整の高速化 H𝐩𝐩 H𝐩𝐱 H𝐱𝐩 H𝐱𝐱 ∆𝐱1 ∆𝐱2 = 𝐛1 𝐛2 ഥ H𝐩𝐩 Ο H𝐱𝐩 H𝐱𝐱 ∆𝐱1 ∆𝐱2 = ҧ 𝐛1 𝐛2 , where ҧ 𝐛1 ≡ 𝐛1 − H𝐩𝐱 H𝐱𝐱 −1𝐛2 ∆𝐱1 = ഥ H𝐩𝐩 −1 ҧ 𝐛1 ∆𝐱2 = H𝐱𝐱 −1 𝐛2 − H𝐱𝐩 ∆𝐱1 ഥ H𝐩𝐩 のサイズはカメラ視点数に比例するので比較的小さい H𝐱𝐱 は３次元点に関するブロック対角行列 ഥ H𝐩𝐩 ∆𝐱1 = ҧ 𝐛1 H𝐱𝐱 ∆𝐱2 = 𝐛2 − H𝐱𝐩 ∆𝐱1
29. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) ループクロージングと ポーズグラフ最適化 29
30. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) ループクロージング 30 カメラ軌跡全体を修正 • 未知領域から計測済みの領域に戻って来たことを認識 • 相対姿勢の累積が0になることを利用 • 二段階の処理 • ループ検出 • 類似画像検索によるループ候補検出 • Sim(3)推定によるループ判定 • ループ修正 • ループ構築 • スケールドリフトを考慮したポーズグラフ最適化 • Global BA
31. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sim(3)推定によるループ判定 31 Sim(3)推定+RANSACによる幾何検証でループ判定 • 復元済みの3D点の形状を比較して類似度を推定 • 単眼カメラの場合はスケールドリフトが発生 • 3次元の相似座標変換Sim(3)により3D点を座標変換し比較 • 複数の3D点の対応からSim(3)変換 S を推定 • Horn’s method，Umeyama’s methodなど複数存在 (c) PGOとGlobal BAで累積誤差を修正 (a) Sim(3)推定によるループ判定 （b）最新キーフレームとその3D点を移動しループ構築
32. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sim(3)推定によるループ判定 32 Sim(3)推定 & RANSACによる幾何検証でループ判定 • 具体的な処理手順 1. Sim(3)推定+RANSACで仮のSを推定 2. 仮のSをガイドとし再度対応点を探索 3. Sim(3)に拡張したmotion-only BAで最適化された෠ Sを取得 4. この෠ Sを利用して正しい対応点を改めて推定 5. 正しい対応点の数が一定数以上であればループキーフレーム𝐾𝑙 として判定 (c) PGOとGlobal BAで累積誤差を修正 (a) Sim(3)推定によるループ判定 （b）最新キーフレームとその3D点を移動しループ構築
33. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) ループ構築 33 (c) PGOとGlobal BAで累積誤差を修正 (a) Sim(3)推定によるループ判定 （b）最新キーフレームとその3D点を移動しループ構築 ローカルマップをループキーフレーム𝐾𝑙 側へ移動 1. 推定されたSim(3)変換෠ Sを以下に適用 • 最新キーフレーム𝐾𝑖 • 𝐾𝑖 の周辺キーフレーム群 • 𝐾𝑖 ， から観測される3D点𝐗𝑖 ， 𝐗𝑐 2. 𝐾𝑙 の周辺キーフレームから観測されている3D点𝐗𝑙 を，移動した𝐾′𝑖 ， に投影して2D点と関係付け • 3D点が重複した場合は統合
34. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) スケールドリフトを考慮したポーズグラフ最適化 ポーズグラフ最適化（ Pose Graph Optimization (PGO) ） • 同一地点に戻ってきた制約を利用してカメラ軌跡の誤差を修正 • 単眼カメラの場合 • スケールドリフトが存在（図.a） • 6自由度のPGOではスケール誤差を修正不可（図.b） • スケールドリフトを考慮した7自由度のPGOにより誤差を修正可能（図.c） 34 スケールドリフトを考慮したポーズグラフ最適化（PGO） H. Strasdat, “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010（画像を引用）
35. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) スケールドリフトを考慮したポーズグラフ最適化 35 • SE(3)（剛体変換）をSim(3)（相似変換）パラメータへ変換 • 絶対姿勢 T𝑖 ∈ SE 3 → S𝑖 ∈ Sim 3 • 相対姿勢 ΔT𝑖𝑗 ∈ SE(3) → ΔS𝑖𝑗 ∈ Sim 3 • BAと同様に残差と誤差関数を定義し最小化 • 相対姿勢ΔSを制約（固定）条件として絶対姿勢Sを最適化 • 推定したSを利用し各KFが観測している3D点の位置も修正 W：残差の重み行列 𝐬 ∈ 𝔰𝔦𝔪 3 ： Sのリー環 H. Strasdat, “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010
36. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Global BA 全てのカメラと3D点に対する6自由度のバンドル調整 • PGOではカメラの相対姿勢のみを拘束条件として最適化 • PGO後に3D点も含めて大域最適化 • Global BA完了後 • ループクロージング中に並列処理のローカルマッピングが新たなKFや3D点を生成 • ループロージング対象外のKFや3D点へ修正量を伝搬 36 (c) PGOとGlobal BAで累積誤差を修正 (a) Sim(3)推定によるループ判定 （b）最新キーフレームとその3D点を移動しループ構築
37. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) ソースコード解説 37
38. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Bundle Adjustment用クラス bundle_adjuster.h 38 num_threads_: スレッド数 use_analytic_diff: 解析微分の使用 use_Sim3_cache: 過去の計算結果を再利用 𝑥s 𝑦s 𝑧s shot’s c.s. 𝑥1 𝑦1 𝑧1 𝑥2 𝑦2 𝑧2 cam1’s c.s. cam2’s c.s. 𝑥o 𝑦o 𝑧o world’s c.s. マルチカメラシステム（2台） 透視投影カメラモデルの追加 id: カメラモデルID, fx, fy : 焦点距離, cx, cy: カメラ中心 pixel_baseline: ステレオカメラのベースライン長（ピクセル単位） pose_offset: shot座標系に対する相対姿勢 flags: 内部パラメータ& pose_offsetを固定 or 最適化 Equirectangularカメラモデルの追加 id: カメラモデルID, rows, cols : 画像の縦横解像度 pose_offset: shot座標系に対する相対姿勢 flags: pose_offsetを固定 or 最適化 pose_offset カメラ（shot）姿勢の追加・取得 id: カメラ（shot）ID Vec6_t params: 𝔰𝔢 3 姿勢パラメータ Vec7_t params: 𝔰𝔦𝔪 3 姿勢パラメータ
39. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Bundle Adjustment用クラス bundle_adjuster.cc 39 損失関数の種類 図の引用: http://ceres-solver.org/nnls_modeling.html Trivial loss: 𝜌 𝑠 = 𝑠 Huber loss: 𝜌 𝑠 = ቊ 𝑠 𝑠 ≤ 1 2 𝑠 − 1 𝑠 > 1 Soft L1 loss: 𝜌 𝑠 = 2 1 + 𝑠 − 1 Cauchy loss: 𝜌 𝑠 = log 1 + 𝑠 単純な二乗誤差 ( 𝑠 is a squared norm ： 𝑠 = 𝑓𝑖 2 ) 誤差が小さい時は二次的、 大きい時は線形的な挙動 （外れ値に頑健） 重度の外れ値に対応可
40. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Bundle Adjustment用クラス bundle_adjuster.cc 40 カメラモデルの内部パラメータを問題に追加 カメラモデルの内部パラメータを固定(最適化しない) カメラモデルのext_params*を固定(最適化しない) カメラモデルのext_params*を問題に追加 *shot座標系に対する相対姿勢 (bundle_adjuster.hのpose_offset) カメラの外部パラメータを問題に追加 カメラの外部パラメータを固定(最適化しない)
41. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Bundle Adjustment用クラス bundle_adjuster.cc 41 ３次元点のパラメータを固定(最適化しない) 再投影誤差とその微分を計算 ロス関数の種類（+スケールパラメータ）を設定 ３次元点のパラメータを問題に追加 コスト関数を問題に追加
42. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Bundle Adjustment用クラス bundle_adjuster.cc 42 キャッシュを有効化（過去の計算結果を再利用） 最適化時に疎なシューア補行列を利用 最適化を実行 キャッシュを無効化
43. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) 再投影誤差 reprojection_error.h 43 再投影誤差と解析微分を計算 再投影誤差と自動微分を計算
44. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in SE(3) demo_corridor_ba.cc カメラの内部パラメータを設定 𝑓：焦点距離、𝑊, 𝐻：画像の解像度 3D点群の手前を直進するカメラ軌跡にランダム性を 加えて生成 47 地面と両脇の壁がある幅4m × 高さ4m × 奥行き20mの廊下を表す3D 点群（解像度 4 10−1 = 0.444…m）を生成 𝑥 𝑦 𝑧 カメラの視点数を設定 3D点、カメラの回転・並進のノイズパラメータを設定 4m 4m 20m 回転パラメータはr ∈ 𝔰𝔬 3 で生成しR ∈ SO(3)へ変換
45. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in SE(3) demo_corridor_ba.cc 1番目の視点(shot)のR, 𝐭を設定 • Lie::SE3::log(): SE(3)を𝔰𝔢 3 へ変換 • FLAG_FIX_PARAMS：外部パラメータを固定 48 バンドル調整用クラスを宣言（スレッド数1、解析微分利用） 透視投影カメラ（id=0）の内部パラメータを設定 FLAG_FIX_INTRINSIC_PARAMS: 内部パラメータを固定 0番目の視点(shot)のR, 𝐭を設定 • Lie::SE3::log(): SE(3)を𝔰𝔢 3 へ変換 • FLAG_FIX_PARAMS：外部パラメータを固定 原点とスケールを固定 カメ姿勢のノイズd𝐫, d𝐭 ∈ 𝔰𝔢 3 を生成 2番目の視点(shot)以降のカメラ姿勢へノイズを付与 R, 𝐭 ∈ SE(3) を𝐫, 𝐭 ∈ 𝔰𝔢 3 へ変換 𝐫, 𝐭へノイズd𝐫, d𝐭を付与 外部パラメータを最適化対象に設定
46. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in SE(3) demo_corridor_ba.cc 49 i番目の３次元点にノイズを加えて登録 false: ３次元点のパラメータを固定しない（最適化対象に設定） j番目の視点(shot)におけるi番目の３次元点の観測座標 𝑢, 𝑣 を計算 ３次元点がカメラの前面にあり観測座標 𝑢, 𝑣 が画像の範囲内の場合、 再投影誤差のパラメータとして以下を設定 • カメラモデル（id=0） • j番目の視点(shot)がi番目の３次元点を観測した時の座標が 𝑢, 𝑣 • Trivial Loss 𝜌 𝑠 = 𝑠（Lossに変更を加えない）を設定 環境変数に”SHOW_IMAGE”が設定されていれば結果の可視化を設定 可視化用の画像を生成 画像を表示 solverに登録した情報からバンドル調整の問題設定を構築
47. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in SE(3) demo_corridor_ba.cc 50 バンドル調整の実行(最大イテレーション回数20回) バンドル調整の最適化計算に要した時間を表示
48. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in Sim(3) demo_sim3_point_alignment.cc 51 cam2 to cam1のSim(3)パラメータS21 = 𝑠21 R21 𝐭21 0 1 を設定 0,0, −7 にノイズを加えワールド座標系におけるカメラ位置trjを設定 半径4の球面上に、経度方向8、 緯度方向５等分した点群を生成 world to cam1の並進ベクトル𝐭𝑤1 を計算 基準となるカメラ1の姿勢を設定 cam2 to cam1のSim(3)パラメータS21 をworld to cam2のSim(3)パラ メータS𝑤2 に変換 Sim(3)変換されたカメラ2の姿勢を設定 S𝑤2 = 𝑠𝑤2 R𝑤2 𝐭𝑤2 0 1 = 𝑠12 R12 𝐭12 0 1 R𝑤1 𝐭𝑤1 0 1 = 𝑠21 R21 𝐭21 0 1 −1 R1 𝐭1 0 1 = 1 𝑠21 R21 T − 1 𝑠21 R21 T𝐭21 0 1 R𝑤1 𝐭𝒘1 0 1 𝑥 𝑦 𝑧 ∅ 𝜃 𝑟
49. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in Sim(3) demo_sim3_point_alignment.cc 52 0番目の視点(shot)は3台のマルチカメラシステムとして設定 ローカル（shot）座標系から各カメラ座標系へのoffsetをランダムに設定 カメラの内部パラメータを設定 𝑓：焦点距離、𝑊, 𝐻：画像の解像度 バンドル調整用クラスを宣言（スレッド数2、解析微分利用） 透視投影カメラ（id=0, 1, 2）の内部・外部パラメータを設定＆固定 FLAG_FIX_INTRINSIC_PARAMS : 内部パラメータを固定 FLAG_FIX_EXTRINSIC_PARAMS: 外部offsetパラメータを固定
50. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in Sim(3) demo_sim3_point_alignment.cc 53 0番目の視点(shot)のSE(3)パラメータを固定、 1番目の視点のSim(3) パラメータを最適化対象に設定 Point1, 2のi番目の３次元点を0, 1番目の視点(shot)の観測として登録 true: ３次元点のパラメータを最適化対象に設定 j番目の視点(shot)におけるi番目の３次元点の観測座標 𝑢, 𝑣 を計算 ３次元点がカメラの前面にあり観測座標 𝑢, 𝑣 が画像の範囲内の場合、 再投影誤差のパラメータとして以下を設定 • カメラモデル（id=j） • 1番目の視点(shot)がi番目の３次元点を観測した時の座標が 𝑢, 𝑣 • Trivial Loss 𝜌 𝑠 = 𝑠（Lossに変更を加えない）を設定
51. Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in Sim(3) demo_sim3_point_alignment.cc 54 solverに登録した情報からバンドル調整の問題設定を構築 バンドル調整の実行(最大イテレーション回数20回) 最適化して得られたSim(3)パラメータを変数に代入 最適化結果の表示