概要

エピポーラ幾何から導かれる座標間の拘束行列を使用して、カメラパラメータを推定する方法を紹介します。記法は前回の記事に基づきます。

エピポーラ幾何

エピポーラ幾何とは、二つのカメラ画像間での対応点の位置関係を表現する幾何学的な手法です。この手法は、二つの画像を用いた3次元復元や物体追跡に使用されます。

エピポールは、一つ目のカメラから見たときの、もう一つのカメラの中心の投影点です。つまり、一つ目のカメラから見て他のカメラがどこに見えるか、その位置がエピポールです。逆も然りです。

エピポーラ線は、一つ目のカメラ座標上の点に対応する可能性がある、二つ目のカメラ座標上の点を集めた直線です。一つ目のカメラ座標上の点が3次元空間のどの座標に対応するかは、深度の情報がないため分かりません。したがって、二つ目のカメラ座標上のどの座標に対応するかも分かりませんが、エピポーラ線上にあることは分かります。エピポーラ線は、立体視の問題を線上の探索問題に簡略化するのに役立ちます。

エピポーラ平面は、二つのカメラの中心と3次元空間中の点を通る平面を指します。この平面はエピポーラ線を生成し、その交差点はエピポールとなります。

カメラ1のレンズ中心$C_1$を基準にしたカメラ座標のベクトルには添字1を、カメラ2のそれには添字2を付与することにします。したがって、$\boldsymbol{M}_{c,1}$と$\boldsymbol{M}_{c,2}$は空間上の同じ点ですが、座標系が異なるので成分の値が異なることに注意してください。カメラ座標1,2の単位長は同じ大きさであるとします。

エピポーラ平面に着目し、エピポーラ平面内に含まれるベクトルとエピポーラ平面に垂直なベクトルの関係から、ある拘束式を導きます。

(\boldsymbol{M}_{c,1} - \boldsymbol{t}_{c,1})^\top (\boldsymbol{t}_{c,1} \times \boldsymbol{M}_{c,1}) = 0.

ここで外積は歪対称行列$S=[\boldsymbol{t}_{c,1}]_{\times}$で表すことができます。

\begin{align}
\boldsymbol{t}_{c,1} \times \boldsymbol{M}_{c,1} &= S\boldsymbol{M}_{c,1}, \\
S&=[\boldsymbol{t}_{c,1}]_{\times} = 
\begin{pmatrix}
0 & -t_{c,1}^3 & t_{c,1}^2 \\
t_{c,1}^3 & 0 & -t_{c,1}^1 \\
-t_{c,1}^2 & t_{c,1}^1 & 0
\end{pmatrix}
\end{align}

カメラ1のカメラ座標における$\boldsymbol{M}_{c,1} - \boldsymbol{t}_{c,1}$をカメラ2のカメラ座標における$\boldsymbol{M}_{c,2}$に変換する行列を$R$とします。

\boldsymbol{M}_{c,2} = R(\boldsymbol{M}_{c,1} - \boldsymbol{t}_{c,1}).

以上2つの式を最初の式に代入します。

\begin{align}
(R^{-1} \boldsymbol{M}_{c,2})^\top S \boldsymbol{M}_{c,1} = 0 
\Rightarrow  \boldsymbol{M}_{c,2}^\top R S \boldsymbol{M}_{c,1}  = 0.
\end{align}

$E = RS$と定義し、$ \boldsymbol{M}_{c,1}, \boldsymbol{M}_{c,2}$のそれぞれの画像平面への射影、

\begin{align}
\boldsymbol{x}_{c,1} &= f_1 \boldsymbol{M}_{c,1}/M_{c,1}^z \\
\boldsymbol{x}_{c,2} &= f_2 \boldsymbol{M}_{c,2}/M_{c,2}^z,
\end{align}

を用いると、次の式を得ます。

\begin{align}
\boldsymbol{x}_{c,2}^\top E \boldsymbol{x}_{c,1}  = 0.
\end{align}

$E$を基本行列(E行列)と呼びます。定義から、E行列は外部パラメータのみで求められることがわかります。この方程式は、導出過程から分かるように、エピポーラ線$\ell_1$上の任意の点とエピポーラ線$\ell_2$上の任意の点が満たすべき拘束関係を表しています。

