Converts a depth image into a 3D pointcloud, using the camera intrinsics.

depth2cloud.py

def dep_2_cld(dpt, K, scale=1000):
	"""
	Converts a depth image into a pointcloud, using the camera intrinsics.

	Arguments
	---------
	dpt : single-channel image
		depth image
	K : array (3x3)
		intrinsics matrix
	"""

	xmap = np.array([[j for i in range(640)] for j in range(480)])
	ymap = np.array([[i for i in range(640)] for j in range(480)])

	if len(dpt.shape) > 2:
		dpt = dpt[:, :, 0]
	msk_dp = dpt > 1e-6

	choose = msk_dp.flatten().nonzero()[0].astype(np.uint32)

	dpt_mskd = dpt.flatten()[choose][:, np.newaxis].astype(np.float32)
	xmap_mskd = xmap.flatten()[choose][:, np.newaxis].astype(np.float32)
	ymap_mskd = ymap.flatten()[choose][:, np.newaxis].astype(np.float32)

	pt2 = dpt_mskd / scale
	cam_cx, cam_cy = K[0][2], K[1][2]
	cam_fx, cam_fy = K[0][0], K[1][1]
	pt0 = (ymap_mskd - cam_cx) * pt2 / cam_fx
	pt1 = (xmap_mskd - cam_cy) * pt2 / cam_fy
	cld = np.concatenate((pt0, pt1, pt2), axis=1)

	return cld