Beispiel #1
0
def test_linearT(Hw2c_set, points_2d, points_3d, points_indices,
                 camera_indices, camera_params, gt_points_3d):
    n_points = points_3d.shape[0]
    n_cameras = camera_params.shape[0]
    # Obtain first frame
    print('\n---------------Testing linearT()------------------\n')

    # collect point clouds
    pkl_folder = 'pkl'
    if not os.path.exists(pkl_folder):
        os.makedirs(pkl_folder)
    if not os.path.exists(os.path.join(pkl_folder, 'PC_set.pkl')):
        PC_set = {}
        gt_3D_set = {}
        for p in range(points_3d.shape[0]):
            p3d = points_3d[p, :]
            gt_p3d = gt_points_3d[p]
            gt_3D_set[p] = gt_p3d
            print('- Point {0}/ {1}'.format(p, points_3d.shape[0]))
            label = points_indices[p]
            PC = PointCloud(label=label, ini_3D=p3d)
            # Find 2d (u,v)
            add2 = 0
            for i in range(points_indices.shape[0]):
                if points_indices[i] == label:
                    PC.newCorres(points_2d[i], camera_indices[i], Hw2c_set[i],
                                 camera_params[camera_indices[i], :])
                    # add2 += 1
            PC_set[p] = PC

        PC_data = {}
        PC_data['pred'] = PC_set
        PC_data['gt'] = gt_3D_set
        with open(os.path.join(pkl_folder, 'PC_set.pkl'), 'wb') as f:
            pkl.dump(PC_data, f, pkl.HIGHEST_PROTOCOL)
    else:
        with open(os.path.join(pkl_folder, 'PC_set.pkl'), 'rb') as f:
            PC_data = pkl.load(f)
        PC_set = PC_data['pred']
        gt_3D_set = PC_data['gt']

    # Test linear
    Errors_orig = []
    Errors_pred = []
    total_ignore = 0
    for i in PC_set.keys():
        cur_PC = PC_set[i]
        gt_p3d = gt_3D_set[i]
        if len(cur_PC.Pos2D) > 1:
            orgerror = compare_with_groundtruth(PC_set[i].Pos3D, gt_p3d)
            # Errors_orig.append(orgerror)
            # print('\n *    Point',cur_PC.Pos3D)
            # print('* Has {0} frames'.format(len(cur_PC.Pos2D)))
            cur_PC, opt_inliers = RansacLinearT(cur_PC, pu=0, pv=0)

            if cur_PC == None:
                print('-------- Ignore! ------------')
                total_ignore += 1
                PC_set.pop(i)
                continue
            PC_set[i] = cur_PC
            # perror = compare_with_groundtruth(cur_PC.Pos3D, gt_p3d)
            perror = linearT_errEst(cur_PC=cur_PC,
                                    pu=0,
                                    pv=0,
                                    inliners=opt_inliers)
            print('>> ', i, '/', len(PC_set.keys()),
                  '                                                Error:',
                  orgerror, ' vs ', perror)
            Errors_pred.append(perror)

            # time.sleep(0.5)

    print('Total errors:')
    print('Original:', np.sum(np.array(Errors_orig)) / len(Errors_pred))
    print('Pred:', np.sum(np.array(Errors_pred)) / len(Errors_pred))
    print('Total ignore PC:', total_ignore)
    # save the obtained PC_set to the file
    points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
        PC_set, n_cameras=n_cameras, n_3dpoints=n_points)
    DATA = {}
    DATA['params'] = camera_params
    DATA['cam_indices'] = camera_indices
    DATA['points_indices'] = points_indices
    DATA['2d'] = points_2d
    DATA['3d'] = points_3d
    with open('linearTed_online_data.pkl', 'wb') as f:
        pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL)

    plt.figure()
    plt.scatter(range(len(Errors_pred)), Errors_pred)
    plt.show()
    return Errors_orig, Errors_pred
