def rotateWithYaxis(crdArr, degree, newOrigin):
ret = [[] for _ in range(height)]
theta = degreeToRadian(degree)
c = math.cos(theta)
s = math.sin(theta)
Ry = [[c, 0.0, s, 0.0], [0.0, 1.0, 0.0, 0.0], [-s, 0.0, c, 0.0],
[0.0, 0.0, 0.0, 1.0]]
for i in range(height):
for j in range(width):
cx = crdArr[i][j][0] - newOrigin[0]
cy = crdArr[i][j][1] - newOrigin[1]
cz = crdArr[i][j][2] - newOrigin[2]
A = [cx, cy, cz, 1.0]
B = []
for k in range(4):
tmp = 0
for l in range(4):
tmp += Ry[k][l] * A[l]
B.append(tmp)
nx = B[0] + newOrigin[0]
ny = B[1] + newOrigin[1]
nz = B[2] + newOrigin[2]
ret[i].append([nx, ny, nz])
return ret
def get_rotation_matrix_x_axis(degree):
theta = degreeToRadian(degree)
c = math.cos(theta)
s = math.sin(theta)
return np.array([[1.0, 0.0, 0.0, 0.0], [0.0, c, -s, 0.0], [0.0, s, c, 0.0],
[0.0, 0.0, 0.0, 1.0]])
def coordinatesTodepth(crdArr, fov_variant, fsz):
hFOV_prime = degreeToRadian(
56.559 + fov_variant) # TODO: Confirm that the number is for hFOV
vFOV_prime = math.atan(height / width * math.tan(hFOV_prime / 2)) * 2
tan_hFOV_half_prime = math.tan(hFOV_prime / 2)
tan_vFOV_half_prime = math.tan(vFOV_prime / 2)
ret = plt.zeros((height, width))
for point in crdArr:
assert point[2] > 0
cx = point[0]
cy = point[1]
cd = point[2]
k_x = tan_hFOV_half_prime * cd / (width / 2)
cc = (width / 2 * k_x - cx) / k_x
k_y = tan_vFOV_half_prime * cd / (height / 2)
cr = (height / 2 * k_y - cy) / k_y
ccs = [math.floor(cc), math.floor(cc), math.ceil(cc), math.ceil(cc)]
crs = [math.floor(cr), math.floor(cr), math.ceil(cr), math.ceil(cr)]
for k in range(len(ccs)):
tc = ccs[k]
tr = crs[k]
if tr < 0 or tc < 0 or tr >= height or tc >= width:
continue
# z값이 음수인 경우에 대해서는 어떻게 처리할지 미정
if ret[tr][tc] == 0:
ret[tr][tc] = cd
else:
ret[tr][tc] = min(ret[tr][tc], cd)
for i in range(height):
for j in range(width):
if ret[i][j] == 0:
ltR = max(i - fsz // 2, 0) # left top row of filter
ltC = max(j - fsz // 2, 0) # left top column of filter
rbR = min(i + fsz // 2, height - 1)
rbC = min(j + fsz // 2, width - 1)
# pixelCnt = (rbR-ltR+1)*(rbC-ltC+1)
cands = []
for tr in range(ltR, rbR + 1):
for tc in range(ltC, rbC + 1):
if (tr == i and tc == j):
continue
cands.append(ret[tr][tc])
ret[i][j] = cands[len(cands) // 2] # median
return ret
def coordinates_to_depth(crdArr, vt, fsz, theta):
ret = plt.zeros((height, width))
print(crdArr)
t = time.time()
for index in range(len(crdArr)):
cx = crdArr[index][0]
cy = crdArr[index][1]
cz = crdArr[index][2]
if(cz==0):
continue
hFOV_prime = degreeToRadian(56.559 + theta)
vFOV_prime = math.atan(height / width * math.tan(hFOV_prime / 2)) * 2
tan_hFOV_half_prime = math.tan(hFOV_prime / 2)
tan_vFOV_half_prime = math.tan(vFOV_prime / 2)
k_x = tan_hFOV_half_prime*cz / (width//2)
k_y = tan_vFOV_half_prime*cz / (height//2)
cc = ((width//2)*k_x - cx)/k_x
cr = ((height//2)*k_y - cy)/k_y
cd = math.sqrt(cx*cx+cy*cy+cz*cz)
ccs = [math.floor(cc), math.floor(cc), math.ceil(cc), math.ceil(cc)]
crs = [math.floor(cr), math.ceil(cr), math.floor(cr), math.ceil(cr)]
for k in range(len(ccs)):
tc = ccs[k]
tr = crs[k]
if tr < 0 or tc < 0 or tr >= height or tc >= width:
continue
# z값이 음수인 경우에 대해서는 어떻게 처리할지 미정
if (ret[tr][tc] == 0):
ret[tr][tc] = cd
elif (ret[tr][tc]>0):
ret[tr][tc] = min(ret[tr][tc], cd)
from skimage.transform import resize
ret = resize(ret, (224, 224))
t0 = time.time()
print(t0 - t)
from scipy import ndimage
ret = ndimage.median_filter(ret, fsz)
from sklearn.preprocessing import minmax_scale
ret = minmax_scale(ret.ravel(), feature_range=(0, 1)).reshape(ret.shape)
return ret
def rotateWithXaxis(crdArr, degree, newOrigin):
theta = degreeToRadian(degree)
c = math.cos(theta)
s = math.sin(theta)
Rx = np.array([[1.0, 0.0, 0.0, 0.0], [0.0, c, -s, 0.0], [0.0, s, c, 0.0],
[0.0, 0.0, 0.0, 1.0]])
T = np.array([[1.0, 0.0, 0.0, newOrigin[0]], [0.0, 1.0, 0.0, newOrigin[1]],
[0.0, 0.0, 1.0, newOrigin[2]], [0.0, 0.0, 0.0, 1.0]])
invT = np.array([[1.0, 0.0, 0.0, -newOrigin[0]],
[0.0, 1.0, 0.0, -newOrigin[1]],
[0.0, 0.0, 1.0, -newOrigin[2]], [0.0, 0.0, 0.0, 1.0]])
return np.transpose(
np.dot(T, np.dot(Rx, np.dot(invT, np.transpose(crdArr)))))
import math
import sys
from utils import degreeToRadian, stringToFloat
from numpy import genfromtxt
# dirNames = ['0', '1', '7', '8', '9', '10', '12', '13', '14', '16']
# dirNames = ['virtualRotate']
dirNames = ['0_test']
# workspacePath = "C:\\Users\\three\\Desktop\\workspace\\EIS_lab_intern\\work1\\my_workspace\\rotated_sample\\test\\"
# workspacePath = '/Users/jafffy/workspace/depthmaps/rotation/sample/'
workspacePath = '/Users/jafffy/workspace/depthmaps/rotation/rotate v3/test/realRotate/'
height = 480
width = 640
hFOV = degreeToRadian(56.559) # TODO: Confirm that the number is for hFOV
vFOV = math.atan(height / width * math.tan(hFOV / 2)) * 2
tan_hFOV_half = math.tan(hFOV / 2)
tan_vFOV_half = math.tan(vFOV / 2)
DEPTH_THRSHOLD = 100
# print ("hFOV: " + str(hFOV) + ", vFOV: " + str(vFOV))
# convert 2D depth array to 2D coordinate(x, y, z) array
def depthToCoordinates(arr):
ret = []
for i in range(height):
for j in range(width):
#-*- encoding:utf-8 -*-
import os
import numpy as np
import matplotlib.pylab as plt
import math
import sys
import time
from utils import degreeToRadian, stringToFloat
height = 480
width = 640
hFOV = degreeToRadian(56.559)
vFOV = math.atan(height / width * math.tan(hFOV / 2)) * 2
tan_hFOV_half = math.tan(hFOV / 2)
tan_vFOV_half = math.tan(vFOV / 2)
DEPTH_THRSHOLD = 100
# convert 2D depth array to 2D coordinate(x, y, z) array
def depth_to_coordinates(arr):
ret = []
for i in range(height):
for j in range(width):
cd = arr[i][j]
k_x = tan_hFOV_half * cd / (width // 2)
cx = (width/2 - j)*k_x