def _cv2_estimate_E_with_intrinsics(cfg, matches, kps1, kps2, calib1, calib2): '''Estimate the Essential matrix from correspondences. Assumes known intrinsics. ''' # Reference: https://docs.opencv.org/3.4.7/d9/d0c/group__calib3d.html # Defalt values for confidence: 0.99 (F), 0.999 (E) # Default value for the reprojection threshold: 3 cur_key = 'config_{}_{}'.format(cfg.dataset, cfg.task) geom = cfg.method_dict[cur_key]['geom'] if geom['method'].lower() == 'cv2-ransac-e': cv_method = 'FM_RANSAC' cv_threshold = geom['threshold'] cv_confidence = geom['confidence'] elif geom['method'].lower() == 'cv2-lmeds-e': cv_method = 'FM_LMEDS' cv_threshold = None cv_confidence = geom['confidence'] else: raise ValueError('Unknown method to estimate E') is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, 5) if not is_valid: return _fail() # Normalize keypoints with ground truth intrinsics kp1_n = normalize_keypoints(kp1, calib1['K']) kp2_n = normalize_keypoints(kp2, calib2['K']) cv2.setRNGSeed(cfg.opencv_seed) E, mask_E = cv2.findEssentialMat(kp1_n, kp2_n, method=getattr(cv2, cv_method), threshold=cv_threshold, prob=cv_confidence) mask_E = mask_E.astype(bool).flatten() # OpenCV can return multiple values as 6x3 or 9x3 matrices if E is None: return _fail() elif E.shape[0] != 3: Es = np.split(E, len(E) / 3) # Or a single matrix else: Es = [E] # Find the best E E, num_inlier = None, 0 # mask_E_cheirality_check = None for _E in Es: _num_inlier, _R, _t, _mask = cv2.recoverPose(_E, kp1_n[mask_E], kp2_n[mask_E]) if _num_inlier >= num_inlier: num_inlier = _num_inlier E = _E # This is unused for now # mask_E_cheirality_check = _mask.flatten().astype(bool) indices = matches[:, mask_E.flatten()] return E, indices
def _cmp_estimate_E_without_intrinsics(cfg, matches, kps1, kps2, calib1, calib2, img1_fname=None, img2_fname=None, scales1=None, scales2=None, ori1=None, ori2=None, A1=None, A2=None): '''Estimate the Essential matrix from correspondences. Computes the Fundamental Matrix first and then retrieves the Essential matrix assuming known intrinsics. ''' cur_key = 'config_{}_{}'.format(cfg.dataset, cfg.task) geom = cfg.method_dict[cur_key]['geom'] min_matches = 8 is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, min_matches) if not is_valid: return _fail() if geom['method'] == 'cmp-degensac-f-laf': sc1 = scales1[matches[0]] sc2 = scales2[matches[1]] ang1 = ori1[matches[0]] ang2 = ori2[matches[1]] if A1 is not None: A1 = A1[matches[0]] A2 = A2[matches[1]] else: A1 = None A2 = None laf1 = get_LAF(kp1, sc1, ang1, A1) laf2 = get_LAF(kp2, sc2, ang2, A2) # print (laf1[:2]) # print (laf2[:2]) F, mask_F = pyransac.findFundamentalMatrix( laf1, laf2, geom['threshold'], geom['confidence'], geom['max_iter'], 2.0, error_type=geom['error_type'], symmetric_error_check=True, enable_degeneracy_check=geom['degeneracy_check']) elif geom['method'] == 'cmp-degensac-f': F, mask_F = pyransac.findFundamentalMatrix( kp1, kp2, geom['threshold'], geom['confidence'], geom['max_iter'], 0, error_type=geom['error_type'], symmetric_error_check=True, enable_degeneracy_check=geom['degeneracy_check']) elif geom['method'] == 'cmp-gc-ransac-f': F, mask_F = pygcransac.findFundamentalMatrix(kp1, kp2, geom['threshold'], geom['confidence'], geom['max_iter']) elif geom['method'] == 'cmp-magsac-f': F, mask_F = pymagsac.findFundamentalMatrix(kp1, kp2, geom['threshold'], geom['confidence'], geom['max_iter']) else: raise ValueError('Unknown method: {}'.format(geom['method'])) mask_F = mask_F.astype(bool).flatten() # OpenCV can return multiple values as 6x3 or 9x3 matrices if F is None: return _fail() elif F.shape[0] != 3: Fs = np.split(F, len(F) / 3) else: Fs = [F] if mask_F.sum() < 8: return _fail() # Find the best F K1, K2 = calib1['K'], calib2['K'] kp1n = normalize_keypoints(kp1, K1) kp2n = normalize_keypoints(kp2, K2) E, num_inlier = None, 0 # mask_E_cheirality_check = None for _F in Fs: _E = np.matmul(np.matmul(K2.T, _F), K1) _E = _E.astype(np.float64) _num_inlier, _R, _t, _mask = cv2.recoverPose(_E, kp1n[mask_F], kp2n[mask_F]) if _num_inlier >= num_inlier: num_inlier = _num_inlier E = _E # This is unused for now # mask_E_cheirality_check = _mask.flatten().astype(bool) # Return the initial list of matches (from F) indices = matches[:, mask_F.flatten()] return E, indices
def _cmp_estimate_E_with_intrinsics(cfg, matches, kps1, kps2, calib1, calib2, img1_fname=None, img2_fname=None): '''Estimate the Essential matrix from correspondences. Assumes known intrinsics. ''' # Reference: https://docs.opencv.org/3.4.7/d9/d0c/group__calib3d.html # Defalt values for confidence: 0.99 (F), 0.999 (E) # Default value for the reprojection threshold: 3 # (We set them to -1 when not applicable as OpenCV complains otherwise) is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, 5) if not is_valid: return _fail() # Normalize keypoints with ground truth intrinsics kp1_n = normalize_keypoints(kp1, calib1['K']) kp2_n = normalize_keypoints(kp2, calib2['K']) if img1_fname is not None: s = (cv2.imread(img1_fname)).size h1, w1 = s[0], s[1] s = (cv2.imread(img2_fname)).size h2, w2 = s[0], s[1] else: raise ValueError('Requires image filenames') cv2.setRNGSeed(cfg.opencv_seed) E, mask_E = pygcransac.findEssentialMatrix(kp1, kp2, calib1['K'], calib2['K'], h1, w1, h2, w2, cfg.method_geom['threshold'], cfg.method_geom['confidence'], cfg.method_geom['max_iter']) mask_E = mask_E.astype(bool).flatten() # OpenCV can return multiple values as 6x3 or 9x3 matrices if E is None: return _fail() elif E.shape[0] != 3: Es = np.split(E, len(E) / 3) # Or a single matrix else: Es = [E] # Find the best E E, num_inlier = None, 0 # mask_E_cheirality_check = None for _E in Es: _num_inlier, _R, _t, _mask = cv2.recoverPose(_E, kp1_n[mask_E], kp2_n[mask_E]) if _num_inlier >= num_inlier: num_inlier = _num_inlier E = _E # This is unused for now # mask_E_cheirality_check = _mask.flatten().astype(bool) indices = matches[:, mask_E.flatten()] return E, indices
def main(cfg): '''Visualization of stereo keypoints and matches. Parameters ---------- cfg: Namespace Configurations for running this part of the code. ''' # Files should not be named to prevent (easy) abuse # Instead we use 0, ..., cfg.num_viz_stereo_pairs viz_folder_hq, viz_folder_lq = get_stereo_viz_folder(cfg) print(' -- Visualizations, stereo: "{}/{}"'.format(cfg.dataset, cfg.scene)) t_start = time() # Load deprecated images list deprecated_images_all = load_json(cfg.json_deprecated_images) if cfg.dataset in deprecated_images_all and cfg.scene in deprecated_images_all[ cfg.dataset]: deprecated_images = deprecated_images_all[cfg.dataset][cfg.scene] else: deprecated_images = [] # Load keypoints, matches and errors keypoints_dict = load_h5_valid_image(get_kp_file(cfg), deprecated_images) matches_dict = load_h5_valid_image(get_match_file(cfg), deprecated_images) ransac_inl_dict = load_h5_valid_image(get_geom_inl_file(cfg), deprecated_images) # Hacky: We need to recompute the errors, loading only for the keys data_dir = get_data_path(cfg) pairs_all = get_pairs_per_threshold(data_dir)['0.1'] pairs = [] for pair in pairs_all: if all([key not in deprecated_images for key in pair.split('-')]): pairs += [pair] # Create results folder if it does not exist if not os.path.exists(viz_folder_hq): os.makedirs(viz_folder_hq) if not os.path.exists(viz_folder_lq): os.makedirs(viz_folder_lq) # Sort alphabetically and pick different images sorted_keys = sorted(pairs) picked = [] pairs = [] for pair in sorted_keys: fn1, fn2 = pair.split('-') if fn1 not in picked and fn2 not in picked: picked += [fn1, fn2] pairs += [pair] if len(pairs) == cfg.num_viz_stereo_pairs: break # Load depth maps depth = {} if cfg.dataset != 'googleurban': for pair in pairs: files = pair.split('-') for f in files: if f not in depth: depth[f] = load_depth( os.path.join(data_dir, 'depth_maps', '{}.h5'.format(f))) # Generate and save the images for i, pair in enumerate(pairs): # load metadata fn1, fn2 = pair.split('-') calib_dict = load_calib([ os.path.join(data_dir, 'calibration', 'calibration_{}.h5'.format(fn1)), os.path.join(data_dir, 'calibration', 'calibration_{}.h5'.format(fn2)) ]) calc1 = calib_dict[fn1] calc2 = calib_dict[fn2] inl = ransac_inl_dict[pair] # Get depth for keypoints kp1 = keypoints_dict[fn1] kp2 = keypoints_dict[fn2] # Normalize keypoints kp1n = normalize_keypoints(kp1, calc1['K']) kp2n = normalize_keypoints(kp2, calc2['K']) # Get {R, t} from calibration information R_1, t_1 = calc1['R'], calc1['T'].reshape((3, 1)) R_2, t_2 = calc2['R'], calc2['T'].reshape((3, 1)) # Compute dR, dt dR = np.dot(R_2, R_1.T) dT = t_2 - np.dot(dR, t_1) if cfg.dataset == 'phototourism': kp1_int = np.round(kp1).astype(int) kp2_int = np.round(kp2).astype(int) kp1_int[:, 1] = np.clip(kp1_int[:, 1], 0, depth[fn1].shape[0] - 1) kp1_int[:, 0] = np.clip(kp1_int[:, 0], 0, depth[fn1].shape[1] - 1) kp2_int[:, 1] = np.clip(kp2_int[:, 1], 0, depth[fn2].shape[0] - 1) kp2_int[:, 0] = np.clip(kp2_int[:, 0], 0, depth[fn2].shape[1] - 1) d1 = np.expand_dims(depth[fn1][kp1_int[:, 1], kp1_int[:, 0]], axis=-1) d2 = np.expand_dims(depth[fn2][kp2_int[:, 1], kp2_int[:, 0]], axis=-1) # Project with depth kp1n_p, kp2n_p = get_projected_kp(kp1n, kp2n, d1, d2, dR, dT) kp1_p = unnormalize_keypoints(kp1n_p, calc2['K']) kp2_p = unnormalize_keypoints(kp2n_p, calc1['K']) # Re-index keypoints from matches kp1_inl = kp1[inl[0]] kp2_inl = kp2[inl[1]] kp1_p_inl = kp1_p[inl[0]] kp2_p_inl = kp2_p[inl[1]] kp1n_inl = kp1n[inl[0]] kp2n_inl = kp2n[inl[1]] kp1n_p_inl = kp1n_p[inl[0]] kp2n_p_inl = kp2n_p[inl[1]] d1_inl = d1[inl[0]] d2_inl = d2[inl[1]] # Filter out keypoints with invalid depth nonzero_index = np.nonzero(np.squeeze(d1_inl * d2_inl)) zero_index = np.where(np.squeeze(d1_inl * d2_inl) == 0)[0] kp1_inl_nonzero = kp1_inl[nonzero_index] kp2_inl_nonzero = kp2_inl[nonzero_index] kp1_p_inl_nonzero = kp1_p_inl[nonzero_index] kp2_p_inl_nonzero = kp2_p_inl[nonzero_index] kp1n_inl_nonzero = kp1n_inl[nonzero_index] kp2n_inl_nonzero = kp2n_inl[nonzero_index] kp1n_p_inl_nonzero = kp1n_p_inl[nonzero_index] kp2n_p_inl_nonzero = kp2n_p_inl[nonzero_index] # Compute symmetric distance using the depth image d = get_truesym(kp1_inl_nonzero, kp2_inl_nonzero, kp1_p_inl_nonzero, kp2_p_inl_nonzero) else: # All points are valid for computing the epipolar distance. zero_index = [] # Compute symmetric epipolar distance for every match. kp1_inl_nonzero = kp1[inl[0]] kp2_inl_nonzero = kp2[inl[1]] kp1n_inl_nonzero = kp1n[inl[0]] kp2n_inl_nonzero = kp2n[inl[1]] # d = np.zeros(inl.shape[1]) d = get_episym(kp1n_inl_nonzero, kp2n_inl_nonzero, dR, dT) # canvas im, v_offset, h_offset = build_composite_image( os.path.join( data_dir, 'images', fn1 + ('.png' if cfg.dataset == 'googleurban' else '.jpg')), os.path.join( data_dir, 'images', fn2 + ('.png' if cfg.dataset == 'googleurban' else '.jpg')), margin=5, axis=1 if (not cfg.viz_composite_vert or cfg.dataset == 'googleurban' or cfg.dataset == 'pragueparks') else 0) plt.figure(figsize=(10, 10)) plt.imshow(im) linewidth = 2 # Plot matches on points without depth for idx in range(len(zero_index)): plt.plot( (kp1_inl[idx, 0] + h_offset[0], kp2_inl[idx, 0] + h_offset[1]), (kp1_inl[idx, 1] + v_offset[0], kp2_inl[idx, 1] + v_offset[1]), color='b', linewidth=linewidth) # Plot matches # Points are normalized by the focals, which are on average ~670. max_dist = 5 if cfg.dataset == 'googleurban': max_dist = 2e-4 if cfg.dataset == 'pragueparks': max_dist = 2e-4 cmap = matplotlib.cm.get_cmap('summer') order = list(range(len(d))) random.shuffle(order) for idx in order: if d[idx] <= max_dist: min_val = 0 max_val = 255 - min_val col = cmap( int(max_val * (1 - (max_dist - d[idx]) / max_dist) + min_val)) # col = cmap(255 * (max_dist - d[idx]) / max_dist) else: col = 'r' plt.plot((kp1_inl_nonzero[idx, 0] + h_offset[0], kp2_inl_nonzero[idx, 0] + h_offset[1]), (kp1_inl_nonzero[idx, 1] + v_offset[0], kp2_inl_nonzero[idx, 1] + v_offset[1]), color=col, linewidth=linewidth) plt.tight_layout() plt.axis('off') viz_file_hq = os.path.join(viz_folder_hq, '{:05d}.png'.format(i)) viz_file_lq = os.path.join(viz_folder_lq, '{:05d}.jpg'.format(i)) plt.savefig(viz_file_hq, bbox_inches='tight') # Convert with imagemagick os.system('convert -quality 75 -resize \"500>\" {} {}'.format( viz_file_hq, viz_file_lq)) plt.close() print('Done [{:.02f} s.]'.format(time() - t_start))
def _skimage_estimate_E_without_intrinsics(cfg, matches, kps1, kps2, calib1, calib2): '''Estimate the Essential matrix from correspondences. Computes the Fundamental Matrix first and then retrieves the Essential matrix assuming known intrinsics. ''' # Reference: https://docs.opencv.org/3.4.7/d9/d0c/group__calib3d.html # Defalt values for confidence: 0.99 (F), 0.999 (E) # Default value for the reprojection threshold: 3 # (We set them to -1 when not applicable as OpenCV complains otherwise) method_geom = cfg.method_geom['method'] if method_geom.lower() == 'skimage-ransac-f': min_matches = 9 cv_reprojection_threshold = cfg.method_geom['threshold'] cv_confidence = cfg.method_geom['confidence'] max_iters = cfg.method_geom['max_iter'] else: raise ValueError('Unknown method to estimate F') is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, min_matches) if not is_valid: return _fail() if len(kp1) < 9: return _fail() try: F, mask_F = skransac((kp1, kp2), FundamentalMatrixTransform, min_samples=8, residual_threshold=cv_reprojection_threshold, max_trials=max_iters, stop_probability=cv_confidence) except Exception: return _fail() mask_F = mask_F.astype(bool).flatten() F = F.params # OpenCV can return multiple values as 6x3 or 9x3 matrices if F is None: return _fail() elif F.shape[0] != 3: Fs = np.split(F, len(F) / 3) else: Fs = [F] # Find the best F K1, K2 = calib1['K'], calib2['K'] kp1n = normalize_keypoints(kp1, K1) kp2n = normalize_keypoints(kp2, K2) E, num_inlier = None, 0 # mask_E_cheirality_check = None for _F in Fs: _E = np.matmul(np.matmul(K2.T, _F), K1) _E = _E.astype(np.float64) _num_inlier, _R, _t, _mask = cv2.recoverPose(_E, kp1n[mask_F], kp2n[mask_F]) if _num_inlier >= num_inlier: num_inlier = _num_inlier E = _E # This is unused for now # mask_E_cheirality_check = _mask.flatten().astype(bool) # Return the initial list of matches (from F) indices = matches[:, mask_F.flatten()] return E, indices
def main(cfg): '''Visualization of stereo keypoints and matches. Parameters ---------- cfg: Namespace Configurations for running this part of the code. ''' # Files should not be named to prevent (easy) abuse # Instead we use 0, ..., cfg.num_viz_stereo_pairs viz_folder_hq, viz_folder_lq = get_stereo_viz_folder(cfg) # # Do not re-run if files already exist -- off for now # if os.path.exists(viz_folder_lq): # if all([ # os.path.exists( # os.path.join(viz_folder_lq, 'stereo-{}.jpg'.format(i))) # for i in range(cfg.num_viz_stereo_pairs) # ]): # print(' -- already exists, skipping stereo visualization') # return print(' -- Visualizations, stereo: "{}/{}"'.format(cfg.dataset, cfg.scene)) t_start = time() # Load keypoints, matches and errors keypoints_dict = load_h5(get_kp_file(cfg)) matches_dict = load_h5(get_match_file(cfg)) # Hacky: We need to recompute the errors, loading only for the keys errors_dict = load_h5(get_stereo_epipolar_final_match_file(cfg, th='0.1')) # Get data directory data_dir = get_data_path(cfg) # Create results folder if it does not exist if not os.path.exists(viz_folder_hq): os.makedirs(viz_folder_hq) if not os.path.exists(viz_folder_lq): os.makedirs(viz_folder_lq) # Sort alphabetically and pick different images sorted_keys = sorted(errors_dict) picked = [] pairs = [] for pair in sorted_keys: fn1, fn2 = pair.split('-') if fn1 not in picked and fn2 not in picked: picked += [fn1, fn2] pairs += [pair] if len(pairs) == cfg.num_viz_stereo_pairs: break # Load all depth maps depth = {} for pair in pairs: files = pair.split('-') for f in files: if f not in depth: depth[f] = load_depth( os.path.join(data_dir, 'depth_maps', '{}.h5'.format(f))) # Generate and save the images for i, pair in enumerate(pairs): # load metadata fn1, fn2 = pair.split('-') calib_dict = load_calib([ os.path.join(data_dir, 'calibration', 'calibration_{}.h5'.format(fn1)), os.path.join(data_dir, 'calibration', 'calibration_{}.h5'.format(fn2)) ]) calc1 = calib_dict[fn1] calc2 = calib_dict[fn2] matches = matches_dict[pair] ransac_inl_dict = load_h5(get_geom_inl_file(cfg)) inl = ransac_inl_dict[pair] # Get depth for keypoints kp1 = keypoints_dict[fn1] kp2 = keypoints_dict[fn2] kp1_int = np.round(kp1).astype(int) kp2_int = np.round(kp2).astype(int) kp1_int[:, 1] = np.clip(kp1_int[:, 1], 0, depth[fn1].shape[0] - 1) kp1_int[:, 0] = np.clip(kp1_int[:, 0], 0, depth[fn1].shape[1] - 1) d1 = np.expand_dims(depth[fn1][kp1_int[:, 1], kp1_int[:, 0]], axis=-1) d2 = np.expand_dims(depth[fn2][kp2_int[:, 1], kp2_int[:, 0]], axis=-1) # Get {R, t} from calibration information R_1, t_1 = calc1['R'], calc1['T'].reshape((3, 1)) R_2, t_2 = calc2['R'], calc2['T'].reshape((3, 1)) # Compute dR, dt dR = np.dot(R_2, R_1.T) dT = t_2 - np.dot(dR, t_1) # Normalize keypoints kp1n = normalize_keypoints(kp1, calc1['K']) kp2n = normalize_keypoints(kp2, calc2['K']) # Project with depth kp1n_p, kp2n_p = get_projected_kp(kp1n, kp2n, d1, d2, dR, dT) kp1_p = unnormalize_keypoints(kp1n_p, calc2['K']) kp2_p = unnormalize_keypoints(kp2n_p, calc1['K']) # Re-index keypoints from matches kp1_inl = kp1[inl[0]] kp2_inl = kp2[inl[1]] kp1_p_inl = kp1_p[inl[0]] kp2_p_inl = kp2_p[inl[1]] kp1n_inl = kp1n[inl[0]] kp2n_inl = kp2n[inl[1]] kp1n_p_inl = kp1n_p[inl[0]] kp2n_p_inl = kp2n_p[inl[1]] d1_inl = d1[inl[0]] d2_inl = d2[inl[1]] # Filter out keypoints with invalid depth nonzero_index = np.nonzero(np.squeeze(d1_inl * d2_inl)) zero_index = np.where(np.squeeze(d1_inl * d2_inl) == 0)[0] kp1_inl_nonzero = kp1_inl[nonzero_index] kp2_inl_nonzero = kp2_inl[nonzero_index] kp1_p_inl_nonzero = kp1_p_inl[nonzero_index] kp2_p_inl_nonzero = kp2_p_inl[nonzero_index] kp1n_inl_nonzero = kp1n_inl[nonzero_index] kp2n_inl_nonzero = kp2n_inl[nonzero_index] kp1n_p_inl_nonzero = kp1n_p_inl[nonzero_index] kp2n_p_inl_nonzero = kp2n_p_inl[nonzero_index] # Compute symmetric distance using the depth image true_d = get_truesym(kp1_inl_nonzero, kp2_inl_nonzero, kp1_p_inl_nonzero, kp2_p_inl_nonzero) # canvas im, v_offset, h_offset = build_composite_image( os.path.join(data_dir, 'images', fn1 + '.jpg'), os.path.join(data_dir, 'images', fn2 + '.jpg'), margin=5, axis=0 if cfg.viz_composite_vert else 1) plt.figure(figsize=(10, 10)) plt.imshow(im) linewidth = 2 # Plot matches on points without depth for idx in range(len(zero_index)): plt.plot((kp1[idx, 0] + h_offset[0], kp2[idx, 0] + h_offset[1]), (kp1[idx, 1] + v_offset[0], kp2[idx, 1] + v_offset[1]), color='b', linewidth=linewidth) # Plot matches on points with depth max_dist = 5 cmap = matplotlib.cm.get_cmap('summer') order = list(range(len(true_d))) random.shuffle(order) for idx in order: if true_d[idx] <= max_dist: min_val = 0 max_val = 255 - min_val col = cmap( int(max_val * (1 - (max_dist - true_d[idx]) / max_dist) + min_val)) # col = cmap(255 * (max_dist - true_d[idx]) / max_dist) else: col = 'r' plt.plot((kp1_inl_nonzero[idx, 0] + h_offset[0], kp2_inl_nonzero[idx, 0] + h_offset[1]), (kp1_inl_nonzero[idx, 1] + v_offset[0], kp2_inl_nonzero[idx, 1] + v_offset[1]), color=col, linewidth=linewidth) plt.tight_layout() plt.axis('off') viz_file_hq = os.path.join(viz_folder_hq, '{:05d}.png'.format(i)) viz_file_lq = os.path.join(viz_folder_lq, '{:05d}.jpg'.format(i)) plt.savefig(viz_file_hq, bbox_inches='tight') # Convert with imagemagick os.system('convert -quality 75 -resize \"500>\" {} {}'.format( viz_file_hq, viz_file_lq)) plt.close() print('done [{:.02f} s.]'.format(time() - t_start))
def _cv2_estimate_E_without_intrinsics(cfg, matches, kps1, kps2, calib1, calib2): '''Estimate the Essential matrix from correspondences. Computes the Fundamental Matrix first and then retrieves the Essential matrix assuming known intrinsics. ''' # Reference: https://docs.opencv.org/3.4.7/d9/d0c/group__calib3d.html # Defalt values for confidence: 0.99 (F), 0.999 (E) # Default value for the reprojection threshold: 3 # (We set them to -1 when not applicable as OpenCV complains otherwise) cur_key = 'config_{}_{}'.format(cfg.dataset, cfg.task) geom = cfg.method_dict[cur_key]['geom'] if geom['method'].lower() in ['cv2-ransac-f', 'cv2-patched-ransac-f']: min_matches = 8 cv_method = 'FM_RANSAC' cv_reprojection_threshold = geom['threshold'] cv_confidence = geom['confidence'] if geom['method'].lower() == 'cv2-patched-ransac-f': cv_max_iter = geom['max_iter'] elif geom['method'].lower() == 'cv2-lmeds-f': min_matches = 8 cv_method = 'FM_LMEDS' cv_reprojection_threshold = -1 cv_confidence = geom['confidence'] elif geom['method'].lower() == 'cv2-7pt': # This should actually be *equal* to 7? We'll probably never use it... min_matches = 7 cv_method = 'FM_7POINT' cv_reprojection_threshold = -1 cv_confidence = -1 elif geom['method'].lower() == 'cv2-8pt': min_matches = 8 cv_method = 'FM_8POINT' cv_reprojection_threshold = -1 cv_confidence = -1 else: raise ValueError('Unknown method to estimate F') is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, min_matches) if not is_valid: return _fail() cv2.setRNGSeed(cfg.opencv_seed) # Temporary fix to allow for patched opencv if geom['method'].lower() == 'cv2-patched-ransac-f': F, mask_F = cv2.findFundamentalMat( kp1, kp2, method=getattr(cv2, cv_method), ransacReprojThreshold=cv_reprojection_threshold, confidence=cv_confidence, maxIters=cv_max_iter) else: F, mask_F = cv2.findFundamentalMat( kp1, kp2, method=getattr(cv2, cv_method), ransacReprojThreshold=cv_reprojection_threshold, confidence=cv_confidence) mask_F = mask_F.astype(bool).flatten() # OpenCV can return multiple values as 6x3 or 9x3 matrices if F is None: return _fail() elif F.shape[0] != 3: Fs = np.split(F, len(F) / 3) else: Fs = [F] # Find the best F K1, K2 = calib1['K'], calib2['K'] kp1n = normalize_keypoints(kp1, K1) kp2n = normalize_keypoints(kp2, K2) E, num_inlier = None, 0 # mask_E_cheirality_check = None for _F in Fs: _E = np.matmul(np.matmul(K2.T, _F), K1) _E = _E.astype(np.float64) _num_inlier, _R, _t, _mask = cv2.recoverPose(_E, kp1n[mask_F], kp2n[mask_F]) if _num_inlier >= num_inlier: num_inlier = _num_inlier E = _E # This is unused for now # mask_E_cheirality_check = _mask.flatten().astype(bool) # Return the initial list of matches (from F) indices = matches[:, mask_F.flatten()] return E, indices
def _intel_estimate_E_without_intrinsics(cfg, matches, kps1, kps2, calib1, calib2, scales1=None, scales2=None, ori1=None, ori2=None, descs1=None, descs2=None): '''Estimate the Essential matrix from correspondences. Computes the Fundamental Matrix first and then retrieves the Essential matrix assuming known intrinsics. ''' method_geom = cfg.method_geom['method'] min_matches = 8 threshold = cfg.method_geom['threshold'] postprocess = cfg.method_geom['postprocess'] is_valid, matches, kp1, kp2 = _preprocess(matches, kps1, kps2, min_matches) if not is_valid: return _fail() pts_dfe = np.concatenate([kp1.reshape(-1, 2), kp2.reshape(-1, 2)], axis=1) if (descs1 is not None) and (descs2 is not None): desc_distance = np.sqrt(( (descs1[matches[0, :]] - descs2[matches[1, :]])**2).mean(axis=1) + 1e-9) desc_distance = desc_distance / (desc_distance.max() + 1e-6) desc_distance = desc_distance.reshape(-1, 1) FLANN_INDEX_KDTREE = 1 index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) search_params = dict(checks=50) matcher = cv2.FlannBasedMatcher(index_params, search_params) prematch = matcher.knnMatch(descs1, descs2, k=2) matches1 = [] ratio = [] matches_set1 = set(matches[0, :]) for m, n in prematch: if m.queryIdx in matches_set1: ratio.append(m.distance / n.distance) ratio = np.array(ratio).reshape(-1, 1) ratio = ratio / (ratio.max() + 1e-6) assert len(ratio) == len(desc_distance) else: warnings.warn('No descriptor provided for DFE', UserWarning) desc_distance = np.zeros((len(pts_dfe), 1)) if (scales1 is not None) and (scales2 is not None): rel_scale = np.abs(scales1[matches[0, :]] - scales2[matches[1, :]]).reshape(-1, 1) rel_scale = rel_scale / (1e-6 + rel_scale.max()) else: warnings.warn('No scale provided for DFE', UserWarning) rel_scale = np.zeros((len(pts_dfe), 1)) if (ori1 is not None) and (ori2 is not None): rel_orient = np.minimum( np.abs(ori1[matches[0, :]] - ori2[matches[1, :]]), np.abs(ori2[matches[1, :]] - ori1[matches[0, :]])).reshape(-1, 1) rel_orient = rel_orient / (1e-6 + rel_orient.max()) else: warnings.warn('No orientation provided for DFE', UserWarning) rel_orient = np.zeros((len(pts_dfe), 1)) side_info = np.concatenate([desc_distance, rel_scale, rel_orient, ratio], axis=1) model = NormalizedEightPointNet(depth=3, side_info_size=4) # TODO re-upload weights model.load_state_dict( torch.load('DFE_phototourism120.pt', map_location=torch.device('cpu'))) try: if torch.cuda.is_available(): device = torch.device('cuda:0') else: device = torch.device('cpu') except: device = torch.device('cpu') model = model.eval() model = model.to(device) pts_orig = pts_dfe.copy() pts_dfe = torch.from_numpy(pts_dfe).to(device).unsqueeze(0).float() side_info = torch.from_numpy(side_info).to( torch.float).to(device).unsqueeze(0) with torch.no_grad(): F_est, rescaling_1, rescaling_2, weights = model(pts_dfe, side_info) F_est = rescaling_1.permute(0, 2, 1).bmm(F_est[-1].bmm(rescaling_2)) F_est = F_est / F_est[:, -1, -1].unsqueeze(-1).unsqueeze(-1) F = F_est[0].data.cpu().numpy() mask_F = compute_residual(pts_orig, F) <= threshold mask_F = mask_F.astype(bool).flatten() score = mask_F.sum() F_best = F.T inliers_best = score if postprocess: import pyransac inliers_best = score for th in [25, 50, 75]: perc = np.percentile(weights, threshold) good = np.where(weights > perc)[0] if len(good) < 9: continue pts_ = pts_orig[good] #_F, _ = cv2.findFundamentalMat(pts_[:, 2:], pts_[:, :2], cv2.FM_LMEDS) _F, _mask_F = pyransac.findFundamentalMatrix( kp1[good], kp2[good], 0.25, 0.99999, 500000, 0, error_type='sampson', symmetric_error_check=True, enable_degeneracy_check=False) if _F is None: continue _mask_F = compute_residual(pts_orig, _F) <= threshold inliers = _mask_F.sum() if inliers > inliers_best: F_best = _F inliers_best = inliers mask_F = _mask_F if inliers_best < 8: return _fail() # OpenCV can return multiple values as 6x3 or 9x3 matrices F = F_best if F_best is None: return _fail() elif F.shape[0] != 3: Fs = np.split(F, len(F) / 3) else: Fs = [F] # Find the best F K1, K2 = calib1['K'], calib2['K'] kp1n = normalize_keypoints(kp1, K1) kp2n = normalize_keypoints(kp2, K2) E, num_inlier = None, 0 # mask_E_cheirality_check = None for _F in Fs: _E = np.matmul(np.matmul(K2.T, _F), K1) _E = _E.astype(np.float64) _num_inlier, _R, _t, _mask = cv2.recoverPose(_E, kp1n[mask_F], kp2n[mask_F]) if _num_inlier >= num_inlier: num_inlier = _num_inlier E = _E # This is unused for now # mask_E_cheirality_check = _mask.flatten().astype(bool) # Return the initial list of matches (from F) indices = matches[:, mask_F.flatten()] return E, indices