めも
import numpy as np
import cv2
##
# @brief 3次元座標群切り出し関数
# @details とある視点からとある距離の平面を通る半径1の球の表面を結んだ交点座標群を切り出す
# @param img_w 画像の幅(分解能)
# @param img_h 画像の高さ(分解能)
# @param senser_size センサに見立てた平面の大きさ(0以上の少数)
# @param view_point 視点位置
# @param senser_point 視点から平面までの距離
# @return x,y,zの座標群
def create_3dmap_from_viewpoint(img_w, img_h, senser_size, view_point, senser_point):
senser_w = senser_size
senser_h = senser_size * img_h / img_w
x1 = view_point # 視点の位置
x2 = view_point + senser_point # 撮像面の位置(必ず視点より前)
w = np.linspace(-senser_w, senser_w, img_w, endpoint=False)
h = np.linspace(-senser_h, senser_h, img_h, endpoint=False)
# 配列を対称形にするためのオフセット
w = w + senser_w / img_w
h = h + senser_h / img_h
# センサの座標
ww, hh = np.meshgrid(w, h)
# 直線の式
a1 = ww / (x2 - x1)
a2 = hh / (x2 - x1)
b1 = -a1 * x1
b2 = -a2 * x1
a = 1 + a1**2 + a2**2
b = 2 * (a1 * b1 + a2 * b2)
c = b1**2 + b2**2 - 1
d = (b**2 - 4*a*c) ** (1/2)
# 球面上の3次元座標
x = (-b + d) / (2 * a)
y = a1 * x + b1
z = a2 * x + b2
return x, y, z
##
# @brief 3次元回転座標を取得する関数
# @details 3次元座標群を原点から指定された角度で回転させる
# @param x x座標群
# @param y y座標群
# @param z z座標群
# @param roll roll角(度単位)
# @param pitch pitch角(度単位)
# @param yaw yaw角(度単位)
# @return 回転後の3次元座標
def rotate_3d(x, y, z, roll, pitch, yaw):
# 回転量
roll = roll * np.pi / 180
pitch = pitch * np.pi / 180
yaw = yaw * np.pi / 180
# 3次元の回転行列
mtx1 = np.array([[1, 0, 0],
[0, np.cos(roll), np.sin(roll)],
[0, -np.sin(roll), np.cos(roll)]])
mtx2 = np.array([[np.cos(pitch), 0, -np.sin(pitch)],
[0, 1, 0],
[np.sin(pitch), 0, np.cos(pitch)]])
mtx3 = np.array([[np.cos(yaw), np.sin(yaw), 0],
[-np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]])
# 回転行列の積
mtx4 = np.dot(mtx3, np.dot(mtx2, mtx1))
# 座標の行列計算
xd = mtx4[0][0] * x + mtx4[0][1] * y + mtx4[0][2] * z
yd = mtx4[1][0] * x + mtx4[1][1] * y + mtx4[1][2] * z
zd = mtx4[2][0] * x + mtx4[2][1] * y + mtx4[2][2] * z
return xd, yd, zd
##
# @brief 緯度経度群を取得する関数
# @details 半径1の球上の3次元座標群を緯度経度表記に変換する
# @param x x座標群
# @param y y座標群
# @param z z座標群
# @return 緯度、経度群(ラジアン)
def conv_equirectangularmap(x, y, z):
# 緯度・経度へ変換
phi = np.arcsin(z)
theta = np.arcsin(np.clip(y / np.cos(phi), -1, 1))
theta = np.where((x<0) & (y<0), -np.pi-theta, theta)
theta = np.where((x<0) & (y>0), np.pi-theta, theta)
return phi, theta
##
# @brief 緯度経度情報にしたがって画像を変換する関数
# @details 1:2で格納されている画像から、そのx,y座標を緯度経度と見立てて対応する座標変換を行う
# @param img 元となる360度画像(1:2)
# @param phi 緯度(縦方向 ラジアン)
# @param theta 経度(横方向 ラジアン)
# @return 変換された画像
def remap_equirectangular(img, phi, theta, interpolation=cv2.INTER_CUBIC, borderMode=cv2.BORDER_WRAP):
img_h, img_w = img.shape[:2]
# 画像座標へ正規化(座標を画素位置に戻すため0.5オフセット)
phi = (phi * img_h / np.pi + img_h / 2).astype(np.float32) - 0.5
theta = (theta * img_w / (2 * np.pi) + img_w / 2).astype(np.float32) - 0.5
return cv2.remap(img, theta, phi, interpolation, borderMode = borderMode)
img = cv2.imread("src.jpg")
x, y, z = create_3dmap_from_viewpoint(1920, 1080, 0.75, -1.2, 0.5)
x, y, z = rotate_3d(x, y, z, 45, 45, 45)
phi, theta = conv_equirectangularmap(x,y,z)
out = remap_equirectangular(img, phi, theta)
cv2.imwrite("dst.jpg", out)
# 回転量
roll = 45.0 * np.pi / 180
pitch = 45.0 * np.pi / 180
yaw = 45.0 * np.pi / 180
# 3次元の回転行列
mtx1 = np.array([[1, 0, 0],
[0, np.cos(roll), np.sin(roll)],
[0, -np.sin(roll), np.cos(roll)]])
mtx2 = np.array([[np.cos(pitch), 0, -np.sin(pitch)],
[0, 1, 0],
[np.sin(pitch), 0, np.cos(pitch)]])
mtx3 = np.array([[np.cos(yaw), np.sin(yaw), 0],
[-np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]])
# 回転行列の積
mtx4 = np.dot(mtx3, np.dot(mtx2, mtx1))
# 座標の行列計算
xd = mtx4[0][0] * x + mtx4[0][1] * y + mtx4[0][2] * z
yd = mtx4[1][0] * x + mtx4[1][1] * y + mtx4[1][2] * z
zd = mtx4[2][0] * x + mtx4[2][1] * y + mtx4[2][2] * z
この記事が気に入ったらサポートをしてみませんか?