def pcd_to_sphproj(pcd_path, nr_scans, width, outdir=None):
    pcd = o3d.io.read_point_cloud(pcd_path, format='pcd')
    pcd_arr = np.asarray(pcd.points)
    if (len(pcd_arr) == 0):
        return None
    R = LA.norm(pcd_arr[:, :3], axis=1)
    print('R {} | {} -- {}'.format(R.shape, np.amin(R), np.amax(R)))
    yaw = np.arctan2(pcd_arr[:, 1], pcd_arr[:, 0])
    pitch = np.arcsin(np.divide(pcd_arr[:, 2], R))
    FOV_Down = np.amin(pitch)
    FOV_Up = np.amax(pitch)
    FOV = (FOV_Up + abs(FOV_Down))
    u = np.around(((nr_scans - 1) * (1.0 - ((pitch - FOV_Down) / FOV)))).astype(np.int16)
    v = np.around(((width - 1) * (0.5 * ((yaw / math.pi) + 1)))).astype(np.int16)
    sph_proj = np.zeros((nr_scans, width))
    R[(R > 100.0)] = 100.0
    R = np.round(((R / 100.0) * 255.0))
    sph_proj[(u, v)] = R
    outpath = join((Path(pcd_path).parent if (outdir is None) else outdir), (splitext(basename(pcd_path))[0] + '.jpg'))
    cv2.imwrite(outpath, sph_proj)
    print(outpath)
    return (np.amin(R), np.amax(R))
def pcd_to_sphproj(pcd_path, nr_scans, width, outdir=None):
    pcd = o3d.io.read_point_cloud(pcd_path, format='pcd')
    pcd_arr = np.asarray(pcd.points)
    if (len(pcd_arr) == 0):
        return None
    R = LA.norm(pcd_arr[:, :3], axis=1)
    print('R {} | {} -- {}'.format(R.shape, np.amin(R), np.amax(R)))
    yaw = np.arctan2(pcd_arr[:, 1], pcd_arr[:, 0])
    pitch = np.arcsin(np.divide(pcd_arr[:, 2], R))
    FOV_Down = np.amin(pitch)
    FOV_Up = np.amax(pitch)
    FOV = (FOV_Up + abs(FOV_Down))
    u = np.around(((nr_scans - 1) * (1.0 - ((pitch - FOV_Down) / FOV)))).astype(np.int16)
    v = np.around(((width - 1) * (0.5 * ((yaw / math.pi) + 1)))).astype(np.int16)
    sph_proj = np.zeros((nr_scans, width))
    R[(R > 100.0)] = 100.0
    R = np.round(((R / 100.0) * 255.0))
    sph_proj[(u, v)] = R
    outpath = join((Path(pcd_path).parent if (outdir is None) else outdir), (splitext(basename(pcd_path))[0] + '.jpg'))
    cv2.imwrite(outpath, sph_proj)
    print(outpath)
    return (np.amin(R), np.amax(R))