次に、カメラ座標から画像座標へ変換します。$A_1, A_2$をそれぞれのカメラの内部パラメータとします。カメラ座標の同次座標を$\tilde{\boldsymbol{x}}_{c,1}, \tilde{\boldsymbol{x}}_{c,２}$をとします。

\begin{align}
\tilde{\boldsymbol{x}}_{c,1} = \begin{pmatrix}
\boldsymbol{x}_{c,1} \\
1
\end{pmatrix}, ~ 
\tilde{\boldsymbol{x}}_{c,2} = \begin{pmatrix}
\boldsymbol{x}_{c,2} \\
1
\end{pmatrix}.
\end{align}

画像座標の同次座標$\tilde{\boldsymbol{m}}_1, \tilde{\boldsymbol{m}}_２$は次のように表せます。

\begin{align}
 s_1 \tilde{\boldsymbol{m}}_1 = 
\begin{pmatrix}
A_1 & \boldsymbol{0}
\end{pmatrix}
\tilde{\boldsymbol{x}}_{c,1}  
= A_1  \boldsymbol{x}_{c,1}  \\
 s_2 \tilde{\boldsymbol{m}}_2 = 
\begin{pmatrix}
A_2 & \boldsymbol{0}
\end{pmatrix}
\tilde{\boldsymbol{x}}_{c,2}  
= A_2  \boldsymbol{x}_{c,2}
\end{align}

これを$\boldsymbol{x}_{c,1}, \boldsymbol{x}_{c,2}$で解いて、画像上の点の拘束関係を表す式に代入します。

\begin{align}
& (s_2 A_2^{-1} \tilde{\boldsymbol{m}}_2)^{\top} E (s_1 A_1^{-1}  \tilde{\boldsymbol{m}}_1) = 0 \\
& \Rightarrow  \tilde{\boldsymbol{m}}_2^{\top} (A_2^{-1})^\top E A_1^{-1} \tilde{\boldsymbol{m}}_1 = 0 \\
& \Rightarrow  \tilde{\boldsymbol{m}}_2^{\top} F \tilde{\boldsymbol{m}}_1 = 0.
\end{align}

ここで、基礎行列（F行列）、

F = (A_2^{-1})^\top E A_1^{-1} ,

を定義しました。定義から、F行列には外部パラメータと内部パラメータの情報が含まれていることが分かります。E行列同様、F行列は画像座標上のエピポーラ線$\ell_1$上の任意の点とエピポーラ線$\ell_2$上の任意の点が満たすべき拘束関係を表しています。

F行列の使い方ですが、例えば、カメラ1の画像座標上の点を一つ選び、$\tilde{\boldsymbol{m}}_1$とします。すると、$\boldsymbol{\beta}=F \tilde{\boldsymbol{m}}_1$として、エピポーラ線$\ell_2$の方程式$\tilde{\boldsymbol{m}}_2^\top \boldsymbol{\beta}=0$を求めることができます。F行列は一つ目の画像座標の点をもう一つの画像座標の直線に写す写像とみなせます。

エピポールは任意のエピポーラ平面に含まれています。したがって、次の式が成り立ちます。

\begin{align}
& \tilde{\boldsymbol{e}}_2^{\top} F \tilde{\boldsymbol{m}}_1 = 0 ,~ ^\forall \tilde{\boldsymbol{m}}_1 \Rightarrow  F^\top \tilde{\boldsymbol{e}}_2 =0, \\
& \tilde{\boldsymbol{m}}_2^{\top} F \tilde{\boldsymbol{e}}_1 = 0 , ~ ^\forall \tilde{\boldsymbol{m}}_2 \Rightarrow  F \tilde{\boldsymbol{e}}_1 =0.
\end{align}

エピポール$\tilde{\boldsymbol{e}}_1,\tilde{\boldsymbol{e}}_2$はそれぞれ$F^\top F$と$FF^\top$の最小固有値の固有ベクトルとして求めることができます。$F$行列から求めるので、このベクトルは画像座標上のベクトルです。E行列でも同様にエピポールを求めることができ、その場合はカメラ座標上のベクトルになります。

