File size: 1,932 Bytes
874cec4 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 | import cv2
import numpy as np
def xyz2lonlat(xyz):
atan2 = np.arctan2
asin = np.arcsin
norm = np.linalg.norm(xyz, axis=-1, keepdims=True)
xyz_norm = xyz / norm
x = xyz_norm[..., 0:1]
y = xyz_norm[..., 1:2]
z = xyz_norm[..., 2:]
lon = atan2(x, z)
lat = asin(y)
lst = [lon, lat]
out = np.concatenate(lst, axis=-1)
return out
def lonlat2XY(lonlat, shape):
X = (lonlat[..., 0:1] / (2 * np.pi) + 0.5) * (shape[1] - 1)
Y = (lonlat[..., 1:] / (np.pi) + 0.5) * (shape[0] - 1)
lst = [X, Y]
out = np.concatenate(lst, axis=-1)
return out
def GetPerspective(image, intrinsic_four, yaw, pitch, height, width):
#
# for yaw, higher is looking right, lower is looking left
# for pitch, higher is looking down, lower is looking up
#
if not type(image) == list:
image = [image]
THETA = yaw
PHI = -pitch
fx,fy,cx,cy = intrinsic_four
K = np.array([
[fx, 0, cx],
[0, fy, cy],
[0, 0, 1],
], np.float32)
K_inv = np.linalg.inv(K)
x = np.arange(width)
y = np.arange(height)
x, y = np.meshgrid(x, y)
z = np.ones_like(x)
xyz = np.concatenate([x[..., None], y[..., None], z[..., None]], axis=-1)
xyz = xyz @ K_inv.T
y_axis = np.array([0.0, 1.0, 0.0], np.float32)
x_axis = np.array([1.0, 0.0, 0.0], np.float32)
R1, _ = cv2.Rodrigues(y_axis * np.radians(THETA))
R2, _ = cv2.Rodrigues(np.dot(R1, x_axis) * np.radians(PHI))
R = R2 @ R1
xyz = xyz @ R.T
lonlat = xyz2lonlat(xyz)
XY = lonlat2XY(lonlat, shape=image[0].shape).astype(np.float32)
result = []
for img in image:
persp = cv2.remap(img, XY[..., 0], XY[..., 1], cv2.INTER_CUBIC, borderMode=cv2.BORDER_WRAP)
persp = np.clip(persp, 0, 255).astype(np.uint8)
result.append(persp)
if len(result) == 1:
return result[0]
return result
|