深度图到点云

def generate_XYZ(depthBuffer,resolution,u,v):
    n_p,f_p = 0.01,3.50
    xAngle_half = 57*math.pi/360
    yAngle_half = math.atan(math.tan(xAngle_half)*resolution[1]/resolution[0])
    z = n_p + (f_p-n_p)*depthBuffer[(resolution[1]-1-v)*resolution[0]+u]
    x = z*math.tan(xAngle_half)*(resolution[0]-2*u)/resolution[0]
    y = z*math.tan(yAngle_half)*(resolution[1]-2*v)/resolution[1]
    return x,y,z
 
 
def coverXYZ(n):
    imag = cv2.imread(date_path + str(n) + '.png', 0)
    resolution = imag.shape
    print(resolution)
    imag = cv2.flip(imag, 0)
    depth_buffer = imag.flatten().astype(float32)/255
    flag = []
    for u in range(resolution[0]):
        for v in range(resolution[1]):
            x,y,z = generate_XYZ(depth_buffer,resolution,u,v)
            flag.append([x,y,z])
    return flag