Beispiel #2
0
def NonLT_main(data_file='linearTed_our_data_PC_set.pkl',
               dump_file_name='linearTed_to_nonlinearT_our_data.pkl',
               PC_set=None):
    # camera_params with shape (n_cameras, 9) contains initial estimates of parameters for all cameras. First 3 components in each row form a rotation vector (https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula), next 3 components form a translation vector, then a focal distance and two distortion parameters.
    # points_3d with shape (n_points, 3) contains initial estimates of point coordinates in the world frame.
    # camera_ind with shape (n_observations,) contains indices of cameras (from 0 to n_cameras - 1) involved in each observation.
    # point_ind with shape (n_observations,) contatins indices of points (from 0 to n_points - 1) involved in each observation.
    # points_2d with shape (n_observations, 2) contains measured 2-D coordinates of points projected on images in each observations.
    # open file to retrieve data
    if PC_set == None:
        with open(data_file, 'rb') as f:
            PC_set = pkl.load(f)
    PC_set = util.clean_PC_set(PC_set)
    print('Doing NonlinearT')
    points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
        PC_set)

    n_cameras = camera_params.shape[0]
    n_points = points_3d.shape[0]

    n = 9 * n_cameras + 3 * n_points
    m = 2 * points_2d.shape[0]

    print("n_cameras: {}".format(n_cameras))
    print("n_points: {}".format(n_points))
    print("Total number of parameters: {}".format(n))
    print("Total number of residuals: {}".format(m))

    cprams = camera_params[0]
    _, pu, pv = util.initial_PC()
    K = np.array([[cprams[6], 0, pu], \
                  [0, cprams[6], pv], \
                  [0, 0, 1]])
    x0 = points_3d.ravel()
    f0 = fun(x0, camera_params, n_cameras, n_points, camera_indices,
             points_indices, points_2d, K)

    # plt.plot(f0)

    A = nonlinear_triangulation_sparsity(n_points, camera_indices,
                                         points_indices)

    t0 = time.time()
    res = least_squares(fun, x0, jac_sparsity = A, verbose = 2, x_scale = 'jac', ftol = 1e-4, method = 'trf', \
                        args = (camera_params, n_cameras, n_points, camera_indices, points_indices, points_2d,K))
    t1 = time.time()

    print('---Final params:', res.x[0:10])

    print("Optimization took {0:.0f} seconds".format(t1 - t0))
    plt.scatter(range(len(res.fun)), res.fun)
    plt.show()

    # Extract results from res.x
    points_3d = np.reshape(res.x, (n_points, 3))

    # Save data
    # optimized data
    DATA = {}
    DATA['params'] = camera_params
    DATA['cam_indices'] = camera_indices
    DATA['points_indices'] = points_indices
    DATA['2d'] = points_2d
    DATA['3d'] = points_3d
    with open(dump_file_name, 'wb') as f:
        pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL)
