像素深度转空间坐标（去畸变） opencv python

def pixel2world(depth_in_meters, cx, cy, intrinsic, camera_distCoeffs=None):
    """
    intrinsic:
        Input 3x3 camera intrinsic matrix
    camera_distCoeffs: 
        Input vector of distortion coefficients (k1, k2, P1, P2, K3, k4, k5, k6)
    """
    if camera_distCoeffs is None:
        x = (cx - intrinsic[0,2]) / intrinsic[0,0]
        y = (cy - intrinsic[1,2]) / intrinsic[1,1]
    else:
        udpixel = cv2.undistortPoints(np.array([[cx, cy]], np.float32), intrinsic, camera_distCoeffs)
        x = udpixel.flatten()[0]
        y = udpixel.flatten()[1]
    return np.array([x, y, 1.0]) * depth_in_meters

camera_intrinsic = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
camera_distCoeffs = [k1, k2, p1, p2, k3]

depth = 1.0
point3d = pixel2world(depth, pixel_x, pixel_y, camera_intrinsic, camera_distCoeffs)