基礎行列から透視投影行列を求める

## 概要

前回の続きです。

https://qiita.com/ground0state/items/70ec4a6ffe337b800af3

カメラの内部パラメータが未知の場合、F行列の自由度が7に対して、内部パラメータの自由度が5、回転と並進のパラメータが6あるので、F行列から全てのパラメータを求めることはできません。ただし、カメラ1のカメラ座標をワールド座標に一致させることで、ワールド座標からカメラ座標への射影行列を求めることができます。

## F行列の再導出

透視投影行列を次のように書きます。

```math
\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$が正則であることを仮定しす。射影は次のようになります。

```math
\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$を消去します。

```math
\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$をかけます。

```math
\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$をかけます。

```math
\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行列は次のように表せます。

```math
\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)$として、次のように書きます。

```math
\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$になるように定数倍し、次の式で定義します。

```math
\begin{align}
F &:= [\tilde{\boldsymbol{e}}_2]_\times X_2 X_1^{-1}.
\end{align}
```

スケールを固定したことの意味を確認します。$\boldsymbol{k}$の定義式に$[\tilde{\boldsymbol{e}}\_2]_\times$を左からかけて、次の式を得ます。

```math
\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$を特異値分解します。

```math
\begin{align}
F = USV^\top.
\end{align}
```

$\tilde{\boldsymbol{e}}_2$は最小特異値0に対応する左特異ベクトル$\boldsymbol{u}_3$に比例します。$\tilde{\boldsymbol{e}}_2$は同次座標なので、$\boldsymbol{u}_3$をその第3成分で割ることで求めることができます。

```math
\begin{align}
\tilde{\boldsymbol{e}}_2 = \frac{\boldsymbol{u}_3}{u_{3}^{(3)}}.
\end{align}
```

$\boldsymbol{y}_2$は次のように表せます。

```math
\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$の公式、

```math
\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$をかけます。

```math
\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$を次のように書くことにします。

```math
\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$が存在するためです。

```math
\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$を考えます。

```math
\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}
```

これにより、

```math
\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$を考えます。

```math
\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}
```
これを作用させます。

```math
\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$を考えます。

```math
\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}
```

これを作用させます。

```math
\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次元再構成できます。

## 実装

```python
"""
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.