概要
前回の続きです。
カメラの内部パラメータが未知の場合、F行列の自由度が7に対して、内部パラメータの自由度が5、回転と並進のパラメータが6あるので、F行列から全てのパラメータを求めることはできません。ただし、カメラ1のカメラ座標をワールド座標に一致させることで、ワールド座標からカメラ座標への射影行列を求めることができます。
F行列の再導出
透視投影行列を次のように書きます。
\begin{align}
P_1 &:= A_1
\begin{pmatrix}
R_1 & \boldsymbol{r}_{c,1}
\end{pmatrix}
=:
\begin{pmatrix}
X_1 & \boldsymbol{y}_{1}
\end{pmatrix},
\\
P_2 &:= A_2
\begin{pmatrix}
R_2 & \boldsymbol{r}_{c,2}
\end{pmatrix}
=:
\begin{pmatrix}
X_2 & \boldsymbol{y}_{2}
\end{pmatrix}.
\end{align}
$X_1$が正則であることを仮定しす。射影は次のようになります。
\begin{align}
s_1 \tilde{\boldsymbol{m}}_1 &= P_1 \tilde{\boldsymbol{M}}_w = X_1 \boldsymbol{M}_w + \boldsymbol{y}_{1}, \\
s_2 \tilde{\boldsymbol{m}}_2 &= P_2 \tilde{\boldsymbol{M}}_w = X_2 \boldsymbol{M}_w + \boldsymbol{y}_{2}.
\end{align}
$\boldsymbol{M}_w$を消去します。
\begin{align}
s_2 \tilde{\boldsymbol{m}}_2 &= s_1 X_2 X_1^{-1} \tilde{\boldsymbol{m}}_1 - X_2 X_1^{-1} \boldsymbol{y}_{1} + \boldsymbol{y}_{2} \\
&= s_1 X_2 X_1^{-1} \tilde{\boldsymbol{m}}_1 + \boldsymbol{k}, \\
\boldsymbol{k} &:= - X_2 X_1^{-1} \boldsymbol{y}_{1} + \boldsymbol{y}_{2}.
\end{align}
両辺に$[\boldsymbol{k}]_\times$をかけます。
\begin{align}
s_2 [\boldsymbol{k}]_\times \tilde{\boldsymbol{m}}_2
= s_1 [\boldsymbol{k}]_\times X_2 X_1^{-1} \tilde{\boldsymbol{m}}_1.
\end{align}
さらに、両辺に$\tilde{\boldsymbol{m}}_2^\top$をかけます。
\begin{align}
0 = s_2 \tilde{\boldsymbol{m}}_2^\top[\boldsymbol{k}]_\times \tilde{\boldsymbol{m}}_2
= s_1 \tilde{\boldsymbol{m}}_2^\top[\boldsymbol{k}]_\times X_2 X_1^{-1} \tilde{\boldsymbol{m}}_1.
\end{align}
F行列は次のように表せます。
\begin{align}
F \propto [\boldsymbol{k}]_\times X_2 X_1^{-1}.
\end{align}
これがF行列の別の形式による導出です。F行列を定義する際に、定数倍の任意性があります。
カメラ2の画像座標のエピポール$\tilde{\boldsymbol{e}}_2$に対し、$\tilde{\boldsymbol{e}}_2^\top F=0$が成り立つことと、外積の性質から、$\boldsymbol{k}$は$\tilde{\boldsymbol{e}}_2$に平行でなければなりません。したがって、$\boldsymbol{k}$は$\tilde{\boldsymbol{e}}_2$の定数倍です。比例定数を$\alpha(\neq 0)$として、次のように書きます。
\begin{align}
\boldsymbol{k} &= \alpha \tilde{\boldsymbol{e}}_2, \\
F &\propto [\tilde{\boldsymbol{e}}_2]_\times X_2 X_1^{-1}.
\end{align}
ここで、$F$は$f_{33}=1$になるように定数倍し、次の式で定義します。
\begin{align}
F &:= [\tilde{\boldsymbol{e}}_2]_\times X_2 X_1^{-1}.
\end{align}
スケールを固定したことの意味を確認します。$\boldsymbol{k}$の定義式に$[\tilde{\boldsymbol{e}}_2]_\times$を左からかけて、次の式を得ます。
\begin{align}
0&=[\tilde{\boldsymbol{e}}_2]_\times\boldsymbol{k} = - [\tilde{\boldsymbol{e}}_2]_\times X_2 X_1^{-1} \boldsymbol{y}_{1} + [\tilde{\boldsymbol{e}}_2]_\times \boldsymbol{y}_{2}
= - F\boldsymbol{y}_{1} + [\tilde{\boldsymbol{e}}_2]_\times \boldsymbol{y}_{2}.
\end{align}
$\tilde{\boldsymbol{e}}_2$は同次座標のため値が定まっています。したがって、$F$のスケールを変更することは、カメラ座標の相対的なスケールを決める自由度を固定したことになっています。
カメラパラメータの推定
さて、$F$が与えられたとき、$P_1, P_2$を求めることができるか考えます。まず、$F$を特異値分解します。
\begin{align}
F = USV^\top.
\end{align}
$\tilde{\boldsymbol{e}}_2$は最小特異値0に対応する左特異ベクトル$\boldsymbol{u}_3$に比例します。$\tilde{\boldsymbol{e}}_2$は同次座標なので、$\boldsymbol{u}_3$をその第3成分で割ることで求めることができます。
\begin{align}
\tilde{\boldsymbol{e}}_2 = \frac{\boldsymbol{u}_3}{u_{3}^{(3)}}.
\end{align}
$\boldsymbol{y}_2$は次のように表せます。
\begin{align}
\boldsymbol{y}_2 &= \boldsymbol{k} + X_2 X_1^{-1} \boldsymbol{y}_{1}
= \alpha \tilde{\boldsymbol{e}}_2 + X_2 X_1^{-1} \boldsymbol{y}_{1}.
\end{align}
次に$X_2$を求めます。$^\forall \boldsymbol{a}$に対する歪対称行列$[\boldsymbol{a}]_\times$の公式、
\begin{align}
[\boldsymbol{a}]_\times[\boldsymbol{a}]_\times
&=\boldsymbol{a} \boldsymbol{a}^\top - \boldsymbol{a}^\top \boldsymbol{a} I, \\
[\boldsymbol{a}]_\times[\boldsymbol{a}]_\times[\boldsymbol{a}]_\times
&=- \boldsymbol{a}^\top \boldsymbol{a} [\boldsymbol{a}]_\times,
\end{align}
を使用します。$F$に左から$[\tilde{\boldsymbol{e}}_2]_\times[\tilde{\boldsymbol{e}}_2]_\times$をかけ、右から$X_1$をかけます。
\begin{align}
[\tilde{\boldsymbol{e}}_2]_\times [\tilde{\boldsymbol{e}}_2]_\times F X_1
&= [\tilde{\boldsymbol{e}}_2]_\times X_2 .
\end{align}
この式から$X_2$を次のように書くことにします。
\begin{align}
X_2 =[\tilde{\boldsymbol{e}}_2]_\times F X_1 + \tilde{\boldsymbol{e}}_2 \boldsymbol{b}^\top.
\end{align}
$\boldsymbol{b}$は任意の3次元ベクトルです。実は、画像2枚からの外部パラメータ推定にはambiguityがあり、$\boldsymbol{b}$の不定性があります。これは、ワールド座標の点を画像座標に射影する際、この射影を不変に保つような座標変換$H$が存在するためです。
\begin{align}
P' &= PH^{-1}, ~ \tilde{\boldsymbol{M}}' = H\tilde{\boldsymbol{M}} ,\\
\Rightarrow P'\tilde{\boldsymbol{M}}'& = PH^{-1}H\tilde{\boldsymbol{M}} = P\tilde{\boldsymbol{M}}.
\end{align}
$H$は$4\times4$の任意の正則行列です。同次座標に作用しているため、これは射影変換になっており、自由度はスケール分を除いた15です。
逆に、この射影変換を利用して、計算に都合の良い座標を選ぶことにします。まず次の変換$H_a$を考えます。
\begin{align}
H_a =
\begin{pmatrix}
X_1 & \boldsymbol{y}_1 \\
\boldsymbol{0}^\top & 1
\end{pmatrix}
, ~
H_a^{-1} =
\begin{pmatrix}
X_1^{-1} & -X_1^{-1}\boldsymbol{y}_1 \\
\boldsymbol{0}^\top & 1
\end{pmatrix}.
\end{align}
これにより、
\begin{align}
P_1 H_a^{-1}&
=
\begin{pmatrix}
X_1 & \boldsymbol{y}_{1}
\end{pmatrix}
\begin{pmatrix}
X_1^{-1} & -X_1^{-1}\boldsymbol{y}_1 \\
\boldsymbol{0} & 1
\end{pmatrix}
\\&=
\begin{pmatrix}
I & \boldsymbol{0}
\end{pmatrix}
,\\
P_2 H_a^{-1}&
=
\begin{pmatrix}
X_2 & \boldsymbol{y}_{2}
\end{pmatrix}
\begin{pmatrix}
X_1^{-1} & -X_1^{-1}\boldsymbol{y}_1 \\
\boldsymbol{0} & 1
\end{pmatrix}
\\&=
\begin{pmatrix}
X_2X_1^{-1} & -X_2X_1^{-1}\boldsymbol{y}_{1} + \boldsymbol{y}_{2}
\end{pmatrix}
\\&=
\begin{pmatrix}
[\tilde{\boldsymbol{e}}_2]_\times F + \tilde{\boldsymbol{e}}_2 \boldsymbol{b}^\top X_1^{-1} & \boldsymbol{k}
\end{pmatrix}
\\&=
\begin{pmatrix}
[\tilde{\boldsymbol{e}}_2]_\times F + \tilde{\boldsymbol{e}}_2 \boldsymbol{c}^\top & \alpha \tilde{\boldsymbol{e}}_2
\end{pmatrix}.
\end{align}
ここで、$\boldsymbol{c}^\top=\boldsymbol{b}^\top X_1^{-1}$としました。
次の変換$H_b$を考えます。
\begin{align}
H_b =
\begin{pmatrix}
I & \boldsymbol{0} \\
\alpha^{-1}\boldsymbol{c}^\top & 1
\end{pmatrix}
, ~
H_b^{-1} =
\begin{pmatrix}
I & \boldsymbol{0} \\
-\alpha^{-1}\boldsymbol{c}^\top & 1
\end{pmatrix}.
\end{align}
これを作用させます。
\begin{align}
P_1 H_a^{-1}H_b^{-1}&
=
\begin{pmatrix}
I & \boldsymbol{0}
\end{pmatrix}
\begin{pmatrix}
I & \boldsymbol{0} \\
-\alpha^{-1}\boldsymbol{c}^\top & 1
\end{pmatrix}
\\&=
\begin{pmatrix}
I & \boldsymbol{0}
\end{pmatrix},\\
P_2 H_a^{-1}H_b^{-1}&
=
\begin{pmatrix}
[\tilde{\boldsymbol{e}}_2]_\times F + \tilde{\boldsymbol{e}}_2 \boldsymbol{c}^\top & \alpha \tilde{\boldsymbol{e}}_2
\end{pmatrix}
\begin{pmatrix}
I & \boldsymbol{0} \\
-\alpha^{-1}\boldsymbol{c}^\top & 1
\end{pmatrix}
\\&=
\begin{pmatrix}
[\tilde{\boldsymbol{e}}_2]_\times F & \alpha \tilde{\boldsymbol{e}}_2
\end{pmatrix}.
\end{align}
さらに次の変換$H_c$を考えます。
\begin{align}
H_c =
\begin{pmatrix}
I & \boldsymbol{0} \\
\boldsymbol{0}^\top & \alpha
\end{pmatrix}
, ~
H_c^{-1} =
\begin{pmatrix}
I & \boldsymbol{0} \\
\boldsymbol{0}^\top & \alpha^{-1}
\end{pmatrix}.
\end{align}
これを作用させます。
\begin{align}
P_1':=P_1 H_a^{-1}H_b^{-1}H_c^{-1}&
=
\begin{pmatrix}
I & \boldsymbol{0}
\end{pmatrix}
\begin{pmatrix}
I & \boldsymbol{0} \\
\boldsymbol{0}^\top & \alpha^{-1}
\end{pmatrix}
\\&=
\begin{pmatrix}
I & \boldsymbol{0}
\end{pmatrix},\\
P_2':=P_2 H_a^{-1}H_b^{-1}H_c^{-1}&
=
\begin{pmatrix}
[\tilde{\boldsymbol{e}}_2]_\times F & \alpha \tilde{\boldsymbol{e}}_2
\end{pmatrix}
\begin{pmatrix}
I & \boldsymbol{0} \\
\boldsymbol{0}^\top & \alpha^{-1}
\end{pmatrix}
\\&=
\begin{pmatrix}
[\tilde{\boldsymbol{e}}_2]_\times F & \tilde{\boldsymbol{e}}_2
\end{pmatrix}.
\end{align}
これがLuong and Vi´evilleが提案した、正準カメラにおけるgood choiceです。
以上で、透視投影行列$P_1',P_2'$を求めることができました。あとは前回の三角測量と同じ手順で3次元再構成できます。
実装
"""
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 vec2outer(a):
assert a.ndim == 1
assert a.shape[0] == 3
return np.array([
[0, -a[2], a[1]],
[a[2], 0, -a[0]],
[-a[1], a[0], 0]
])
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 estimate_projection(F):
U, _, _ = svd(F)
e = U[:, -1] / U[-1, -1]
S = vec2outer(e)
P1 = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0]
])
P2 = np.column_stack([S @ F, e])
return P1, P2
if __name__ == "__main__":
# Parameters ---------------
seed = 0
data_dir = "."
image1_name = "sample1.jpg"
image2_name = "sample2.jpg"
n_matches = 100
verbose = 0
# --------------------------
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)
P1, P2 = estimate_projection(F)
print(P1)
print(P2)
Reference
中村恭之, 小枝正直, 上田悦子,『OpenCVによるコンピュータビジョン・機械学習入門』, 講談社サイエンティフィク, 2017.
徐剛, 辻三郎, 『3次元ビジョン』, 共立出版, 1998.
Richard Hartley, Andrew Zisserman, "Multiple View Geometry in Computer Vision", Cambridge University Press; second edition, 2004.