カメラ射影行列計算(未完成です）

1.座標の取得
最初に、世界座標 world_cor [Wx, Wy, Wz, 1] とカメラ座標 ca_cor [x, y, z] を取得します。これらの座標を元に射影行列の計算を行います。

import numpy as np

# 座標の定義（例）
world_cor = np.array([Wx, Wy, Wz, 1])
ca_cor = np.array([x, y, z])

2.行列 P の定義
次に、X軸とY軸方向で計算した行列 P を定義します。これは次のような形式になります。

# P行列の定義 (少なくとも6つの対応する点、12x12の行列を取得）
P = np.array([
    [Wx, Wy, Wz, 1, 0, 0, 0, 0, -Wx * x, -Wy * x, -Wz * x, -x],
    [0, 0, 0, 0, Wx, Wy, Wz, 1, -Wx * y, -Wy * y, -Wz * y, -y]
    ----------------------------------------------------------
    ---------------------------------------------------------
])

3.特異値分解
行列 P を特異値分解します。これによって以下の3つの行列が得られます。

# 特異値分解
U, Z, Vt = np.linalg.svd(P)

# Vt の最後の列が最小特異値
V = Vt.T
v_last_column = V[:, -1]  # 最小特異値のベクトル

• U

• Z

• Vt

ここで Vt を転置して V を得ます。この V の最後の列が最小特異値であり、1次元ベクトルです。

4.射影ベクトルの計算
射影ベクトルは (3x4) の行列でなければならないため、最小特異値を (3x4) の形に変換します。この行列を pM と呼びます。

# 射影行列 pM の生成
pM = v_last_column.reshape(3, 4)

5.回転行列 A と平行移動ベクトル b の分離
射影行列 pM から、回転行列 A (3x3) と平行移動ベクトル b (3x1) を分離します。

# 回転行列 A (3x3) と平行移動ベクトル b (3x1) の分離
A = pM[:, :3]
b = pM[:, 3]

6.スケールファクタ p の計算
回転行列 A のZ軸方向のノルムを計算して、スケールファクタ p を求めます。

# Z軸方向のノルムを計算してスケールファクタ p を求める
Z_axis_norm = np.linalg.norm(A[:, 2])
p = Z_axis_norm

7.主点座標 cx, cy の計算
回転行列 A のX軸方向とZ軸方向、Y軸方向とZ軸方向の内積から、主点座標 cx と cy を計算します。

# 主点座標 cx, cy の計算
cx = np.dot(A[:, 0], A[:, 2])
cy = np.dot(A[:, 1], A[:, 2])

8.法線ベクトルとシータの計算
X軸方向とZ軸方向、Y軸方向とZ軸方向の外積から法線ベクトル u と v を計算し、これらのベクトルのコサイン角を内積で計算することで、シータ θ を算出します。計算は以下のようになります。

# 法線ベクトル u, v の計算（外積）
u = np.cross(A[:, 0], A[:, 2])
v = np.cross(A[:, 1], A[:, 2])

# シータ θ の計算（内積とコサイン角）
cos_theta = np.dot(u, v) / (np.linalg.norm(u) * np.linalg.norm(v))
theta = np.arccos(cos_theta)

9.αとβの計算
スケールファクタ p とノルム、シータの値を使って、次のように α と β を計算します。

# α と β の計算
alpha = p**2 * np.linalg.norm(u) * np.sin(theta)
beta = p**2 * np.linalg.norm(v) * np.sin(theta)

10.カメラ内部パラメータ行列 K の計算
上記の値を用いて、カメラ内部パラメータ行列 K を次のように定義します。

# カメラ内部パラメータ行列 K の定義
K = np.array([
    [alpha, -alpha / np.tan(theta), cx],
    [0, beta / np.sin(theta), cy],
    [0, 0, 1]
])

11.射影行列 M の計算
射影行列 M は次のように定義されます。

• K はカメラの内部パラメータ

• R は回転行列

• t は平行移動ベクトル

12.回転行列 R の計算
カメラの3D空間での向きを示す回転行列 R は、以下のように計算されます。

回転行列 R は Rx, Ry, Rz から次のように定義されます。

# 回転行列 R の計算
Rx = v / np.linalg.norm(v)
Rz = p * A[:, 2]
Ry = np.cross(Rz, Rx)

R = np.column_stack((Rx, Ry, Rz))

13.平行移動ベクトル T の計算
行列 P の最後の列を b と定義し、平行移動ベクトル T を次のように計算します。

# 平行移動ベクトル T の計算
T = p * np.dot(np.linalg.inv(K), b)

14.最終的な射影行列 M
最終的に、射影行列 M は以下のようにまとめられます。

# 射影行列 M の計算
M = np.dot(K, np.column_stack((R, T)))

# 結果の出力
print("射影行列 M:")
print(M)