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 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 make_xy(pair_index, img, kp, desc, aff, K, R, t, cur_folder): kp_initial = copy.deepcopy(kp) xs = [] xs_initial = [] ys = [] Rs = [] ts = [] img1s = [] img2s = [] cx1s = [] cy1s = [] f1s = [] cx2s = [] cy2s = [] f2s = [] affine = [] # 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) pool_arg = [] for idx in range(pair_index.shape[0]): ii = pair_index[idx, 0] jj = pair_index[idx, 1] print("\rExtracting keypoints {} / {}".format(idx, pair_index.shape[0]), end="") sys.stdout.flush() # Check and extract keypoints if necessary for i in [ii, jj]: i = int(i) dump_file = os.path.join(dump_dir, "kp-aff-desc-{}.h5".format(i)) if not os.path.exists(dump_file): # Correct coordinates using K cx = K[i, 2] cy = K[i, 5] # Correct focals fx = K[i, 0] fy = K[i, 4] kp[i] = (kp[i] - np.array([[cx, cy]])) / np.asarray([[fx, fy]]) # Write descs to harddisk to parallize dump_dict = {} dump_dict["kp_normal"] = kp[i] dump_dict["kp"] = kp_initial[i] dump_dict["desc"] = desc[i] dump_dict["aff"] = aff[i] saveh5(dump_dict, dump_file) else: dump_dict = loadh5(dump_file) kp[i] = dump_dict["kp_normal"] kp_initial[i] = dump_dict["kp"] desc[i] = dump_dict["desc"] aff[i] = dump_dict["aff"] pool_arg += [(dump_dir, idx, int(ii), int(jj))] print("") # Run mp job ratio_CPU = 0.9 number_of_process = int(ratio_CPU * mp.cpu_count()) pool = mp.Pool(processes=number_of_process) manager = mp.Manager() queue = manager.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 ratio1 = [] ratio2 = [] for idx in range(pair_index.shape[0]): ii = int(pair_index[idx, 0]) jj = int(pair_index[idx, 1]) print("\rWorking on {} / {}".format(idx, pair_index.shape[0]), end="") sys.stdout.flush() K1 = K[ii].reshape(3, 3) K2 = K[jj].reshape(3, 3) # ------------------------------ # Get dR R_i = R[ii].reshape(3, 3) R_j = R[jj].reshape(3, 3) dR = np.dot(R_j, R_i.T) # Get dt t_i = t[ii].reshape(3, 1) t_j = t[jj].reshape(3, 1) dt = t_j - np.dot(dR, t_i) # ------------------------------ # Get keypoints for the first image x1 = kp[ii] y1 = np.concatenate((kp[ii], np.ones((kp[ii].shape[0], 1))), 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 keypoints 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 idx_sort = loadh5( os.path.join(dump_dir, "idx_sort-{}-{}.h5".format(ii, jj)))["idx_sort"] # 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) # geod_d = get_episym(x1, x2, dR, dt, K1_inv, K2_inv, K1, K2) # 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)) ''' mask = np.zeros(len(geod_d)) for i in range(len(geod_d)): if geod_d[i] < config.obj_geod_th: mask[i] = 1 np.savetxt(os.path.join(dump_dir, "mask-{}-{}.txt".format(ii, jj)), mask) ''' 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 = K[ii, 2] cy = K[ii, 5] cx1s += [cx] cy1s += [cy] cx = K[jj, 2] cy = K[jj, 5] cx2s += [cx] cy2s += [cy] fx = K[ii, 0] fy = K[ii, 4] if np.isclose(fx, fy): f = fx else: f = (fx, fy) f1s += [f] fx = K[jj, 0] fy = K[jj, 4] if np.isclose(fx, fy): f = fx else: f = (fx, fy) f2s += [f] # Generate xs_initial and T-transform x1 = kp_initial[ii] x2 = kp_initial[jj] aff1 = aff[ii] aff2 = aff[jj] aff1 = np.repeat(aff1[:, :][..., None], len(aff2), axis=-1).transpose(0, 2, 1) aff2 = np.repeat(aff2[:, :][None], len(aff1), axis=0) 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) idx_sort = loadh5( os.path.join(dump_dir, "idx_sort-{}-{}.h5".format(ii, jj)))["idx_sort"] # 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] aff1 = aff1[idx_sort] aff2 = aff2[idx_sort] # Turn into x1, x1p, 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) affine += [ np.concatenate( [aff1.reshape(-1, 9), aff2.reshape(-1, 9)], axis=1) ] xs_initial += [ np.concatenate([x1, x2], axis=1).T.reshape(4, 1, -1).transpose( (1, 2, 0)) ] print("") # 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["xs_initial"] = xs_initial res_dict["affine"] = affine 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 return res_dict
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