F行列の推定

F行列を推定し、カメラパラメータを求めます。画像座標の同次座標を成分で次のように表します。

\begin{align}
\tilde{\boldsymbol{m}}_1 = 
\begin{pmatrix}
\boldsymbol{m}_1 \\
1
\end{pmatrix}
=
\begin{pmatrix}
u_1 \\
v_1 \\
1
\end{pmatrix}, ~ 
\tilde{\boldsymbol{m}}_2 
= 
\begin{pmatrix}
\boldsymbol{m}_2 \\
1
\end{pmatrix}
=
\begin{pmatrix}
u_2 \\
v_2 \\
1
\end{pmatrix}.
\end{align}

画像1と画像2の対応点が分かれば、方程式を立てることができます。

\begin{align}
& \tilde{\boldsymbol{m}}_2^{\top} F \tilde{\boldsymbol{m}}_1 = 0 \\
& \Leftrightarrow 
\begin{pmatrix}
u_2 & v_2 & 1
\end{pmatrix}
\begin{pmatrix}
f_{11} & f_{12} & f_{13} \\
f_{21} & f_{22} & f_{23} \\
f_{31} & f_{32} & f_{33}
\end{pmatrix}
\begin{pmatrix}
u_1 \\
v_1 \\
1
\end{pmatrix}
=0 \\
& \Leftrightarrow 
\begin{pmatrix}
u_1 u_2 & v_1 u_2 & u_2 & u_1 v_2 & v_1 v_2 & v_2 & u_1 & v_1 & 1 
\end{pmatrix}
\begin{pmatrix}
f_{11} \\
f_{12} \\
f_{13} \\
f_{21} \\
f_{22} \\
f_{23} \\
f_{31} \\
f_{32} \\
f_{33} 
\end{pmatrix}
= 0.
\end{align}

対応点をスタックした行列を$X$とすると、

X\boldsymbol{f} = 0

と書けます。これの解は$X^\top X$の最小固有値の固有ベクトルとして求められます。

ただし、F行列のスケールの不定性および、3次元歪対称行列の行列式が$0$になること、

\det(F) \propto \det([\boldsymbol{t}_{c,1}]_{\times}) = 0,

から、F行列の自由度は7です。対応点7つで解くアルゴリズムを7点アルゴリズム、8点以上を使ってSVDで解くアルゴリズムを8点アルゴリズムと呼びます。8点アルゴリズムには問題が知られており、同次座標のスケール次元の大きさがピクセル次元の値と比べて非常に小さいために数値計算がうまくいきません。そこで、ピクセル次元を正規化し、$(-\sqrt2, \sqrt2)$の範囲に収まるようにして8点アルゴリズムを解くアルゴリズムを正規化8点アルゴリズムと呼びます。

カメラパラメータの推定

カメラの内部パラメータが既知の場合、エピポーラ幾何の章で計算したように、求めたF行列をE行列に変換できます。

E = A_2^{\top}F A_1.

E行列の定義は、次の式でした。

E=R[ \boldsymbol{t}_{c,1}]_\times.

E行列を特異値分解で分解し、カメラパラメータを求める事ができます。Eを特異値分解します。

E=USV^\top.

$3\times 3$の歪対称行列の特異値三つのうち、二つは正で同じ値、一つは$0$になることが示せます。詳細は下記記事に譲ります。

したがって、$S$は次のように書けます。

