示例#1
0
    def __init__(self, shape, n=1, fovea_shape=None, fill_method='smudge'):
        """
        Arguments
        ---------
        shape - Expected shape of disparity images
        n - Number of past frames to retain
        fovea_shape - Shape of foveal region to remember (None=whole image)
        fill_method - default 'smudge' works fine and is fast, other options
            are 'none' or 'interp' (which is way too slow)
        """

        self.shape = shape
        self.fovea_shape = fovea_shape
        self.n = n

        calib = Calib()
        self.disp2imu = calib.get_disp2imu()
        self.imu2disp = calib.get_imu2disp()

        self.past_position = []
        self.past_disparity = []

        self.transforms = []

        assert fill_method in ('smudge', 'interp', 'none')
        self.fill_method = fill_method
示例#2
0
    def __init__(self, shape, n=1, fovea_shape=None, fill_method='smudge'):
        """
        Arguments
        ---------
        shape - Expected shape of disparity images
        n - Number of past frames to retain
        fovea_shape - Shape of foveal region to remember (None=whole image)
        fill_method - default 'smudge' works fine and is fast, other options
            are 'none' or 'interp' (which is way too slow)
        """

        self.shape = shape
        self.fovea_shape = fovea_shape
        self.n = n

        calib = Calib()
        self.disp2imu = calib.get_disp2imu()
        self.imu2disp = calib.get_imu2disp()

        self.past_position = []
        self.past_disparity = []

        self.transforms = []

        assert fill_method in ('smudge', 'interp', 'none')
        self.fill_method = fill_method
示例#3
0
def load_disparity_points(drive, frame, color=False, **kwargs):

    calib = Calib(color=color, **kwargs)

    # read velodyne points
    points = load_velodyne_points(drive, frame, **kwargs)

    # remove all points behind image plane (approximation)
    points = points[points[:, 0] >= 1, :]

    # convert points to each camera
    xyd = calib.velo2disp(points)

    # take only points that fall in the first image
    xyd = calib.filter_disps(xyd)

    return xyd
示例#4
0
def test_disp2rect():

    drive = 11
    frame = 0
    color = True

    img0, img1 = load_stereo_frame(drive, frame, color=color)

    calib = Calib(color=color)  # get calibration

    # get points
    vpts = load_velodyne_points(drive, frame)

    # remove invalid points
    # m = (vpts[:, 0] >= 5)
    # m = (vpts[:, 0] >= 5) & (np.abs(vpts[:, 1]) < 5)
    m = (vpts[:, 0] >= 5) & (vpts[:, 2] >= -3)
    vpts = vpts[m, :]

    rpts = calib.velo2rect(vpts)

    # get disparities
    xyd = calib.rect2disp(rpts)
    xyd, valid_rpts = calib.filter_disps(xyd, return_mask=True)

    if 1:
        # plot disparities
        disp = np.zeros(image_shape, dtype=np.uint8)
        for x, y, d in np.round(xyd):
            disp[y, x] = d

        plt.figure(101)
        plt.clf()
        plt.subplot(211)
        plt.imshow(img0, cmap='gray')
        plt.subplot(212)
        plt.imshow(disp)
        plt.show()

        # assert False

    # convert back to rect
    rpts2 = calib.disp2rect(xyd)

    assert np.allclose(rpts[valid_rpts], rpts2)

    # plotting
    if 0:
        plt.figure(101)
        plt.clf()
        img0, img1 = load_stereo_frame(drive, frame, color=color)
        plt.imshow(img0, cmap='gray')

        from mpl_toolkits.mplot3d import Axes3D
        fig = plt.figure(1)
        fig.clf()
        ax = fig.add_subplot(111, projection='3d')
        ax.plot3D(rpts[:, 0], rpts[:, 1], rpts[:, 2], '.')
        ax.set_xlabel('x')
        ax.set_ylabel('y')
        ax.set_zlabel('z')
        plt.show()
示例#5
0
    print(down_factors)
    print(iters)
    print(errors)
    idx = errors.index(min(errors))
    return down_factors[idx], iters[idx], fovea_shapes[idx]

def get_coarse_subwindow():
    pass

def get_seed():
    pass

if __name__ == '__main__':
    plt.ion()

    calib = Calib()
    disp2imu = calib.get_disp2imu()
    imu2disp = calib.get_imu2disp()

    drive = 51
    video = load_stereo_video(drive)
    positions = load_video_odometry(drive)
    initial = video[0]
    
    fig = plt.figure(1)
    fig.clf()
    ax_disp = plt.gca()
    plot_disp = ax_disp.imshow(mean_disparity(drive, n_disp), vmin=0, vmax=n_disp)

    fovea_ij = 100, 600
    
示例#6
0
def test_disp2rect():

    drive = 11
    frame = 0
    color = True

    img0, img1 = load_stereo_frame(drive, frame, color=color)

    calib = Calib(color=color)  # get calibration

    # get points
    vpts = load_velodyne_points(drive, frame)

    # remove invalid points
    # m = (vpts[:, 0] >= 5)
    # m = (vpts[:, 0] >= 5) & (np.abs(vpts[:, 1]) < 5)
    m = (vpts[:, 0] >= 5) & (vpts[:, 2] >= -3)
    vpts = vpts[m, :]

    rpts = calib.velo2rect(vpts)

    # get disparities
    xyd = calib.rect2disp(rpts)
    xyd, valid_rpts = calib.filter_disps(xyd, return_mask=True)

    if 1:
        # plot disparities
        disp = np.zeros(image_shape, dtype=np.uint8)
        for x, y, d in np.round(xyd):
            disp[y, x] = d

        plt.figure(101)
        plt.clf()
        plt.subplot(211)
        plt.imshow(img0, cmap='gray')
        plt.subplot(212)
        plt.imshow(disp)
        plt.show()

        # assert False

    # convert back to rect
    rpts2 = calib.disp2rect(xyd)

    assert np.allclose(rpts[valid_rpts], rpts2)

    # plotting
    if 0:
        plt.figure(101)
        plt.clf()
        img0, img1 = load_stereo_frame(drive, frame, color=color)
        plt.imshow(img0, cmap='gray')

        from mpl_toolkits.mplot3d import Axes3D
        fig = plt.figure(1)
        fig.clf()
        ax = fig.add_subplot(111, projection='3d')
        ax.plot3D(rpts[:, 0], rpts[:, 1], rpts[:, 2], '.')
        ax.set_xlabel('x')
        ax.set_ylabel('y')
        ax.set_zlabel('z')
        plt.show()