Beispiel #3
0
def bundle(data_file='linearTed_our_data_PC_set.pkl',
           input_type='PC_set',
           dump_file_name='linearTed_to_bundle_online_data.pkl',
           PC_set=None):
    # camera_params with shape (n_cameras, 9) contains initial estimates of parameters for all cameras. First 3 components in each row form a rotation vector (https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula), next 3 components form a translation vector, then a focal distance and two distortion parameters.
    # points_3d with shape (n_points, 3) contains initial estimates of point coordinates in the world frame.
    # camera_ind with shape (n_observations,) contains indices of cameras (from 0 to n_cameras - 1) involved in each observation.
    # point_ind with shape (n_observations,) contatins indices of points (from 0 to n_points - 1) involved in each observation.
    # points_2d with shape (n_observations, 2) contains measured 2-D coordinates of points projected on images in each observations.
    # open file to retrieve data
    if PC_set == None and input_type == 'PC_set':
        with open(data_file, 'rb') as f:
            PC_set = pkl.load(f)
    elif PC_set is not None:
        points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
            PC_set)

    else:
        with open(data_file, 'rb') as f:
            data = pkl.load(f)
        camera_params = data['params']
        camera_indices = data['cam_indices']
        points_indices = data['points_indices']
        points_2d = data['2d']  #
        points_3d = data['3d']

    n_cameras = camera_params.shape[0]
    n_points = points_3d.shape[0]

    n = 9 * n_cameras + 3 * n_points
    m = 2 * points_2d.shape[0]

    print("n_cameras: {}".format(n_cameras))
    print("n_points: {}".format(n_points))
    print("Total number of parameters: {}".format(n))
    print("Total number of residuals: {}".format(m))

    cprams = camera_params[0]
    _, pu, pv = util.initial_PC()
    K = np.array([[cprams[6], 0, pu], \
                  [0, cprams[6], pv], \
                  [0, 0, 1]])

    x0 = np.hstack((camera_params.ravel(), points_3d.ravel()))

    f0 = fun(x0, n_cameras, n_points, camera_indices, points_indices,
             points_2d, K)

    f0 = np.reshape(f0, (points_indices.shape[0], 2))

    plt.subplot(1, 2, 1)
    plt.plot(f0)
    print('Done ploting')

    A = bundle_adjustment_sparsity(n_cameras, n_points, camera_indices,
                                   points_indices, PC_set)

    t0 = time.clock()
    res = least_squares(fun, x0, jac_sparsity = A, verbose = 2, x_scale = 'jac',\
            ftol = 1e-4, method = 'trf', max_nfev=100,\
            args = (n_cameras, n_points, camera_indices, points_indices, points_2d, K))
    t1 = time.clock()

    print('---Final params:', res.x[0:10])
    final_rot, _ = cv2.Rodrigues(res.x[0:3])

    # print('---Init rot:\n',init_rot)
    print('---Final rot:\n', final_rot)

    print("Optimization took {0:.0f} seconds".format(t1 - t0))
    plt.subplot(1, 2, 2)
    plt.plot(res.fun)
    plt.show()

    # Extract results from res.x
    camera_params = np.reshape(res.x[0:n_cameras * 9], (n_cameras, 9))
    points_3d = np.reshape(res.x[n_cameras * 9:], (n_points, 3))

    # Save data
    # optimized data
    DATA = {}
    DATA['params'] = camera_params
    DATA['cam_indices'] = camera_indices
    DATA['points_indices'] = points_indices
    DATA['2d'] = points_2d
    DATA['3d'] = points_3d
    with open(dump_file_name, 'wb') as f:
        pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL)
Beispiel #4
0
def apply_linearT(etol=100000):
    '''etol: error tolaration for each point cloud. If the reprojection error is higher than this value, just don't
    do triangulation.'''

    # Intialize PC set
    PC_set, pu, pv = util.initial_PC()
    print(pu, pv)
    # Apply linearT
    Errors_pred = []
    total_ignore = 0
    for i in PC_set.keys():
        cur_PC = PC_set[i]
        if len(cur_PC.Pos2D) <= 1:
            print(
                '-------- Ignore because there is only 1 correspondence! ------------'
            )
            total_ignore += 1
            continue
        if len(cur_PC.Pos2D) >= 2:
            # Use ransacLinearT
            # TODO: using window truncatation of 10
            cur_PC, opt_inliers = RansacLinearT(cur_PC=cur_PC,
                                                window_frame=10,
                                                pu=pu,
                                                pv=pv)
            # If return None, ignore the point
            if cur_PC == None:
                print('-------- Ignore due to return None! ------------')
                total_ignore += 1
                continue

            # perror = compare_with_groundtruth(cur_PC.Pos3D, gt_p3d)
            perror = linearT_errEst(cur_PC=cur_PC,
                                    pu=pu,
                                    pv=pv,
                                    inliners=opt_inliers)
            print('>> ', i, '/', len(PC_set.keys()),
                  '                                        Error:', perror)
            PC_set[i] = cur_PC

            if perror > etol:
                cur_PC.Pos3D = None
                print('-------- Ignore! ------------')
                total_ignore += 1
                continue

            # Check why error is large
            # if perror > 100:
            #     print('Why it is larger than 100?')
            #     time.sleep(2)
            Errors_pred.append(perror)

    print('Total errors Pred:',
          np.sum(np.array(Errors_pred)) / len(Errors_pred))
    print('Total ignore PC:', total_ignore)

    plt.figure()
    plt.scatter(range(len(Errors_pred)), Errors_pred)
    plt.show()

    # save the obtained PC_set to the file
    points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
        PC_set)

    DATA = {}
    DATA['params'] = camera_params
    DATA['cam_indices'] = camera_indices
    DATA['points_indices'] = points_indices
    DATA['2d'] = points_2d
    DATA['3d'] = points_3d
    with open('pkl/linearTed.pkl', 'wb') as f:
        pkl.dump(DATA, f, pkl.HIGHEST_PROTOCOL)

    # save PC_set:
    with open('pkl/linearTed_PC_set.pkl', 'wb') as f:
        pkl.dump(PC_set, f, pkl.HIGHEST_PROTOCOL)

    return Errors_pred
