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
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)
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)
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
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