Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP)

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

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

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

座標変換とカメラの投影モデル 4

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

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

予備知識：回転行列と回転ベクトルカメラ姿勢の剛体変換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

エピポーラ幾何 9

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

基礎行列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

• 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

基本行列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

基本行列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

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

2D-2D対応点を利用した相対姿勢の推定 16

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

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

2D-3D対応点を利用した絶対姿勢の推定 19

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

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

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

3D-3D対応点を利用した剛体・相似変換の推定 23

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

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

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

アルゴリズム解説 P3Pアルゴリズム（Grunertの手法） 27

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

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

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

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

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

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

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

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

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

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

アルゴリズム解説 Umeyamaアルゴリズム 38

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

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

Random Sample Consensus (RANSAC) 41

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

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

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

ソースコード解説 50

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関数をテンプレートで記述することで全ソルバーへ統一的に適用可

ソースコード解説 RANSAC 52

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

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

RANSACアルゴリズム ransac.h 55 任意のサンプルセットから推定したモデルに対するInlier データの判別

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

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

ソースコード解説 2D-2D対応点を利用した相対姿勢の推定 58

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か否かを判定するラムダ関数を定義（誤差を考慮）

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

ソースコード解説 2D-3D対応点を利用した絶対姿勢の推定 62

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

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

ソースコード解説 3D-3D対応点を利用した剛体・相似変換の推定 65

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

Umeyamaアルゴリズム sim3_solver.cc 回転行列Rを計算 R = USVT スケールsを計算 s = 1 𝜎𝑥 2 tr DS 並進ベクトル𝐭を計算 𝐭 = 𝛍𝑦 − 𝑠R𝛍𝑥 67

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

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

まとめ • カメラの幾何の基礎 • 座標変換とカメラの投影モデル • エピポーラ幾何 • 基礎行列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)推定

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

Tutorial of Geometric Solvers for Reconstructi...

Tutorial of Geometric Solvers for Reconstruction And Pose estimation (GSRAP)

More Decks by Open Source AI Association

Other Decks in Technology

Featured

Transcript