Beispiel #5
0
def test_bundle_sparse_matrix():
    PC_set = {}
    PC1 = PointCloud(label=0,
                     ini_3D=None,
                     ini_2D=[0, 0],
                     ini_frame=1,
                     ini_Hw2c=None,
                     cprams=None,
                     inliners=None)

    PC2 = PointCloud(label=1,
                     ini_3D=[0, 0, 0],
                     ini_2D=[10, 10],
                     ini_frame=0,
                     ini_Hw2c=None,
                     cprams=None,
                     inliners=1)
    PC3 = PointCloud(label=2,
                     ini_3D=[10, 10, 10],
                     ini_2D=[20, 20],
                     ini_frame=0,
                     ini_Hw2c=None,
                     cprams=None,
                     inliners=1)
    PC4 = PointCloud(label=3,
                     ini_3D=[20, 20, 20],
                     ini_2D=[30, 30],
                     ini_frame=1,
                     ini_Hw2c=None,
                     cprams=None,
                     inliners=1)
    PC5 = PointCloud(label=4,
                     ini_3D=[30, 30, 30],
                     ini_2D=[40, 40],
                     ini_frame=1,
                     ini_Hw2c=None,
                     cprams=None,
                     inliners=0)

    PC6 = PointCloud(label=5,
                     ini_3D=None,
                     ini_2D=None,
                     ini_frame=None,
                     ini_Hw2c=None,
                     cprams=None,
                     inliners=None)

    PC7 = PointCloud(label=6,
                     ini_3D=None,
                     ini_2D=None,
                     ini_frame=None,
                     ini_Hw2c=None,
                     cprams=None,
                     inliners=None)

    # add new correspondences
    PC2.newCorres(p2d=[100, 100],
                  cam_ind=1,
                  Hwc2=None,
                  cprams=None,
                  inliners=0)
    # add new correspondences
    PC2.newCorres(p2d=[1000, 1000],
                  cam_ind=2,
                  Hwc2=None,
                  cprams=None,
                  inliners=1)

    PC_set[1] = PC1
    PC_set[2] = PC2
    PC_set[3] = PC3
    PC_set[4] = PC4
    PC_set[5] = PC5
    PC_set[6] = PC6
    PC_set[7] = PC7
    points_3d, points_2d, points_indices, camera_indices, camera_params = util.convertPC(
        PC_set)
    print('Set of points_2d:')
    print(points_2d)
    n_cameras = camera_params.shape[0]
    n_points = points_3d.shape[0]
    A = bundle_adjustment_sparsity(n_cameras, n_points, camera_indices,
                                   points_indices, PC_set)
    print A