Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 開発者：大川 快、渋谷 樹弥 支援者：Open Source AI Association 1

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) GSRAPとは？ Geometric Solvers for Reconstruction And Pose estimation • 機能 (追加予定) • 2D-2D対応点を利用した相対姿勢の推定 • Nister’s Five points algorithm • 2D-3D対応点を利用した絶対姿勢の推定 • P3P algorithm, EPnP • 3D-3D対応点を利用した剛体・相似変換の推定 • Umeyamaアルゴリズム • Random Sample Consensus (RANSAC) • 外れ値除去の汎用フレームワーク • 特徴 • C++17 • Python wrapper • Tutorial and sample codes 2

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 目次 • 座標変換とカメラの投影モデル • エピポーラ幾何 • 基礎行列F (Fundamental Matrix) • 基本行列E (Essential Matrix) • 2D-2D対応点を利用した相対姿勢の推定 • 2D-3D 〃 絶対 〃 • 3D-3D 〃 剛体・相似変換 〃 • アルゴリズム解説 • P3Pアルゴリズム（SE3推定） • Umeyamaアルゴリズム（Sim3推定） • Random Sample Consensus (RANSAC) • ソースコード解説

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 座標変換とカメラの投影モデル 4

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 座標変換とカメラの投影モデル 3D点を画像平面へ投影 • ワールド座標系→カメラ座標系→画像座標系 • 座標系 • ワールド：全カメラ共通の座標系 • カメラ ：任意のカメラに固定された座標系 • 画像 ：任意の画像平面 〃 • 内部パラメータ • カメラ座標系→画像座標系の変換 • 外部パラメータ • ワールド座標系→カメラ座標系の変換 5 , ワールド座標系 カメラ座標系 R , R , , ワールド R , カメラ座標系と画像座標系の関係 ワールド座標系とカメラ座標系の関係

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 座標変換とカメラの投影モデル カメラは空間中の物体に反射した光が撮像素子に投影されることでシーン を記録 • この関係を表すのがカメラの内部パラメータK • 最も単純な透視投影モデル 𝑓𝑢 , 𝑓𝑣 ：各軸の焦点距離 𝑐𝑢 , 𝑐𝑣 ：光学中心 𝐗𝑐 = 𝑋𝑐 , 𝑌𝑐 , 𝑍𝑐 T：カメラ座標系の3D点 𝐳𝑐 = 𝑢, 𝑣 T ：画像平面上の2D点 K′ ≔ 𝑓𝑢 0 𝑐𝑢 0 𝑓𝑣 𝑐𝑣 K ≔ 𝑓𝑢 0 𝑐𝑢 0 𝑓𝑣 𝑐𝑣 0 0 1 𝐳 = 1 𝑍𝑐 K′𝐗𝑐 , ワールド座標系 カメラ座標系 R , R , , ワールド R , カメラ座標系と画像座標系の関係 ワールド座標系とカメラ座標系の関係

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 座標変換とカメラの投影モデル カメラ座標系がワールド座標系と異なる場合，ワールド座標系からカメラ 座標系へ変換 • 前ページの投影モデルに 𝐗𝑐 = 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 ： 𝐗𝑤 の同次座標 7 , ワールド座標系 カメラ座標系 R , R , , ワールド R , カメラ座標系と画像座標系の関係 ワールド座標系とカメラ座標系の関係

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 予備知識：回転行列と回転ベクトル カメラ姿勢の剛体変換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 𝐑（3×3の行列表現）は最適化変数として扱うには不適 • Rは3自由度の座標変換であるにも関わらず9つのパラメータで表現 • 回転行列の条件（直交性と行列式が1）も満たす必要性 8 H. Strasdat et al., “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) エピポーラ幾何 9

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) エピポーラ幾何 2つのカメラと3D点の空間的対応関係 • 3Dシーンを撮影した画像間の対応点における拘束条件 • 基礎行列F：画像座標系（K が未知） • 基本行列E：カメラ座標系（K が既知） • F or Eからカメラ姿勢R𝑐𝑤 , 𝐭𝑐𝑤 を推定 エピポーラ平面 • カメラ中心𝐎𝟏 ， 𝐎𝟐 ，3D点𝐗を通る平面 エピポーラ線𝐥 • エピポーラ平面と画像平面が交わる線 エピポール𝐞 • 𝐎𝟏 ， 𝐎𝟐 を結んだ直線が画像平面と交わる点 （全ての𝐥は𝐞を通過） エピポーラ幾何の概要 10

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 基礎行列F (Fundamental Matrix) カメラの内部パラメータKと2つのカメラ間の外部パラメータ（相対姿勢） R | 𝐭 の情報を含む行列 エピポーラ拘束 • 一方の画像上の点෤ 𝐳1 が決まれば，もう一方の画像上の対応点෤ 𝐳2 に関する制約を与える • 画像𝐼1 上の点෤ 𝐳1 に対応する画像𝐼2 上のエピポーラ線は 𝐥2 = F෤ 𝐳1 • これを先の式に代入すると෤ 𝐳2 T𝐥2 = 0 ⇒ 直線のベクトル方程式 • Fの自由度は7 • スケール不定 • det F = 0 11 R. Hartley, A. Zisserman, “Multiple View Geometry in Computer Vision”, Cambridge University Press, 2003 𝑢2 𝑣2 1 𝑓11 𝑓12 𝑓13 𝑓21 𝑓22 𝑓23 𝑓31 𝑓32 𝑓33 𝑢1 𝑣1 1 = ෤ 𝐳2 TF෤ 𝐳1 = 0

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) • K1 ，K2 ：各カメラの内部パラメータ • ො 𝐳1 = K1 −1෤ 𝐳1 ，ො 𝐳2 = K2 −1෤ 𝐳2 ： 𝑍𝑐 =1の平面（正規化画像座標系）に射影した点 • Eの自由度は5（詳細は下記論文を参照） • スケール不定 • det E = 0 • 2EETE − tr EET E = 0 カメラ間の外部パラメータ（相対姿勢） R | 𝐭 の情報を含む行列 ෤ 𝐳2 TF෤ 𝐳1 = 0に代入すると 基本行列E (Essential Matrix) E ≔ K2 T F K1 12 ෤ 𝐳2 T K2 T −1 E K1 −1 ෤ 𝐳1 = 0 ො 𝐳2 T E ො 𝐳1 = 0 O. D. Faugeras, S. Maybank , “Motion from point matches: Multiplicity of solutions”, IJCV, Vol.4, 1990

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 基本行列E (Essential Matrix) Eから R | 𝐭 を計算 ො 𝐳2 T 𝐭 × Rො 𝐳1 = 0 𝐳1 𝐗 𝐳2 𝐞𝟏 𝐞𝟐 O1 O2 R, 𝐭 ො 𝐳2 T 𝐭 × Rො 𝐳1 = 0 カメラ2の座標系から見た画素𝐳1 の方向ベクトル ො 𝐳1 ො 𝐳2 エピポーラ平面に垂直なベクトル （つまり𝐳2 の方向ベクトルො 𝐳2 とも直交するため内積0） 上式を歪み対称行列 𝐭 × で表すと E = 𝐭 × R 先のエピポーラ拘束の式 ො 𝐳2 T E ො 𝐳1 = 0 から 13 R. Hartley, A. Zisserman, “Multiple View Geometry in Computer Vision”, Cambridge University Press, 2003

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 基本行列E (Essential Matrix) Eから R | 𝐭 を計算 • Singular Value Decomposition of E （SVDには符号の不定性あり） • 2通りの行列分解 E = 𝐭 × R = SRが存在 • Eの符号が不定なため並進𝐭の符号も不定 • 合計で４通りの解が存在 W = 0 −1 0 1 0 0 0 0 1 and Z = 0 0 0 −1 0 0 0 0 1 S = U𝑍UT R = UWVT or UWTVT E = U diag 1,1,0 VT 𝐭 = 𝐮3 or -𝐮3 R. Hartley, A. Zisserman, “Multiple View Geometry in Computer Vision”, Cambridge University Press, 2003 R | 𝐭 = UWVT| +𝐮3 or UWVT| −𝐮3 or UWTVT| +𝐮3 or UWTVT| −𝐮3 14

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) A B (a) A B (b) A B’ (c) A B’ (d) ✔ × × × 基本行列E (Essential Matrix) Eから R | 𝐭 を計算 Eを分解した4通りの R | 𝐭 を図示すると以下の通り • 「3D点が両カメラの前方に復元」という条件で正しい解を一つ選択 R. Hartley, A. Zisserman, “Multiple View Geometry in Computer Vision”, Cambridge University Press, 2003 15

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 2D-2D対応点を利用した相対姿勢の推定 16

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 基礎行列Fの推定 𝑛点のエピポーラ拘束を線型方程式で表現 • 8個の対応点を用いてFを推定（8点アルゴリズム） • スケールが任意のため線形解法を解く上での未知数は８（本来の自由度は７） • 対応点が8つ以上ある場合 • 全ての対応点を利用して特異値分解(SVD)などの最小二乗法で計算 • 特徴点が同一平面上に分布する場合は行列Aが縮退 • 基本行列Eやホモグラフィ行列Hで R | 𝐭 を推定 17

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 基本行列Eの推定 大きく分けて二通り • 8点アルゴリズムなどで先に求めたFに代入して基本行列E を推定 E ≔ K2 T F K1 • 5点アルゴリズムのように直接Eを推定 • 5点アルゴリズムは8点アルゴリズムよりも必要な対応点数が少ない • RANSACでランダムサンプルした対応点に外れ値（誤対応）を含む確率が低く， 少ない試行回数で正しい解を求めることが可能 18

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 2D-3D対応点を利用した絶対姿勢の推定 19

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Perspective-n-Point (PnP)問題 3次元空間中のn点の座標とその画像中の対応点が与えらた時にカメラの位 置姿勢R, 𝐭を推定する問題 World Coordinate Camera Coordinate R, 20

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Perspective-n-Point (PnP)問題 P3P 問題 • 幾何学的には最少で𝑛 = 3個の対応点が与えられれば解ける • 1841年にGrunertが最初の解法を提案 • J. A. Grunert, “Das pothenotische problem in erweiterter gestalt nebst bber seine anwendungen in der geodasie,” Grunerts Archiv fur Mathematik und Physik, pages 238–248, 1841 PnP問題 • 𝑛 ≥ 3の点を利用して誤差を最小化 • 𝑛点ソルバーとしてEPnPが一般的 • EPnP: Efficient Perspective-n-Point Camera Pose Estimation • V. Lepetit, F. Moreno-Noguer, P . Fua, “EPnP: An Accurate O(n) Solution to the PnP Problem”, International Journal of Computer Vision (IJCV), 2009 Gaku Nakano, “A Versatile Approach for Solving PnP, PnPf, and PnPfr Problems”, ECCV, 2016 21

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Perspective-n-Point (PnP)問題 PnPf，PnPfr問題 • カメラの位置姿勢に加えて，焦点距離𝑓やレンズ歪み𝑘を推定する問題 • 未知パラメータ数に応じて最小点数も変化 PnP問題と派生問題における未知パラータと推定に必要な対応点の数 Gaku Nakano, “A Versatile Approach for Solving PnP, PnPf, and PnPfr Problems”, ECCV, 2016（表を引用） PnP PnPf PnPfr 回転行列 R ✓ ✓ ✓ 並進ベクトル 𝐭 ✓ ✓ ✓ 焦点距離 𝑓 ✓ ✓ レンズ歪み 𝑘1 , 𝑘2 , 𝑘3 ✓ 未知パラメータ数 6 7 8 (𝑘1 ), 10 (𝑘1 , 𝑘2 , 𝑘3 ) 最小点数 𝑛 3 4 4 (𝑘1 ), 5 (𝑘1 , 𝑘2 , 𝑘3 ) 22

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 3D-3D対応点を利用した剛体・相似変換 の推定 23

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 3D-3D対応点を利用した剛体・相似変換の推定 剛体変換 • SE(3)推定 • 回転行列R ∈ SO(3) と並進ベクトル𝐭 ∈ ℝ3 相似変換 • Sim(3)推定 • 回転行列R ∈ SO(3) と並進ベクトル𝐭 ∈ ℝ3 、スケールパラメータ𝑠 24

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 3D-3D対応点を利用した剛体変換の推定 剛体変換 • SE(3)推定 • 回転行列R ∈ SO(3) と並進ベクトル𝐭 推定したR, 𝐭 で座標変換 Point Cloud Library, https://github.com/PointCloudLibrary/pcl/blob/master/test/bunny.pcd （点群データを引用） 𝑥2 𝑦2 𝑧2 1 = R 𝐭 0 1 𝑥1 𝑦1 𝑧1 1 R, 𝐭 25

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 3D-3D対応点を利用した相似変換の推定 相似変換 • Sim(3)推定 • 回転行列R ∈ SO(3) と並進ベクトル𝐭 ∈ ℝ3 、スケールパラメータ𝑠 Point Cloud Library, https://github.com/PointCloudLibrary/pcl/blob/master/test/bunny.pcd （点群データを引用） R, 𝐭, s 推定したR, 𝐭, s で座標変換 𝑥2 𝑦2 𝑧2 1 = 𝑠R 𝐭 0 1 𝑥1 𝑦1 𝑧1 1 26

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) アルゴリズム解説 P3Pアルゴリズム（Grunertの手法） 27

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 3組の2D-3D対応点からT ∈ SE(3)を推定 カメラ中心𝐗𝐨 と3つの3次元点𝐗𝟏 , 𝐗𝟐 , 𝐗𝟑 により構築される三角錐を考える 1. 光線の長さ𝑙1 , 𝑙2 , 𝑙3 を推定 2. カメラ姿勢T ∈ SE(3)を推定 𝐗𝐨 𝐗𝟐 𝐗𝟏 𝐗𝟑 𝛼 𝛽 𝛾 𝑎 𝑏 𝑐 𝑙2 𝑙3 𝑙1 𝐱𝟐 𝐱𝟑 𝐱𝟏 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 28

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 1. 光線の長さ𝑙1 , 𝑙2 , 𝑙3 の推定 カメラ座標系における３次元点の方向ベクトルො 𝐱𝟏 , ො 𝐱𝟐 , ො 𝐱𝟑 𝐗𝐨 𝐗𝟐 𝐗𝟏 𝐗𝟑 𝛼 𝛽 𝛾 𝑎 𝑏 𝑐 𝑙2 𝑙3 𝑙1 ො 𝐱𝟐 ො 𝐱𝟑 ො 𝐱𝟏 𝐱𝟐 𝐱𝟏 𝐱𝟑 ො 𝐱𝒊 = K−𝟏𝐱𝒊 K−𝟏𝐱𝒊 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 29

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 1. 光線の長さ𝑙1 , 𝑙2 , 𝑙3 の推定 光線ベクトルො 𝐱𝟏 , ො 𝐱𝟐 , ො 𝐱𝟑 が成す角度𝛼, 𝛽, 𝛾を計算 𝛼 = arccos ො 𝐱𝟐 , ො 𝐱𝟑 , 𝛽 = arccos ො 𝐱𝟑 , ො 𝐱𝟏 ,𝛾 = arccos ො 𝐱𝟏 , ො 𝐱𝟐 𝐗𝐨 𝐗𝟐 𝐗𝟏 𝐗𝟑 𝛼 𝛽 𝛾 𝑎 𝑏 𝑐 𝑙2 𝑙3 𝑙1 ො 𝐱𝟐 ො 𝐱𝟑 ො 𝐱𝟏 𝐱𝟐 𝐱𝟏 𝐱𝟑 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 30

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 1. 光線の長さ𝑙1 , 𝑙2 , 𝑙3 の推定 余弦定理より以下の３つの方程式が成り立つ（𝑎, 𝑏, 𝑐, 𝛼, 𝛽, 𝛾は既知） 𝐗𝐨 𝐗𝟐 𝐗𝟏 𝐗𝟑 𝛼 𝛽 𝛾 𝑎 𝑏 𝑐 𝑙2 𝑙3 𝑙1 𝑎2 = 𝑙2 2 + 𝑙3 2 − 2𝑙2 𝑙3 cos𝛼 𝑏2 = 𝑙3 2 + 𝑙1 2 − 2𝑙3 𝑙1 cos𝛽 𝑐2 = 𝑙1 2 + 𝑙2 2 − 2𝑙1 𝑙2 cos𝛾 ො 𝐱𝟐 ො 𝐱𝟑 ො 𝐱𝟏 𝐱𝟐 𝐱𝟏 𝐱𝟑 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 31

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 1. 光線の長さ𝑙1 , 𝑙2 , 𝑙3 の推定 𝑢 = 𝑙2 𝑙1 , 𝑣 = 𝑙3 𝑙1 として代入すると 𝑎2 = 𝑙1 2 𝑢2 + 𝑣2 − 2𝑢𝑣cos𝛼 𝑏2 = 𝑙1 2 1 + 𝑣2 − 2𝑣cos𝛽 𝑐2 = 𝑙1 2 1 + 𝑢2 − 2ucos𝛾 𝑙1 2 = 𝑎2 𝑢2 + 𝑣2 − 2𝑢𝑣cos𝛼 𝑙1 2 = 𝑏2 1 + 𝑣2 − 2𝑣cos𝛽 𝑙1 2 = 𝑐2 1 + 𝑢2 − 2𝑢cos𝛾 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 32

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 1. 光線の長さ𝑙1 , 𝑙2 , 𝑙3 の推定 1、3つ目の式を2つ目の式に代入して𝑙1 , 𝑢を消去 𝑙1 2 = 𝑏2 1 + 𝑣2 − 2𝑣cos𝛽 𝑙1 2 = 𝑎2 𝑢2 + 𝑣2 − 2𝑢𝑣cos𝛼 𝑙1 2 = 𝑐2 1 + 𝑢2 − 2𝑢cos𝛾 𝐴4 𝑣4 + 𝐴3 𝑣3 + 𝐴2 𝑣2 + 𝐴1 𝑣 + 𝐴0 = 0 ４次方程式に帰着 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 33

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 1. 光線の長さ𝑙1 , 𝑙2 , 𝑙3 の推定 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 4ሼ ሽ − 𝑎2 − 𝑐2 𝑎2 + 𝑏2 − 𝑐2 cos𝛽 + 2𝑎2𝑏2cos2𝛾cos𝛽 + 𝑏2 𝑎2 − 𝑏2 + 𝑐2 cos𝛼cos𝛾 𝐴4 = 𝑎2 − 𝑏2 − 𝑐2 2 − 4𝑏2𝑐2cos2𝛼 4ሼ ሽ − 𝑎2 − 𝑐2 𝑎2 − 𝑏2 − 𝑐2 cos𝛽 + 𝑎2𝑏2 + 𝑏2𝑐2 − 𝑏4 cos𝛼cos𝛾 + 2𝑏2𝑐2cos2𝛼cos𝛽 2ሼ ሽ 𝑎2 − 𝑐2 2 − 𝑏4 + 2 𝑎2 − 𝑐2 2cos2𝛽 − 2𝑏2 𝑎2 − 𝑐2 cos2𝛼 − 4𝑏2 𝑎2 + 𝑐2 cos𝛼cos𝛽cos𝛾 − 2𝑏2 𝑎2 − 𝑏2 cos2𝛾 𝐴2 = 𝐴0 = 𝑎2 + 𝑏2 − 𝑐2 2 − 4𝑎2𝑏2cos2𝛾 𝐴3 = 𝐴1 = 𝐴4 𝑣4 + 𝐴3 𝑣3 + 𝐴2 𝑣2 + 𝐴1 𝑣 + 𝐴0 = 0 34

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 1. 光線の長さ𝑙1 , 𝑙2 , 𝑙3 の推定 𝐴4 𝑣4 + 𝐴3 𝑣3 + 𝐴2 𝑣2 + 𝐴1 𝑣 + 𝐴0 = 0を𝑣について解き𝑙1 , 𝑙2 , 𝑙3 を計算 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 𝑙1 2 = 𝑏2 1 + 𝑣2 − 2𝑣cos𝛽 𝑙3 = 𝑙1 𝑣 𝑎2 = 𝑙1 2 𝑢2 + 𝑣2 − 2𝑢𝑣cos𝛼 𝑙2 2 − 2𝑙1 𝑙2 𝑣𝑐𝑜𝑠𝛼 + 𝑙1 2𝑣2 − 𝑎2 = 0 𝑙1 , 𝑙2 , 𝑙3 は４通りの解が存在 35

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 1. 光線の長さ𝑙1 , 𝑙2 , 𝑙3 の推定 追加情報① or ②との整合性で4つの解から1つの解を選択 ① GPSなどの追加のセンサ情報 ② 4つ目の対応点 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 36

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pアルゴリズム（Grunertの手法） 2. カメラ姿勢T ∈ SE(3)を推定 推定した𝑙𝑖 を代入してR, 𝐗𝐨 を計算 𝑙𝑖 ො 𝐱𝒊 = R 𝐗𝒊 − 𝐗𝐨 𝑖 = 1,2,3 𝐗𝐨 𝐗𝟐 𝐗𝟏 𝐗𝟑 𝛼 𝛽 𝛾 𝑎 𝑏 𝑐 𝑙2 𝑙3 𝑙1 ො 𝐱𝟐 ො 𝐱𝟑 ො 𝐱𝟏 𝐱𝟐 𝐱𝟏 𝐱𝟑 Cyrill Stachniss, “Projective 3-Point (P3P) Algorithm / Spatial Resection”, Photogrammetry I & II Course, 2021 37

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) アルゴリズム解説 Umeyamaアルゴリズム 38

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Sim(3)推定（Umeyamaアルゴリズム） 𝑛組の3D−3D対応点から相似変換行列を推定 • 入力： 𝑛組の3D−3D対応点 X = 𝐱1 , 𝐱2 , ⋯ , 𝐱𝑛 、Y = 𝐲1 , 𝐲2 , ⋯ , 𝐲𝑛 • 出力： R, 𝐭, 𝑠 ∈ Sim(3) • 次の誤差関数を最小化 • 相似変換したXがYと一致するように R, 𝐭, 𝑠 を推定 𝑒2 R, 𝐭, 𝑠 = 1 𝑛 ෍ 𝑖=1 𝑛 𝐲𝑖 − 𝑠R𝐱𝑖 + 𝐭 2 𝐱𝑖 を相似変換した座標 Shinji Umeyama, “Least-Squares estimation of transformation parameters between two point patterns”, TPAMI, Vol.13, Issue 4, 1991 39

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Sim(3)推定（Umeyamaアルゴリズム） 具体的な計算方法 1. 各点群の重心と共分散行列を計算 2. 共分散行列Σ𝑥𝑦 を特異値分解（Singular Value Decomposition） 3. 1, 2の結果から R, 𝐭, 𝑠 を計算 R = USVT 𝐭 = 𝛍𝑦 − 𝑠R𝛍𝑥 s = 1 𝜎𝑥 2 tr DS 𝛍𝑥 = 1 𝑛 ෍ 𝑖=1 𝑛 𝐱𝑖 𝛍𝑦 = 1 𝑛 ෍ 𝑖=1 𝑛 𝐲𝑖 Σ𝑥𝑦 = 1 𝑛 ෍ 𝑖=1 𝑛 𝐲𝑖 − 𝛍𝑦 𝐱𝑖 − 𝛍𝑥 T SVD Σ𝑥𝑦 = UDVT S = ቊ 𝐼 diag 1,1, ⋯ , 1, −1 if det Σ𝑥𝑦 = 1 if det Σ𝑥𝑦 = −1 D = diag 𝑑𝑖 , 𝑑1 ≥ 𝑑2 ≥ ⋯ ≥ 𝑑𝑚 ≥ 0 Shinji Umeyama, “Least-Squares estimation of transformation parameters between two point patterns”, TPAMI, Vol.13, Issue 4, 1991 40 𝜎𝑥 = 1 𝑛 ෍ 𝑖=1 𝑛 𝐱𝑖 − 𝛍𝑥 2

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Random Sample Consensus (RANSAC) 41

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Random Sample Consensus (RANSAC) • 例：直線パラメータの推定 1. モデル推定に必要なデータ(２点)をサンプル 2. モデルパラメータを推定 3. 当てはめ誤差が閾値内となるInlierを数えてス コアを計算 4. 1-3を繰り返しベストスコアのサンプルを取得 5. 4のサンプルのInlier全てを利用して誤差を最 小化するモデルパラメータを計算 外れ値を含む観測データから数理モデルのパラメータを推定する 反復計算法の一つ 42

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Random Sample Consensus (RANSAC) • 例：直線パラメータの推定 1. モデル推定に必要なデータ(２点)をサンプル 2. モデルパラメータを推定 3. 当てはめ誤差が閾値内となるInlierを数えてス コアを計算 4. 1-3を繰り返しベストスコアのサンプルを取得 5. 4のサンプルのInlier全てを利用して誤差を最 小化するモデルパラメータを計算 外れ値を含む観測データから数理モデルのパラメータを推定する 反復計算法の一つ 43

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Random Sample Consensus (RANSAC) • 例：直線パラメータの推定 1. モデル推定に必要なデータ(２点)をサンプル 2. モデルパラメータを推定 3. 当てはめ誤差が閾値内となるInlierを数えてス コアを計算 4. 1-3を繰り返しベストスコアのサンプルを取得 5. 4のサンプルのInlier全てを利用して誤差を最 小化するモデルパラメータを計算 外れ値を含む観測データから数理モデルのパラメータを推定する 反復計算法の一つ 44

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Random Sample Consensus (RANSAC) • 例：直線パラメータの推定 1. モデル推定に必要なデータ(２点)をサンプル 2. モデルパラメータを推定 3. 当てはめ誤差が閾値内となるInlierを数えてス コアを計算 4. 1-3を繰り返しベストスコアのサンプルを取得 5. 4のサンプルのInlier全てを利用して誤差を最 小化するモデルパラメータを計算 外れ値を含む観測データから数理モデルのパラメータを推定する 反復計算法の一つ 45

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Random Sample Consensus (RANSAC) • 例：直線パラメータの推定 1. モデル推定に必要なデータ(２点)をサンプル 2. モデルパラメータを推定 3. 当てはめ誤差が閾値内となるInlierを数えてス コアを計算 4. 1-3を繰り返しベストスコアのサンプルを取得 5. 4のサンプルのInlier全てを利用して誤差を最 小化するモデルパラメータを計算 外れ値を含む観測データから数理モデルのパラメータを推定する 反復計算法の一つ 46

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Random Sample Consensus (RANSAC) • 例：直線パラメータの推定 1. モデル推定に必要なデータ(２点)をサンプル 2. モデルパラメータを推定 3. 当てはめ誤差が閾値内となるInlierを数えてス コアを計算 4. 1-3を繰り返しベストスコアのサンプルを取得 5. 4のサンプルのInlier全てを利用して誤差を最 小化するモデルパラメータを計算 外れ値を含む観測データから数理モデルのパラメータを推定する 反復計算法の一つ 47

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Random Sample Consensus (RANSAC) • 観測データからインライアのみ選択，つまり正しい推定値を確率𝑝で得る ために必要な試行回数 𝑁 外れ値を含む観測データから数理モデルのパラメータを推定する 反復計算法の一つ 𝑁 = log(1 − 𝑝) log(1 − 𝑟𝑠) 𝑠 : モデル計算に観測データ数 𝑟 : 観測データのインライアの割合 𝑟𝑠 = 1 − 1 − 𝑝 1 𝑁 𝑟𝑠: 重複ありでInlierのみをサンプルする確率 48

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Random Sample Consensus (RANSAC) 局所特徴量による最近傍探索 8点アルゴリズム+RANSACによる誤対応除去 マッチング結果の例（元画像はFountain-P11データセット[68]の画像を利用） 例：8点アルゴリズム 1. 仮の対応点からランダムに8個の対応点を選択し F を推定 2. 推定された F を用いて各対応点に対しエピポーラ線 𝐥 を計算 3. 対応点がインライア（エピポーラ線との距離が閾値以下）か否か を判定 4. 1〜３の計算を 𝑁 回試行した後，インライアが最も多い F の全イ ンライアを用いた最小二乗法により最終的な推定値 ෠ F を推定 49

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) ソースコード解説 50

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) GSRAPに実装されているソルバー（追加予定） 2D-2D対応点を利用した相対姿勢の推定 • Nister’s 5-point algorithm (𝑛 ≥ 5, 基本行列E) • D. Nistér, “An efficient solution to the five-point relative pose problem”, TPAMI, Vol.26, Issue 6, pp.756–777, 2004 2D-3D対応点を利用した絶対姿勢R, 𝐭の推定 • Ke’s P3P algorithm (𝑛 = 3) • T. Ke, S. I. Roumeliotis, “An Efficient Algebraic Solution to the Perspective-Three-Point Problem”, pp.7225-7233, CVPR, 2017 • EPnP: Efficient Perspective-n-Point Camera Pose Estimation (𝑛 ≥ 4) • V. Lepetit, F . Moreno-Noguer, P. Fua, “EPnP: An Accurate O(n) Solution to the PnP Problem”, IJCV, 2009 3D-3D対応点を利用した剛体・相似変換R, 𝐭, sの推定 • Umeyamaアルゴリズム(𝑛 ≥ 3) • S. Umeyama, “Least-Squares Estimation of Transformation Parameters Between Two Point Patterns”, pp. 376-380, Vol.13, TPAMI, 1991 ※ Nister’s 5-point algoritm, EPnPはOpenGV, Ke’s P3P algorithmはOpenCVのコードを利用 • L. Kneip, P. Furgale, "OpenGV: A unified and generalized approach to real-time calibrated geometric vision", ICRA, May 2014 • OpenCV, https://github.com/opencv/opencv 51 RANSAC関数をテンプレートで記述することで全ソルバーへ統一的に適用可

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) ソースコード解説 RANSAC 52

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) RANSACパラメーター gsrap-def.h 観測データからインライアのみサンプリングする確率 𝐩 サンプリングの最大回数 サンプリングの最低回数 その時点でのベストなモデルでInlierの割合𝑟を計算し、Inlierのみを サンプルできる確率𝑟𝑠が指定した確率𝑝を超えたと分かった時点で iterationを終了するフラグ 重複なしのサンプリングで試行回数𝑁を計算する時のフラグ（サ ンプル数が十分大きい場合は近似可能） 53

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) RANSACアルゴリズム ransac.h サンプルデータからモデルを推定 モデルの推定に必要な最小数のデータをランダムサンプル サンプリングの繰返し回数が事前に設定した最大回数を超えたら終了 54

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) RANSACアルゴリズム ransac.h 55 任意のサンプルセットから推定したモデルに対するInlier データの判別

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) RANSACアルゴリズム ransac.h その時点でのベストなモデルのInlier数 重複なし 𝑞no_duplicate = 𝐶𝑥 𝑀 × ⋯× 𝐶− 𝑠−1 𝑀− 𝑠−1 𝑀：母集団の数, 𝐶：母集団中の𝐼𝑛𝑙𝑖𝑒𝑟の数, 𝑠：サンプル数 ベストモデルのInlierの割合からInlierのみサンプルする確率𝑞を計算 重複あり 𝑞duplicate = 𝑟𝑠 ※𝑞duplicate の計算のみ重複を考慮（実際には重複なしでサンプル） 厳密には重複なしの方が正確であるが、母集団のサンプル数𝑀が 一般的に大きいため𝑞no_duplicate ≈ 𝑞duplicate となる 56

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) RANSACアルゴリズム ransac.h itr_upperlimitが 𝑁より大きければ更新 (つまり、ベストモデルが更新されるとInlierの割合𝑟が高くなる ため、より少ないサンプル回数で済む) 計算した𝑞が、現在の上限回数itr_upperlimitから計算したInlierのみ サンプルする確率よりも大きい場合はサンプル回数𝑁を計算 確率𝑞から必要なサンプル回数𝑁を計算（𝑁 の計算における0割り &オーバーフロー防止） 57

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) ソースコード解説 2D-2D対応点を利用した相対姿勢の推定 58

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) E行列からのR, 𝐭推定 essential_solver.cc Nister’s 5-point algorithmによりE行列を計算 （入力：各画像における特徴点の方向ベクトルbearings1, bearings2） 並進ベクトルのスケールを1にするために正規化 59 １、２つ目の特異値が等しく、3つ目の特異値が0であるこ とを確認（数値誤差を考慮） ∵ SVD E = U diag 1,1,0 VT 特異値分解を実行 3 × 3の行列Mの行列式が1か否かを判定するラムダ関数を定義 （誤差を考慮）

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) E行列からのR, 𝐭推定 essential_solver.cc 60 U, VTどちらかだけが鏡映変換（det ∙ < 0）だと右手・左手座標 系が入れ替わるためWの最後の要素で調整 𝐭 = 𝐮3 or -𝐮3 R | 𝐭 = UWVT| +𝐮3 or UWVT| −𝐮3 or UWTVT| +𝐮3 or UWTVT| −𝐮3 Eを分解した4通りの R | 𝐭 から「3D点が両カメラの前方に復 元」という条件で正しい解を一つ選択

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Nisterの5点アルゴリズムを利用したE行列推定 example_essential_solver.cc RANSACのパラメーター設定 ※std::thread::hardware_concurrency()は処理系でサポートされている スレッド並行数を取得する関数 E行列を計算 RANSACのパラメーター設定 E行列の固有値が条件を満たしていることをチェックする機能をセット 誤対応なしの対応点情報を生成 シミュレーション用のカメラ姿勢と対応点を生成 • 3D点群の中心座標、半径 • 3D点群の生成 • 2視点のカメラ姿勢の設定 • 3D点群を各カメラ座標系における方向ベクトルへ変換 61

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) ソースコード解説 2D-3D対応点を利用した絶対姿勢の推定 62

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) P3Pを利用した絶対姿勢R, 𝐭推定 example_p3p_solver.cc RANSACのパラメーター設定 R, 𝐭を計算 P3Pソルバーのパラメーター設定 誤対応なしの対応点情報を生成 シミュレーション用のカメラ姿勢と対応点を生成 • 3D点群の中心座標、半径 • 3D点群の生成 • 2視点のカメラ姿勢の設定 • 3D点群を各カメラ座標系における方向ベクトルへ変換 ベアリングベクターの角度誤差でInlier判定する場合に宣言 （AssumePerspectiveCameraの場合は画像空間の再投影誤差） 63

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) EPnPを利用した絶対姿勢R, 𝐭推定 example_pnp_solver.cc RANSACのパラメーター設定 PnPソルバーのパラメーター設定 誤対応なしの対応点情報を生成 シミュレーション用のカメラ姿勢と対応点を生成 • 3D点群の中心座標、半径 • 3D点群の生成 • 2視点のカメラ姿勢の設定 • 3D点群を各カメラ座標系における方向ベクトルへ変換 64 ベアリングベクターの角度誤差でInlier判定する場合に宣言 （PnpInlierCheckParamsUsingProjectedPointの場合は画像 空間の再投影誤差） R, 𝐭を計算

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) ソースコード解説 3D-3D対応点を利用した剛体・相似変換 の推定 65

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Umeyamaアルゴリズム sim3_solver.cc 各点群の重心を計算 𝛍𝑥 = 1 𝑛 σ𝑖=1 𝑛 𝐱𝑖 、𝛍𝑦 = 1 𝑛 σ𝑖=1 𝑛 𝐲𝑖 各点群の座標系の原点を重心に移動 𝐱𝑖 − 𝛍𝑥 , 𝐲𝑖 − 𝛍𝑦 共分散行列を計算 Σ𝑥𝑦 = 1 𝑛 σ𝑖=1 𝑛 𝐲𝑖 − 𝛍𝑦 𝐱𝑖 − 𝛍𝑥 T 特異値分解SVD Σ𝑥𝑦 = UDVT D = diag 𝑑𝑖 , 𝑑1 ≥ 𝑑2 ≥ ⋯ ≥ 𝑑𝑚 ≥ 0 rank Σ𝑥𝑦 ≥ 2の時、最適解が一意に定まる 回転行列(右手座標系)の行列式が1となるように det Σ𝑥𝑦 = −1の場合は鏡映変換(reflection) S = ቊ 𝐼 diag 1,1, ⋯, 1, −1 if det Σ𝑥𝑦 = 1 if det Σ𝑥𝑦 = −1 66

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Umeyamaアルゴリズム sim3_solver.cc 回転行列Rを計算 R = USVT スケールsを計算 s = 1 𝜎𝑥 2 tr DS 並進ベクトル𝐭を計算 𝐭 = 𝛍𝑦 − 𝑠R𝛍𝑥 67

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Umeyamaアルゴリズムを用いたSim(3)推定 example_sim3_solver.cc 正解の回転行列 Rgt ：回転軸 1 1 1 T、回転角𝜋 4 正解のスケール 𝑠gt 正解の並進ベクトル 𝐭gt 中心が 0 0 0 T 、各辺が長さ1の立方体内に ランダムに500個の点群P1 を生成 点群P1 を Rgt , 𝐭gt , 𝑠gt で相似変換し点群P2 を生成 誤対応なしの対応点情報を生成 68

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) Umeyamaアルゴリズムを用いたSim(3)推定 example_sim3_solver.cc Inlierのみで最小二乗法により最適化 RANSACのパラメーター設定 R, 𝐭, sを計算 Inlier判定の閾値設定 69

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) まとめ • カメラの幾何の基礎 • 座標変換とカメラの投影モデル • エピポーラ幾何 • 基礎行列F (Fundamental Matrix)、基本行列E (Essential Matrix) • 2D-2D、2D-3D、3D-3D対応点を利用した相対姿勢、絶対姿勢、剛体・相似 変換の推定 • アルゴリズム解説 • P3P、Umeyamaアルゴリズム • Random Sample Consensus (RANSAC) • RANSAC関数をテンプレートで記述することで全ソルバーへ統一的に適用可 • ソースコード解説 • RANSAC 、 E行列の推定とR, 𝐭分解、P3P、EPnP、UmeyamaアルゴリズムとSim(3)推定

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP) 謝辞 本資料の作成にあたり，多視点幾何の基礎が丁寧に説明されてい る参考書「Multiple View Geometry in Computer Vision」の著 者であるRichard Hartley先生, Andrew Zisserman先生、非常に 分かりやすいコンピュータビジョンの授業資料を公開されている Cyrill Stachniss先生，図表の編集，引用をご快諾くださった中 野学様，日頃ご支援頂いている多くの皆様方に心より感謝申し上 げます。