def process(calibration, pcd, depthfile):
#read PCD and calibration
calibration = utils.parseCalibration(calibration)
points = utils.parsePCD(pcd)
utils.setWidth(int(240 * 0.75))
utils.setHeight(int(180 * 0.75))
#convert to depthmap
width = utils.getWidth()
height = utils.getHeight()
output = np.zeros((width, height, 3))
for p in points:
v = utils.convert3Dto2D(calibration[1], p[0], p[1], p[2])
x = int(width - v[0] - 1)
y = int(height - v[1] - 1)
if x >= 0 and y >= 0 and x < width and y < height:
output[x][y][0] = p[3]
output[x][y][2] = p[2]
#write depthmap
with open('data', 'wb') as file:
file.write(str(width) + 'x' + str(height) + '_0.001_255\n')
for y in range(height):
for x in range(width):
depth = int(output[x][y][2] * 1000)
confidence = int(output[x][y][0] * 255)
file.write(chr(depth / 256))
file.write(chr(depth % 256))
file.write(chr(confidence))
#zip data
with zipfile.ZipFile(depthfile, "w", zipfile.ZIP_DEFLATED) as zip:
zip.write('data', 'data')
zip.close()
def showResult():
fig = plt.figure()
fig.canvas.mpl_connect('button_press_event', onclick)
width = utils.getWidth()
height = utils.getHeight()
output = np.zeros((width, height, 3))
for x in range(width):
for y in range(height):
depth = utils.parseDepth(x, y)
if (depth):
#convert ToF coordinates into RGB coordinates
vec = utils.convert2Dto3D(calibration[1], x, y, depth)
vec[0] += calibration[2][0]
vec[1] += calibration[2][1]
vec[2] += calibration[2][2]
vec = utils.convert3Dto2D(calibration[0], vec[0], vec[1],
vec[2])
#set output pixel
output[x][height - y - 1][0] = utils.parseConfidence(x, y)
if im_array and vec[0] > 0 and vec[1] > 1 and vec[
0] < width and vec[1] < height:
output[x][height - y - 1][1] = im_array[vec[1]][
vec[0]][1] / 255.0 # test matching on RGB data
output[x][height - y - 1][2] = 1.0 - min(
depth / 2.0, 1.0) # depth data scaled to be visible
plt.imshow(output, extent=[0, height, 0, width])
def showEdges():
fig = plt.figure()
fig.canvas.mpl_connect('button_press_event', onclick)
width = utils.getWidth()
height = utils.getHeight()
output = np.zeros((width, height, 3))
for x in range(2, width - 2):
for y in range(2, height - 2):
d = utils.parseDepth(x, y)
mx = utils.parseDepth(x - 1, y)
px = utils.parseDepth(x + 1, y)
my = utils.parseDepth(x, y - 1)
py = utils.parseDepth(x, y + 1)
depth = utils.convert2Dto3DOriented(calibration[1], x, y, d)[1]
mx = utils.convert2Dto3DOriented(calibration[1], x - 1, y, mx)[1]
px = utils.convert2Dto3DOriented(calibration[1], x + 1, y, px)[1]
my = utils.convert2Dto3DOriented(calibration[1], x, y - 1, my)[1]
py = utils.convert2Dto3DOriented(calibration[1], x, y + 1, py)[1]
edge = (abs(depth - mx) + abs(depth - px) + abs(depth - my) +
abs(depth - py))
if d:
output[x][height - y - 1][0] = depth
output[x][height - y - 1][1] = edge * 10
for x in range(1, width - 1):
for y in range(1, height - 1):
output[x][height - y - 1][0] = 0
plt.imshow(output, extent=[0, height, 0, width])
def lenovo_pcd2depth(pcd, calibration):
points = utils.parsePCD(pcd)
width = utils.getWidth()
height = utils.getHeight()
output = np.zeros((width, height, 1))
for p in points:
v = utils.convert3Dto2D(calibration[1], p[0], p[1], p[2])
x = round(width - v[0] - 1)
y = round(v[1])
y = round(height - v[1] - 1)
if x >= 0 and y >= 0 and x < width and y < height:
output[x][y] = p[2]
return output
def process(calibration_fname: str, pcd_fpath: str, output_depth_fpath: str):
# Read pcd and calibration files
calibration = utils.parseCalibration(calibration_fname)
points = utils.parsePCD(pcd_fpath)
utils.setWidth(int(240 * 0.75))
utils.setHeight(int(180 * 0.75))
# Convert to depthmap
width = utils.getWidth()
height = utils.getHeight()
output = np.zeros((width, height, 3))
for p in points:
v = utils.convert3Dto2D(calibration[1], p[0], p[1], p[2])
x = int(width - v[0] - 1)
y = int(height - v[1] - 1)
if x >= 0 and y >= 0 and x < width and y < height:
output[x][y][0] = p[3]
output[x][y][2] = p[2]
# Write depthmap
with open('data', 'wb') as f:
header_str = str(width) + 'x' + str(height) + '_0.001_255\n'
f.write(header_str.encode(ENCODING))
for y in range(height):
for x in range(width):
depth = int(output[x][y][2] * 1000)
confidence = int(output[x][y][0] * 255)
depth_byte = chr(int(depth / 256)).encode(ENCODING)
depth_byte2 = chr(depth % 256).encode(ENCODING)
confidence_byte = chr(confidence).encode(ENCODING)
f.write(depth_byte)
f.write(depth_byte2)
f.write(confidence_byte)
# Zip data
with zipfile.ZipFile(output_depth_fpath, "w", zipfile.ZIP_DEFLATED) as f:
f.write('data', 'data')
f.close()
def process(plt, dir, depth, rgb):
#extract depthmap
with zipfile.ZipFile(dir + '/depth/' + depth, 'r') as zip_ref:
zip_ref.extractall('.')
utils.parseData('data')
#read rgb data
global im_array
if rgb:
width = utils.getWidth()
height = utils.getHeight()
pil_im = Image.open(dir + '/rgb/' + rgb)
pil_im = pil_im.resize((width, height), Image.ANTIALIAS)
im_array = np.asarray(pil_im)
else:
im_array = 0
#parse calibration
global calibration
calibration = utils.parseCalibration(dir + '/camera_calibration.txt')
def onclick(event):
width = utils.getWidth()
height = utils.getHeight()
if event.xdata is not None and event.ydata is not None:
x = int(event.ydata)
y = height - int(event.xdata) - 1
if x > 1 and y > 1 and x < width - 2 and y < height - 2:
depth = utils.parseDepth(x, y)
if depth:
res = utils.convert2Dto3D(calibration[1], x, y, depth)
if res:
diff = [last[0] - res[0], last[1] - res[1], last[2] - res[2]]
dst = np.sqrt(diff[0] * diff[0] + diff[1] * diff[1] + diff[2] * diff[2])
res.append(dst)
print('x=' + str(res[0]) + ', y=' + str(res[1])
+ ', depth=' + str(res[2]) + ', diff=' + str(res[3]))
last[0] = res[0]
last[1] = res[1]
last[2] = res[2]
return
print('no valid data')
def showEdges():
fig = plt.figure()
fig.canvas.mpl_connect('button_press_event', onclick)
#calculate smoothed normalmap
smoothing = 3
maxdepth = 0
width = utils.getWidth()
height = utils.getHeight()
normalmap = np.zeros((width, height, 3))
for x in range(1 + smoothing, width - 1 - smoothing):
for y in range(1 + smoothing, height - 1 - smoothing):
maxdepth = max(maxdepth, utils.parseDepth(x, y))
cmx = utils.convert2Dto3D(
calibration[1], x - 1, y,
utils.parseDepthSmoothed(x - 1, y, smoothing))
cmy = utils.convert2Dto3D(
calibration[1], x, y - 1,
utils.parseDepthSmoothed(x, y - 1, smoothing))
cpx = utils.convert2Dto3D(
calibration[1], x + 1, y,
utils.parseDepthSmoothed(x + 1, y, smoothing))
cpy = utils.convert2Dto3D(
calibration[1], x, y + 1,
utils.parseDepthSmoothed(x, y + 1, smoothing))
n = utils.normalize(
utils.cross(utils.sub(cpx, cmx), utils.sub(cpy, cmy)))
normalmap[x][height - y - 1][0] = min(abs(n[0]), 1)
normalmap[x][height - y - 1][1] = min(abs(n[1]), 1)
normalmap[x][height - y - 1][2] = min(abs(n[2]), 1)
#fade normalmap
#for x in range(1, width):
# for y in range(1, height):
# factor = utils.parseDepth(x, y) / maxdepth
# normalmap[x][height - y - 1][0] *= (1 - factor)
# normalmap[x][height - y - 1][1] *= (1 - factor)
# normalmap[x][height - y - 1][2] *= (1 - factor)
#calculate edgemap
edgemap = np.zeros((width, height, 3))
for x in range(2, width - 2):
for y in range(2, height - 2):
#calculate edge from depthmap
d = utils.convert2Dto3D(calibration[1], x, y,
utils.parseDepth(x, y))
mx = utils.convert2Dto3D(calibration[1], x - 1, y,
utils.parseDepth(x - 1, y))
my = utils.convert2Dto3D(calibration[1], x, y - 1,
utils.parseDepth(x, y - 1))
px = utils.convert2Dto3D(calibration[1], x + 1, y,
utils.parseDepth(x + 1, y))
py = utils.convert2Dto3D(calibration[1], x, y + 1,
utils.parseDepth(x, y + 1))
edge = max(max(utils.length(d, mx), utils.length(d, my)),
max(utils.length(d, px), utils.length(d, py)))
if edge > 0.1: # difference of depth in meters
edgemap[x][height - y - 1][1] = 1
#calculate edge from normalmap
d = normalmap[x][height - y - 1]
mx = normalmap[x - 1][height - y - 1]
my = normalmap[x][height - y - 1 - 1]
px = normalmap[x + 1][height - y - 1]
py = normalmap[x][height - y - 1 + 1]
edge = max(max(utils.dot(d, mx), utils.dot(d, my)),
max(utils.dot(d, px), utils.dot(d, py)))
if edge > 0.9: # normal matching in percentage
edgemap[x][height - y - 1][0] = 1
#calculate output
output = np.zeros((width, height, 3))
for x in range(2, width - 2):
for y in range(2, height - 2):
if edgemap[x][height - y - 1][1] == 1:
output[x][height - y - 1][2] = 1
else:
if edgemap[x][height - y - 1][0] == 0:
if edgemap[x - 1][height - y - 1][0] == 1:
output[x][height - y - 1][1] = 1
if edgemap[x][height - y - 1 - 1][0] == 1:
output[x][height - y - 1][1] = 1
if edgemap[x + 1][height - y - 1][0] == 1:
output[x][height - y - 1][1] = 1
if edgemap[x][height - y - 1 + 1][0] == 1:
output[x][height - y - 1][1] = 1
#plt.imshow(normalmap, extent=[0, height, 0, width])
#plt.imshow(edgemap, extent=[0, height, 0, width])
plt.imshow(output, extent=[0, height, 0, width])