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
