depthmap_to_pcd.py

  q = np.array([
    [1, 0, 0, -width/2],
    [0, 1, 0, -height/2],
    [0, 0, 0, focal_length],
    [0, 0, -1/tx, 0]
    ])
  _points, _colors = calc_point_cloud(image, disp, q)

  points = list(_points)
  colors = list(_colors)
  size = len(points)

  f = open('points.pcd', 'w')

  header = """# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z rgb
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH %(size)d
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS %(size)d
DATA ascii
""" % { "size" : size }

  f.write(header)

  x = 0
  for i in range(size):
    color = 0
    color += colors[i][0] << 16
    color += colors[i][1] << 8
    color += colors[i][2] << 0
    f.write("%f %f %f %f\n" % ( points[i][0], points[i][1], points[i][2], float(color) ))
    x += 1
  f.close

func.py

def remove_invalid(disp_arr, points, colors):
  mask = (
    (disp_arr > disp_arr.min()) &
    np.all(~np.isnan(points), axis=1) &
    np.all(~np.isinf(points), axis=1)
  )
  return points[mask], colors[mask]


def calc_point_cloud(image, disp, q):
  points = cv2.reprojectImageTo3D(disp, q).reshape(-1,3)
  colors = image.reshape(-1,3)
  return remove_invalid(disp.reshape(-1), points, colors)