S = \sigma \begin{pmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 0 \\
\end{pmatrix}
\equiv \sigma \Sigma.

$\boldsymbol{t}_{c,1}$は$\boldsymbol{t}_{c,2}$経由で求められます。これらには次の関係があります。

\begin{align}
\boldsymbol{t}_{c,2} = -R\boldsymbol{t}_{c,1}.
\end{align}

$\boldsymbol{t}_{c,2}$はエピポール$\boldsymbol{e}_2$と平行なので、定数倍の不定性を除いて$EE^\top$の最小固有値の固有ベクトルとして求められます。これは特異値$0$に対応する、$U$に含まれる特異ベクトル$\boldsymbol{u}_3$です。

ここで、カメラ2のカメラ座標のスケールの取り方には任意性があることを利用し、単位長を$\boldsymbol{t}_{c,2}$のノルムにとることにします。したがって、$\boldsymbol{t}_{c,2}=\pm\boldsymbol{u}_3$とできます。このスケールの選択によって、3次元再構成する際のワールド座標のスケールは他の方法で決定する必要が生じることに注意してください。

次に、$R$を求めます。まず、行列$W$を導入します。

\begin{align}
W &= \begin{pmatrix}
0 & -1 & 0 \\
1 & 0 & 0 \\
0 & 0 & 1 \\
\end{pmatrix}
\end{align}

$W$は次の性質を持っています。

\begin{align}
W^{-1} &= W^\top = \begin{pmatrix}
0 & 1 & 0 \\
-1 & 0 & 0 \\
0 & 0 & 1 \\
\end{pmatrix}\\
W^{-1}\Sigma &= \begin{pmatrix}
0 & 1 & 0 \\
-1 & 0 & 0 \\
0 & 0 & 0 \\
\end{pmatrix}
\end{align}

E行列を次のように書き直します。

\begin{align}
E &= USV^\top \\
&= U W V^\top (V^\top)^{-1} W^{-1}\sigma\Sigma V^\top \\
&= U W V^\top \sigma V W^{-1}\Sigma V^\top
\end{align}

これを直交行列と歪対称行列に分けます。$R$を行列式$1$の連結成分、すなわち右手系にとることにします。

R = 
\begin{cases}
U W V^\top & \mathrm{if} ~ \det(UV^\top)=1 \\
- U W V^\top & \mathrm{if} ~ \det(UV^\top)=-1
\end{cases}

以下では、$\det(UV^\top)=1$であったとして話を進めます。$R= U W V^\top$および$S =\sigma V W^{-1}\Sigma V^\top$とすると、それぞれ直交行列と歪対称行列になっています。これで$R$を求められました。ただし、$W$を$W^{-1}$で入れ替えて計算してもこの結論が導けるので、$R=UWV^\top$か$R=U W^{-1} V^\top$を選ぶ不定性が残ります。

以上から結局、$\boldsymbol{t}_{c,2}=\pm\boldsymbol{u}_3$と$R=UW^{\pm1}V^\top$の組み合わせで4通りの解の候補が得られました。実は、F行列と内部パラメータだけでは、これ以上解を絞ることができません。

透視投影行列の計算

一旦解の絞り込みは置いておくことにして、外部パラメータは求まったことにして話を勧めます。

外部パラメータが求まると、透視投影行列を求めることができます。ワールド座標をカメラ1のカメラ座標と一致させることにします。ワールド座標とカメラ座標の一般的な関係式は次のように書けます。

\begin{align}
\boldsymbol{M}_{c,1} &= R_1 \boldsymbol{M}_{w} + \boldsymbol{r}_{c,1}\\
\boldsymbol{M}_{c,2} &= R_2 \boldsymbol{M}_{w} + \boldsymbol{r}_{c,2}.
\end{align}

二つの座標の変換は次のように書けます。

\begin{align}
\boldsymbol{M}_{c,2} &=  R_2R_1^{-1}\boldsymbol{M}_{c,1} - R_2R_1^{-1}\boldsymbol{r}_{c,1} + \boldsymbol{r}_{c,2} \\
&= R \boldsymbol{M}_{c,1} + \boldsymbol{t}_{c,2} \\
R &= R_2R_1^{-1} \\
\boldsymbol{t}_{c,2} &= - R_2R_1^{-1}\boldsymbol{r}_{c,1} + \boldsymbol{r}_{c,2}
\end{align}

ワールド座標をカメラ1のカメラ座標と一致させると次のようになります。

\begin{align}
R_1 &= I \\
\boldsymbol{r}_{c,1} &= \boldsymbol{0}\\
R_2 &= R \\
\boldsymbol{r}_{c,2} &= \boldsymbol{t}_{c,2} = -R\boldsymbol{t}_{c,1} 
\end{align}

透視投影行列は次のようになります。

\begin{align}
P_1 &= A_1 
\begin{pmatrix}
I & \boldsymbol{0}
\end{pmatrix}
\\
P_2 &= A_2 
\begin{pmatrix}
R & \boldsymbol{t}_{c,2}
\end{pmatrix}
\end{align}

三角測量による3次元再構成

透視投影行列が求まると、三角測量によって3次元再構成することができます。ベクトルを成分で表します。

s_1 \tilde{\boldsymbol{m}}_1 = 
\begin{pmatrix}
u_1 \\
v_1 \\
1
\end{pmatrix}, \quad
s_2 \tilde{\boldsymbol{m}}_2 = \begin{pmatrix}
u_2 \\
v_2 \\
1
\end{pmatrix}, \quad
\tilde{\boldsymbol{M}}_w = 
\begin{pmatrix}
x_w \\
y_w \\
z_w \\
1
\end{pmatrix},\\
P_n = 
\begin{pmatrix}
p_{n,11} & p_{n,12} & p_{n,13} & p_{n,14} \\
p_{n,21} & p_{n,22} & p_{n,23} & p_{n,24} \\
p_{n,31} & p_{n,32} & p_{n,33} & p_{n,34}
\end{pmatrix}
~ (n=1,2).

射影の式、

s_1 \tilde{\boldsymbol{m}}_1 = P_1 \tilde{\boldsymbol{M}}_w, \\
s_2 \tilde{\boldsymbol{m}}_2 = P_2 \tilde{\boldsymbol{M}}_w,

に代入して、式を整理すると次の連立方程式を得ます。

\begin{pmatrix}
x_1 p_{1,31} - p_{1,11} & x_1 p_{1,32} - p_{1,12} & x_1 p_{1,33} - p_{1,13} \\
y_1 p_{1,31} - p_{1,21} & y_1 p_{1,32} - p_{1,22} & y_1 p_{1,33} - p_{1,23} \\
x_2 p_{2,31} - p_{2,11} & x_2 p_{2,32} - p_{2,12} & x_2 p_{2,33} - p_{2,13} \\
y_2 p_{2,31} - p_{2,21} & y_2 p_{2,32} - p_{2,22} & y_2 p_{2,33} - p_{2,23}
\end{pmatrix}
\begin{pmatrix}
x_w \\
y_w \\
z_w
\end{pmatrix}
=
\begin{pmatrix}
p_{1,14} - x_1 p_{1,34} \\
p_{1,24} - y_1 p_{1,34} \\
p_{2,14} - x_2 p_{2,34} \\
p_{2,24} - y_2 p_{2,34}
\end{pmatrix}.

解の自由度が3に対して方程式が4つあるため、最小二乗法で解くことができます。

外部パラメータの絞り込み

最後に、4通りの外部パラメータ候補の絞り込み方法を説明します。

三角測量による3次元再構成を$\boldsymbol{t}_{c,2}=\pm\boldsymbol{u}_3$と$R=UW^{\pm1}V^\top$の組み合わせ4通りで行います。4通りのうち、3通りにおいて再構成された点は、二つのカメラのうち少なくとも一つのカメラの後ろ側になってしまい、カメラに映るということに矛盾します。無矛盾となる1通りのみが正しい外部パラメータになります。以上で外部パラメータを求めることができました。

カメラの内部パラメータが未知の場合は、次回解説します。

実装

"""
Copyright 2023 ground0state
Released under the MIT license
"""
from os.path import join

import cv2
import numpy as np
from scipy.linalg import svd


def get_sift_keypoints(img1, img2, n_matches=None):
    sift = cv2.SIFT_create()
    keypoints1, descriptors1 = sift.detectAndCompute(img1, None)
    keypoints2, descriptors2 = sift.detectAndCompute(img2, None)

    bf = cv2.BFMatcher()
    matches = bf.match(descriptors1, descriptors2)
    matches = sorted(matches, key=lambda x: x.distance)[:n_matches]
    return keypoints1, keypoints2, matches


def match_filter(keypoints1, keypoints2, matches):
    match_points1, match_points2 = [], []
    for i_match in matches:
        match_points1.append(keypoints1[i_match.queryIdx].pt)
        match_points2.append(keypoints2[i_match.trainIdx].pt)
    match_points1 = np.array(match_points1).astype(np.float64)
    match_points2 = np.array(match_points2).astype(np.float64)
    return match_points1, match_points2, matches


def normalize_points(homo_points):
    """ 画像座標の1,2次元成分を正規化する. """
    mean = homo_points[:2].mean(axis=1)
    scale = np.sqrt(2) / np.mean(
        np.linalg.norm(
            homo_points[:2] - mean.reshape(2, 1), axis=0))

    mean = np.append(mean, 0)
    T = np.array([[scale, 0, -scale*mean[0]],
                  [0, scale, -scale*mean[1]],
                  [0, 0, 1]])

    homo_points = T @ homo_points
    return homo_points, T


def find_fundamental_matrix(points1, points2, verbose=0):
    """ 正規化8点アルゴリズムでF行列を推定する. """

    assert points1.shape[1] == points2.shape[1] == 2
    assert points1.shape[0] >= 8

    points1 = np.array(
        [(kpt[0], kpt[1], 1) for kpt in points1]).T.astype(np.float64)
    points2 = np.array(
        [(kpt[0], kpt[1], 1) for kpt in points2]).T.astype(np.float64)

    # 正規化
    points1_norm, T1 = normalize_points(points1)
    points2_norm, T2 = normalize_points(points2)

    # エピポーラ拘束式
    A = np.zeros((points1.shape[1], 9), dtype=np.float64)
    for i in range(points1.shape[1]):
        A[i] = [
            points1_norm[0, i]*points2_norm[0, i],
            points1_norm[1, i]*points2_norm[0, i],
            points2_norm[0, i],
            points1_norm[0, i]*points2_norm[1, i],
            points1_norm[1, i]*points2_norm[1, i],
            points2_norm[1, i],
            points1_norm[0, i],
            points1_norm[1, i],
            1.0
        ]

    # 特異値分解で連立方程式を解く
    U, S, Vt = svd(A, lapack_driver="gesvd")
    # S, U, Vt = cv2.SVDecomp(A.T @ A)  # OpenCVを使う場合

    if verbose >= 1:
        print("SVD decomposition eigen values:", S)
    F = Vt[-1].reshape(3, 3)

    # 最小特異値を厳密に0とし、F行列をランク2にする
    U, S, Vt = svd(F, lapack_driver="gesvd")
    # S, U, Vt = cv2.SVDecomp(F)  # OpenCVを使う場合

    if verbose >= 1:
        print("Rank SVD decomposition eigen values:", S)

    S = S.reshape(-1)
    S[2] = 0
    F = U @ np.diag(S) @ Vt

    # 正規化を戻す
    F = T2.T @ F @ T1
    # f_33を1にする
    F = F / F[2, 2]

    return F


def _triangulate_one_point(P1, P2, pt1, pt2):
    x1, y1 = pt1
    x2, y2 = pt2
    X = np.vstack([
        x1*P1[2, :]-P1[0, :],
        y1*P1[2, :]-P1[1, :],
        x2*P2[2, :]-P2[0, :],
        y2*P2[2, :]-P2[1, :],
    ])
    _, _, Vt = svd(X)
    x_w = Vt[-1]
    x_w = x_w / x_w[-1]
    return X_w


def triangulate(P1, P2, points1, points2):
    assert points1.shape == points2.shape
    assert points1.ndim <= 2

    if points1.ndim == 1:
        # 2次元にする
        points1 = points1.reshape(1, -1)
        points2 = points2.reshape(1, -1)

    if points1.shape[1] == 3:
        # 同次座標の場合
        points1 = points1[:, :2]
        points2 = points2[:, :2]

    X_w = []
    for pt1, pt2 in zip(points1, points2):
        x_w = _triangulate_one_point(P1, P2, pt1, pt2)
        X_w.append(x_w)
    X_w = np.vstack(X_w)

    return x_w


def recover_pose(E, pts1, pts2):
    assert E.shape == (3, 3)
    assert len(pts1) == len(pts2)

    # 同次座標にする
    if pts1.shape[1] == 2:
        pts1 = np.column_stack([pts1, np.ones(len(pts1))])
    if pts2.shape[1] == 2:
        pts2 = np.column_stack([pts2, np.ones(len(pts2))])

    # SVDを実行
    U, _, Vt = svd(E)

    # 右手系にする
    if np.linalg.det(np.dot(U, Vt)) < 0:
        Vt = -Vt

    # 回転行列は２つの候補がある
    W = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
    R1 = U @ W @ Vt
    R2 = U @ W.T @ Vt

    # 並進ベクトルはE.Tの固有値0の固有ベクトルとして求められる(up to scale)
    # 正負の不定性で二つの候補がある
    t1 = U[:, 2]
    t2 = -U[:, 2]

    # 4つの姿勢候補がある
    pose_candidates = [
        [R1, t1],
        [R1, t2],
        [R2, t1],
        [R2, t2]
    ]

    # 正しい姿勢を選択する
    # 各姿勢候補について、両カメラの前に再構成された3次元点の数をカウント
    # 一番カウントが多いのが正しい姿勢
    # 自然画像からの3次元最高性は誤差も大きいため、カウントで判断する
    front_count = []
    for _R, _t in pose_candidates:
        count = 0
        for pt1, pt2 in zip(pts1, pts2):
            P1 = np.array([
                [1.0, 0.0, 0.0, 0.0],
                [0.0, 1.0, 0.0, 0.0],
                [0.0, 0.0, 1.0, 0.0],
            ])
            P2 = np.column_stack([_R, _t])
            x_w = triangulate(P1, P2, pt1[:2], pt2[:2])
            x_c_1 = x_w[:3]
            x_c_2 = np.dot(_R, x_c_1) + _t
            if (x_c_1[-1] > 0) and (x_c_2[-1] > 0):
                count += 1
        front_count.append(count)
    R, t = pose_candidates[int(np.argmax(front_count))]
    return R, t


def recover_pose_opencv(E, points1, points2):
    n_points, R, t, mask = cv2.recoverPose(E, points1, points2)
    return R, t


if __name__ == "__main__":
    # Parameters ---------------
    seed = 0
    data_dir = "."
    image1_name = "sample1.jpg"
    image2_name = "sample2.jpg"
    n_matches = 100
    verbose = 0

    # 前回求めた内部パラメータ
    A = np.array([
        [3.46557471e+03, - 5.10312233e+00,  9.54708195e+02],
        [0.00000000e+00,  3.23967055e+03,  9.37249336e+01],
        [0.00000000e+00,  0.00000000e+00,  1.00000000e+00]
    ])
    # --------------------------

    img1 = cv2.imread(
        join(data_dir, image1_name), cv2.IMREAD_COLOR)
    img2 = cv2.imread(
        join(data_dir, image2_name), cv2.IMREAD_COLOR)

    # 画像の対応する点を求める
    img1_gray = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
    img2_gray = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
    keypoints1, keypoints2, matches = get_sift_keypoints(
        img1_gray, img2_gray, n_matches)

    match_img = cv2.drawMatches(
        img1, keypoints1, img2, keypoints2, matches,
        None, flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS)
    cv2.imwrite("match_image.jpg", match_img)

    # F行列を推定する
    match_points1, match_points2, matches = match_filter(
        keypoints1, keypoints2, matches)
    F = find_fundamental_matrix(
        match_points1, match_points2, verbose=verbose)
    F_, mask = cv2.findFundamentalMat(
        match_points1, match_points2, method=cv2.FM_8POINT)
    rmse = np.sqrt(np.mean((F - F_)**2))
    print("OpenCVとの差 RMSE:", rmse)

    # E行列への変換
    E = A.T @ F @ A
    R, t = recover_pose(E, match_points1, match_points2)
    # OpenCVの場合
    E_ = A.T @ F_ @ A
    R_, t_ = recover_pose_opencv(E_, match_points1, match_points2)

    rmse = np.sqrt(np.mean((R - R_)**2))
    print("OpenCVとの差 RMSE:", rmse)

    rmse = np.sqrt(np.mean((t - t_.reshape(-1))**2))
    print("OpenCVとの差 RMSE:", rmse)

Reference

中村恭之, 小枝正直, 上田悦子,『OpenCVによるコンピュータビジョン・機械学習入門』, 講談社サイエンティフィク, 2017.