def make_xy(self, ii, jj): geom_i, geom_j = parse_geom(self.geom[ii]), parse_geom(self.geom[jj]) # should check the image size here #load img and check img_size image_i, image_j = self.image_fullpath_list[ii], self.image_fullpath_list[jj] kp_i = loadh5(image_i+'.'+self.desc_name+'.hdf5')["keypoints"][:, :2] kp_j = loadh5(image_j+'.'+self.desc_name+'.hdf5')["keypoints"][:, :2] cx1, cy1, f1 = self.unpack_K(geom_i) cx2, cy2, f2 = self.unpack_K(geom_j) x1 = self.norm_kp(cx1, cy1, f1[0], f1[1], kp_i) x2 = self.norm_kp(cx2, cy2, f2[0], f2[1], kp_j) R_i, R_j = geom_i["R"], geom_j["R"] dR = np.dot(R_j, R_i.T) t_i, t_j = geom_i["t"].reshape([3, 1]), geom_j["t"].reshape([3, 1]) dt = t_j - np.dot(dR, t_i) if np.sqrt(np.sum(dt**2)) <= 1e-5: return [] dtnorm = np.sqrt(np.sum(dt**2)) dt /= dtnorm nn_info = loadh5(os.path.join(self.intermediate_dir, "nn-{}-{}.h5".format(ii, jj))) idx_sort, ratio_test, mutual_nearest = nn_info["idx_sort"], nn_info["ratio_test"], nn_info["mutual_nearest"] x2 = x2[idx_sort[1],:] xs = np.concatenate([x1, x2], axis=1).reshape(1,-1,4) geod_d = get_episym(x1, x2, dR, dt) ys = geod_d.reshape(-1,1) return xs, ys, dR, dt, ratio_test, mutual_nearest, cx1, cy1, f1, cx2, cy2, f2
def extractkp(idx, img, geom, geom_type): cv_kp, cv_desc = sift.detectAndCompute(img.transpose( 1, 2, 0), None) img_out = img.copy() img_out=cv2.drawKeypoints(img.transpose(1, 2, 0),cv_kp,img_out) # cv2.imwrite('sift_keypoints.png',img_out) cx = (img[0].shape[1] - 1.0) * 0.5 cy = (img[0].shape[0] - 1.0) * 0.5 # Correct coordinates using K cx = parse_geom(geom, geom_type)["K"][idx, 0, 2] cy = parse_geom(geom, geom_type)["K"][idx, 1, 2] xy = np.array([_kp.pt for _kp in cv_kp]) # Correct focals fx = parse_geom(geom, geom_type)["K"][idx, 0, 0] fy = parse_geom(geom, geom_type)["K"][idx, 1, 1] kp = ( xy - np.array([[cx, cy]]) ) / np.asarray([[fx, fy]]) desc = cv_desc if np.isclose(fx, fy): f = fx else: f = (fx, fy) return kp, desc, cx, cy, f
def compute_z_value(img, i, geom, geom_type, kp, depth): cx = (img[i][0].shape[1] - 1.0) * 0.5 cy = (img[i][0].shape[0] - 1.0) * 0.5 fx = parse_geom(geom, geom_type)["K"][i, 0, 0] fy = parse_geom(geom, geom_type)["K"][i, 1, 1] xy = kp[i] * np.asarray([[fx, fy]]) + np.array([[cx, cy]]) if len(depth) > 0: cz = depth[i][0, np.round(xy[:, 1]).astype(int), np.round(xy[:, 0]).astype(int)][..., None] else: cz = np.ones((xy.shape[0], 1)) return cz
def getxy(idx_i, idx_j, kpi, kpj, idx_sort, geom, geom_type): # ------------------------------ # Get dR R_i = parse_geom(geom, geom_type)["R"][idx_i] R_j = parse_geom(geom, geom_type)["R"][idx_j] dR = np.dot(R_j, R_i.T) # Get dt t_i = parse_geom(geom, geom_type)["t"][idx_i].reshape([3, 1]) t_j = parse_geom(geom, geom_type)["t"][idx_j].reshape([3, 1]) dt = t_j - np.dot(dR, t_i) # ------------------------------ # Get sift points for the first image x1 = kpi x2 = kpj # ------------------------------ # create x1, y1, x2, y2 as a matrix combo x1mat = np.repeat(x1[:, 0][..., None], len(x2), axis=-1) y1mat = np.repeat(x1[:, 1][..., None], len(x2), axis=1) x2mat = np.repeat(x2[:, 0][None], len(x1), axis=0) y2mat = np.repeat(x2[:, 1][None], len(x1), axis=0) # Move back to tuples idx_sort = (idx_sort[0], idx_sort[1]) x1mat = x1mat[idx_sort] y1mat = y1mat[idx_sort] x2mat = x2mat[idx_sort] y2mat = y2mat[idx_sort] # Turn into x1, x2 x1 = np.concatenate( [x1mat.reshape(-1, 1), y1mat.reshape(-1, 1)], axis=1) x2 = np.concatenate( [x2mat.reshape(-1, 1), y2mat.reshape(-1, 1)], axis=1) # make xs in NHWC xs = np.concatenate([x1, x2], axis=1).T.reshape(4, 1, -1).transpose((1, 2, 0)) # ------------------------------ # Get the geodesic distance using with x1, x2, dR, dt geod_d = get_episym(x1, x2, dR, dt) ys = geod_d # Save R, t for evaluation Rs = np.array(dR).reshape(3, 3) # normalize t before saving dtnorm = np.sqrt(np.sum(dt**2)) assert (dtnorm > 1e-5) dt /= dtnorm ts = np.array(dt).flatten() return xs, ys, Rs, ts
def compute_intrinsics(img, i, geom, geom_type): h, w = img[i].shape[1:] cx = (w - 1.0) * 0.5 cy = (h - 1.0) * 0.5 # Correct coordinates using K Ks = parse_geom(geom, geom_type)["K"] cx += Ks[i, 0, 2] cy += Ks[i, 1, 2] # Correct focals fx = Ks[i, 0, 0] fy = Ks[i, 1, 1] # exec(embed_breakpoint()) return cx, cy, fx, fy
def make_xy(num_sample, pairs, kp, z, desc, img, geom, vis, depth, geom_type, cur_folder): xs = [] ys = [] Rs = [] ts = [] mutuals = [] ratios = [] img1s = [] img2s = [] cx1s = [] cy1s = [] f1s = [] cx2s = [] cy2s = [] f2s = [] # Create a random folder in scratch dump_dir = os.path.join(cur_folder, "dump") if not os.path.exists(dump_dir): os.makedirs(dump_dir) # randomly suffle the pairs and select num_sample amount np.random.seed(1234) cur_pairs = [ pairs[_i] for _i in np.random.permutation(len(pairs))[:num_sample] ] idx = 0 for ii, jj in cur_pairs: idx += 1 print( "\rExtracting keypoints {} / {}".format(idx, len(cur_pairs)), end="") sys.stdout.flush() # Check and extract keypoints if necessary for i in [ii, jj]: dump_file = os.path.join(dump_dir, "kp-z-desc-{}.h5".format(i)) if not os.path.exists(dump_file): if kp[i] is None: cv_kp, cv_desc = sift.detectAndCompute(img[i].transpose( 1, 2, 0), None) cx = (img[i][0].shape[1] - 1.0) * 0.5 cy = (img[i][0].shape[0] - 1.0) * 0.5 # Correct coordinates using K cx += parse_geom(geom, geom_type)["K"][i, 0, 2] cy += parse_geom(geom, geom_type)["K"][i, 1, 2] xy = np.array([_kp.pt for _kp in cv_kp]) # Correct focals fx = parse_geom(geom, geom_type)["K"][i, 0, 0] fy = parse_geom(geom, geom_type)["K"][i, 1, 1] kp[i] = ( xy - np.array([[cx, cy]]) ) / np.asarray([[fx, fy]]) desc[i] = cv_desc if z[i] is None: cx = (img[i][0].shape[1] - 1.0) * 0.5 cy = (img[i][0].shape[0] - 1.0) * 0.5 fx = parse_geom(geom, geom_type)["K"][i, 0, 0] fy = parse_geom(geom, geom_type)["K"][i, 1, 1] xy = kp[i] * np.asarray([[fx, fy]]) + np.array([[cx, cy]]) if len(depth) > 0: z[i] = depth[i][ 0, np.round(xy[:, 1]).astype(int), np.round(xy[:, 0]).astype(int)][..., None] else: z[i] = np.ones((xy.shape[0], 1)) # Write descs to harddisk to parallize dump_dict = {} dump_dict["kp"] = kp[i] dump_dict["z"] = z[i] dump_dict["desc"] = desc[i] saveh5(dump_dict, dump_file) else: dump_dict = loadh5(dump_file) kp[i] = dump_dict["kp"] z[i] = dump_dict["z"] desc[i] = dump_dict["desc"] print("") # Create arguments pool_arg = [] idx = 0 for ii, jj in cur_pairs: idx += 1 pool_arg += [(dump_dir, idx, ii, jj)] # Run mp job ratio_CPU = 0.8 number_of_process = int(ratio_CPU * mp.cpu_count()) pool = mp.Pool(processes=number_of_process) manager = mp.Manager() queue = manager.Queue() # for debugging in dump_data_pair # dump_data_pair(pool_arg[1] + (queue,)) for idx_arg in xrange(len(pool_arg)): pool_arg[idx_arg] = pool_arg[idx_arg] + (queue,) # map async pool_res = pool.map_async(dump_data_pair, pool_arg) # monitor loop while True: if pool_res.ready(): break else: size = queue.qsize() print("\rDistMat {} / {}".format(size, len(pool_arg)), end="") sys.stdout.flush() time.sleep(1) pool.close() pool.join() print("") # Pack data idx = 0 total_num = 0 good_num = 0 bad_num = 0 for ii, jj in cur_pairs: idx += 1 print("\rWorking on {} / {}".format(idx, len(cur_pairs)), end="") sys.stdout.flush() # ------------------------------ # Get dR R_i = parse_geom(geom, geom_type)["R"][ii] R_j = parse_geom(geom, geom_type)["R"][jj] dR = np.dot(R_j, R_i.T) # Get dt t_i = parse_geom(geom, geom_type)["t"][ii].reshape([3, 1]) t_j = parse_geom(geom, geom_type)["t"][jj].reshape([3, 1]) dt = t_j - np.dot(dR, t_i) # ------------------------------ # Get sift points for the first image x1 = kp[ii] y1 = np.concatenate([kp[ii] * z[ii], z[ii]], axis=1) # Project the first points into the second image y1p = np.matmul(dR[None], y1[..., None]) + dt[None] # move back to the canonical plane x1p = y1p[:, :2, 0] / y1p[:, 2, 0][..., None] # ------------------------------ # Get sift points for the second image x2 = kp[jj] # # DEBUG ------------------------------ # # Check if the image projections make sense # draw_val_res( # img[ii], # img[jj], # x1, x1p, np.random.rand(x1.shape[0]) < 0.1, # (img[ii][0].shape[1] - 1.0) * 0.5, # (img[ii][0].shape[0] - 1.0) * 0.5, # parse_geom(geom, geom_type)["K"][ii, 0, 0], # (img[jj][0].shape[1] - 1.0) * 0.5, # (img[jj][0].shape[0] - 1.0) * 0.5, # parse_geom(geom, geom_type)["K"][jj, 0, 0], # "./debug_imgs/", # "debug_img{:04d}.png".format(idx) # ) # ------------------------------ # create x1, y1, x2, y2 as a matrix combo x1mat = np.repeat(x1[:, 0][..., None], len(x2), axis=-1) y1mat = np.repeat(x1[:, 1][..., None], len(x2), axis=1) x1pmat = np.repeat(x1p[:, 0][..., None], len(x2), axis=-1) y1pmat = np.repeat(x1p[:, 1][..., None], len(x2), axis=1) x2mat = np.repeat(x2[:, 0][None], len(x1), axis=0) y2mat = np.repeat(x2[:, 1][None], len(x1), axis=0) # Load precomputed nearest neighbors, ratios and mutual idx_sort = loadh5(os.path.join( dump_dir, "idx_sort-{}-{}.h5".format(ii, jj)))["idx_sort"] mutual = loadh5(os.path.join( dump_dir, "mutual_ratio-{}-{}.h5".format(ii, jj)))["mutual"] ratio = loadh5(os.path.join( dump_dir, "mutual_ratio-{}-{}.h5".format(ii, jj)))["ratio"] mutuals += [mutual] ratios += [ratio] # Move back to tuples idx_sort = (idx_sort[0], idx_sort[1]) x1mat = x1mat[idx_sort] y1mat = y1mat[idx_sort] x1pmat = x1pmat[idx_sort] y1pmat = y1pmat[idx_sort] x2mat = x2mat[idx_sort] y2mat = y2mat[idx_sort] # Turn into x1, x1p, x2 x1 = np.concatenate( [x1mat.reshape(-1, 1), y1mat.reshape(-1, 1)], axis=1) x1p = np.concatenate( [x1pmat.reshape(-1, 1), y1pmat.reshape(-1, 1)], axis=1) x2 = np.concatenate( [x2mat.reshape(-1, 1), y2mat.reshape(-1, 1)], axis=1) # make xs in NHWC xs += [ np.concatenate([x1, x2], axis=1).T.reshape(4, 1, -1).transpose( (1, 2, 0)) ] # ------------------------------ # Get the geodesic distance using with x1, x2, dR, dt if config.obj_geod_type == "sampson": geod_d = get_sampsons(x1, x2, dR, dt) elif config.obj_geod_type == "episqr": geod_d = get_episqr(x1, x2, dR, dt) elif config.obj_geod_type == "episym": geod_d = get_episym(x1, x2, dR, dt) # Get *rough* reprojection errors. Note that the depth may be noisy. We # ended up not using this... reproj_d = np.sum((x2 - x1p)**2, axis=1) # count inliers and outliers total_num += len(geod_d) good_num += np.sum((geod_d < config.obj_geod_th)) bad_num += np.sum((geod_d >= config.obj_geod_th)) ys += [np.stack([geod_d, reproj_d], axis=1)] # Save R, t for evaluation Rs += [np.array(dR).reshape(3, 3)] # normalize t before saving dtnorm = np.sqrt(np.sum(dt**2)) assert (dtnorm > 1e-5) dt /= dtnorm ts += [np.array(dt).flatten()] # Save img1 and img2 for display img1s += [img[ii]] img2s += [img[jj]] cx = (img[ii][0].shape[1] - 1.0) * 0.5 cy = (img[ii][0].shape[0] - 1.0) * 0.5 # Correct coordinates using K cx += parse_geom(geom, geom_type)["K"][ii, 0, 2] cy += parse_geom(geom, geom_type)["K"][ii, 1, 2] fx = parse_geom(geom, geom_type)["K"][ii, 0, 0] fy = parse_geom(geom, geom_type)["K"][ii, 1, 1] if np.isclose(fx, fy): f = fx else: f = (fx, fy) cx1s += [cx] cy1s += [cy] f1s += [f] cx = (img[jj][0].shape[1] - 1.0) * 0.5 cy = (img[jj][0].shape[0] - 1.0) * 0.5 # Correct coordinates using K cx += parse_geom(geom, geom_type)["K"][jj, 0, 2] cy += parse_geom(geom, geom_type)["K"][jj, 1, 2] fx = parse_geom(geom, geom_type)["K"][jj, 0, 0] fy = parse_geom(geom, geom_type)["K"][jj, 1, 1] if np.isclose(fx, fy): f = fx else: f = (fx, fy) cx2s += [cx] cy2s += [cy] f2s += [f] # Do *not* convert to numpy arrays, as the number of keypoints may differ # now. Simply return it print(".... done") if total_num > 0: print(" Good pairs = {}, Total pairs = {}, Ratio = {}".format( good_num, total_num, float(good_num) / float(total_num))) print(" Bad pairs = {}, Total pairs = {}, Ratio = {}".format( bad_num, total_num, float(bad_num) / float(total_num))) res_dict = {} res_dict["xs"] = xs res_dict["ys"] = ys res_dict["Rs"] = Rs res_dict["ts"] = ts res_dict["img1s"] = img1s res_dict["cx1s"] = cx1s res_dict["cy1s"] = cy1s res_dict["f1s"] = f1s res_dict["img2s"] = img2s res_dict["cx2s"] = cx2s res_dict["cy2s"] = cy2s res_dict["f2s"] = f2s res_dict["mutuals"] = mutuals res_dict["ratios"] = ratios res_dict["pairs"] = cur_pairs return res_dict