def getHeightMap(depthImage, missingMask, cameraMatrix):
height, width = depthImage.shape
pc, N, yDir, h, R = processDepthImage(depthImage, missingMask,
cameraMatrix)
X = pc[:, :, 0]
Y = h
Z = pc[:, :, 2]
#X = X - np.min(X) + 1
# Z = Z - np.min(Z) + 1
roundX = X.astype(int)
roundZ = Z.astype(int)
maxX = np.max(roundX)
maxZ = np.max(roundZ)
minX = np.min(roundX)
minZ = np.min(roundZ)
x_range = maxX - minX + 1
z_range = maxZ - minZ + 1
print(maxX)
print(maxZ)
print(minX)
print(minZ)
mat_boundx = max(x_range, maxX + 1)
mat_boundz = max(z_range, maxZ + 1)
mx, mz = np.meshgrid(np.array(range(mat_boundx)),
np.array(range(mat_boundz)))
heightMap = np.ones([mat_boundz, mat_boundx]) * np.inf
for i in range(height):
for j in range(width):
tx = roundX[i, j]
tz = roundZ[i, j]
heightMap[tz, tx] = min(h[i, j], heightMap[tz, tx])
heightMap[np.where(heightMap == np.inf)] = 0
# heightMap = np.fliplr(heightMap)
# colorMap = cv2.cvtColor(heightMap, cv2.COLOR_GRAY2BGR)
# cv2.cvtcolor(heightMap, colorMap, COLOR_GRAY2BGR)
imageio.imwrite('height2.png', heightMap)
fig = plt.figure()
ax = fig.gca(projection='3d')
surf = ax.plot_surface(mx,
mz,
heightMap,
cmap=cm.coolwarm,
linewidth=0,
antialiased=False)
ax.set_xlabel('width')
ax.set_ylabel('height')
ax.set_zlabel('height above ground')
# Add a color bar which maps values to colors.
fig.colorbar(surf, shrink=0.5, aspect=5)
plt.show()
return heightMap
def getHeightMap(self, missingMask, cameraMatrix, generateHAA):
height, width = self.depthImage.shape
pc, N, yDir, h, R = processDepthImage(self.depthImage, missingMask,
cameraMatrix)
pcRot = rotatePC(pc, R)
if (generateHAA):
self.getHHAImage(pc, N, yDir, h)
# where each pixel will be located in 3d world
roundX = pcRot[:, :, 0].astype(int)
roundZ = pcRot[:, :, 2].astype(int)
# [minX, maxX, minZ, maxZ]
self.imgbounds = [
np.min(roundX),
np.max(roundX),
np.min(roundZ),
np.max(roundZ)
]
x_range = self.imgbounds[1] - self.imgbounds[0] + 1
z_range = self.imgbounds[3] - self.imgbounds[2] + 1
mat_boundx = max(x_range, self.imgbounds[1] + 1)
mat_boundz = max(z_range, self.imgbounds[3] + 1)
self.heightMap = np.ones([mat_boundz, mat_boundx],
dtype="float") * np.inf
self.height2Img = dict.fromkeys(range(len(self.heightMap.flatten())),
[])
self.img2Height = np.zeros(
(self.depthImage.shape[0], self.depthImage.shape[1], 2), dtype=int)
for i in range(height):
for j in range(width):
tx = roundX[i, j] - self.imgbounds[0]
tz = roundZ[i, j]
self.img2Height[i, j] = [mat_boundz - tz, tx]
# boudz-z cause we will flipup heightMap later
idx_height = (mat_boundz - tz) * mat_boundx + tx
if (self.height2Img[idx_height]):
self.height2Img[idx_height].append(i * width + j)
else:
self.height2Img[idx_height] = [i * width + j]
if h[i, j] < self.heightMap[tz, tx]:
self.heightMap[tz, tx] = h[i, j]
self.heightMap[np.where(self.heightMap == np.inf)] = 0
# plot3dHeightMap(mat_boundx,mat_boundz,self.heightMap)
self.heightMap = np.flipud(self.heightMap)
self.heightMatBounds = [mat_boundz, mat_boundx]
def getHHAImg(depthImage, missingMask, cameraMatrix):
pc, N, yDir, h, R = processDepthImage(depthImage * 100, missingMask,
cameraMatrix)
tmp = np.multiply(N, yDir)
# with np.errstate(invalid='ignore'):
acosValue = np.minimum(1, np.maximum(-1, np.sum(tmp, axis=2)))
angle = np.array([math.degrees(math.acos(x)) for x in acosValue.flatten()])
angle = np.reshape(angle, h.shape)
pc[:, :, 2] = np.maximum(pc[:, :, 2], 100)
I = np.zeros(pc.shape)
I[:, :, 0] = 31000 / pc[:, :, 2]
I[:, :, 1] = h
I[:, :, 2] = (angle + 128 - 90)
HHA = I.astype(np.uint8)
return HHA
def getHeightMap(depthImage, missingMask, cameraMatrix):
height, width = depthImage.shape
pc, N, yDir, h, R = processDepthImage(depthImage, missingMask,
cameraMatrix)
X = pc[:, :, 0]
Y = h
Z = pc[:, :, 2]
X = X - np.min(X) + 1
Z = Z - np.min(Z) + 1
roundX = X.astype(int)
roundZ = Z.astype(int)
maxX = np.max(roundX)
maxZ = np.max(roundZ)
mx, mz = np.meshgrid(np.array(range(maxX + 1)), np.array(range(maxZ + 1)))
heightMap = np.ones([maxZ + 1, maxX + 1]) * np.inf
for i in range(height):
for j in range(width):
tx = roundX[i, j]
tz = roundZ[i, j]
heightMap[tz, tx] = min(h[i, j], heightMap[tz, tx])
fig = plt.figure()
ax = fig.gca(projection='3d')
surf = ax.plot_surface(mx,
mz,
heightMap,
cmap=cm.coolwarm,
linewidth=0,
antialiased=False)
ax.set_xlabel('width')
ax.set_ylabel('height')
ax.set_zlabel('height above ground')
# Add a color bar which maps values to colors.
fig.colorbar(surf, shrink=0.5, aspect=5)
plt.show()