def expand(self):
it = 1
queue = self.patches.copy()
while len(queue) > 0:
p = queue.pop(0)
for cell in p.cells:
C = []
for i in range(-1, 2):
for j in range(-1, 2):
if i == 0 and j == 0:
continue
idx = (cell.y + i * 2, cell.x + j * 2)
if idx in self.cells[cell.ref_idx]:
C.append(self.cells[cell.ref_idx][idx])
# cp = p.cam_center + p.k * p.d
# p_depth = (self.extrinsics[p.ref_idx] @ np.array([*cp, 1]))[-1]
C_filtered = []
for c in C:
neccessary = True
if len(c.q) > 0:
for pp in c.q:
# cpp = pp.cam_center + pp.k * pp.d
# pp_depth = (self.extrinsics[pp.ref_idx] @ np.array([*cpp, 1]))[-1]
# rho1 = 2 * self.intrinsic_inv[0, 0] * (p_depth + pp_depth) / 2
# if np.abs(np.dot(cp - cpp, sph2norm(p.theta, p.phi))) + np.abs(np.dot(cp - cpp, sph2norm(pp.theta, pp.phi))) < 2 * rho1:
if self.is_neighbor(p, pp):
neccessary = False
break
if neccessary and len(c.q_star) > 0:
for pp in c.q_star:
if self.gp(pp, pp.ref_idx, pp.v_star) < 0.3:
neccessary = False
break
if neccessary:
C_filtered.append(c)
C = C_filtered
# print(len(C))
for c in C:
pprime = Patch(patch=p)
# set new pc
n = sph2norm(p.theta, p.phi)
p_center = p.cam_center + p.k * p.d
plane = -np.dot(n, p_center)
pt = np.array([c.x + 1, c.y + 1, 1])
pt_world = np.linalg.pinv(self.extrinsics[pprime.ref_idx]) @ self.intrinsic_inv @ pt
pt_world = pt_world[:3] / pt_world[-1]
cam2pt = pt_world - p.cam_center
pprime_center = -(plane + np.dot(n, p.cam_center)) / np.dot(n, cam2pt) * cam2pt + p.cam_center
# print(p_center)
# print(pprime_center)
pprime.d = pprime_center - p.cam_center
pprime.k = np.linalg.norm(pprime.d)
pprime.d /= pprime.k
# import pdb; pdb.set_trace()
proj_ref = pprime.project_onto_img(self.intrinsic, self.extrinsics[pprime.ref_idx])
v_star = []
for cand in pprime.v:
proj_cand = pprime.project_onto_img(self.intrinsic, self.extrinsics[cand])
zncc = calc_zncc(self.grays[pprime.ref_idx], self.grays[cand], proj_ref, proj_cand)
if 1. - zncc < 0.6:
v_star.append(cand)
pprime.v_star = v_star
if len(pprime.v_star) < 2:
continue
def gp(x):
backup = pprime.k, pprime.theta, pprime.phi
pprime.k, pprime.theta, pprime.phi = x
res = self.gp(pprime, pprime.ref_idx, pprime.v_star)
pprime.k, pprime.theta, pprime.phi = backup
return res
def gp_grad(x):
backup = pprime.k, pprime.theta, pprime.phi
pprime.k, pprime.theta, pprime.phi = x
res = self.gp_grad(pprime, pprime.ref_idx, pprime.v_star)
pprime.k, pprime.theta, pprime.phi = backup
return res
# print(patch.k, patch.theta, patch.phi)
best_x = optimize.fmin_cg(gp, np.array([pprime.k, pprime.theta, pprime.phi]), fprime=gp_grad, disp=False)
pprime.k, pprime.theta, pprime.phi = best_x
# determine depth test threshold
n = sph2norm(pprime.theta, pprime.phi)
pprime_center = pprime.cam_center + pprime.k * pprime.d
plane = -np.dot(n, pprime_center)
pt = np.array([c.x + 3, c.y + 1, 1]) # offset two pixels
pt_world = np.linalg.pinv(self.extrinsics[pprime.ref_idx]) @ self.intrinsic_inv @ pt
pt_world = pt_world[:3] / pt_world[-1]
cam2pt = pt_world - pprime.cam_center
delta = np.linalg.norm(-(plane + np.dot(n, pprime.cam_center)) / np.dot(n, cam2pt) * cam2pt + pprime.cam_center - pprime_center)
pprime_depth = (self.extrinsics[pprime.ref_idx] @ np.array([*pprime_center, 1]))[-1]
for cand in range(len(self.grays)):
if cand == pprime.ref_idx:
continue
proj = self.intrinsic @ self.extrinsics[cand] @ np.array([*(pprime.cam_center + pprime.k * pprime.d), 1])
proj = (proj[:2] / proj[-1]).astype(np.int)
if (proj[1], proj[0]) in self.cells[cand]:
if len(self.cells[cand][(proj[1], proj[0])].q) > 0:
valid = True
for pp in self.cells[cand][(proj[1], proj[0])].q:
pp = self.cells[cand][(proj[1], proj[0])].q[0]
pp_proj = self.extrinsics[pp.ref_idx] @ np.array([*(pp.cam_center + pp.k * pp.d), 1])
depth = pp_proj[-1]
if pprime_depth > depth + delta:
valid = False
break
if valid:
pprime.v.append(cand)
pprime.v = list(set(pprime.v))
v_star = []
for cand in pprime.v:
proj_cand = pprime.project_onto_img(self.intrinsic, self.extrinsics[cand])
zncc = calc_zncc(self.grays[pprime.ref_idx], self.grays[cand], proj_ref, proj_cand)
if 1. - zncc < 0.3:
v_star.append(cand)
pprime.v_star = list(set(v_star))
if len(pprime.v_star) < 2:
continue
queue.append(pprime)
self.patches.append(pprime)
for idx in pprime.v:
proj = self.intrinsic @ self.extrinsics[idx] @ np.array([*pprime_center, 1])
proj = (proj[:2] / proj[-1]).astype(np.int)
if (proj[1], proj[0]) in self.cells[idx]:
self.cells[idx][(proj[1], proj[0])].q.append(pprime)
for idx in pprime.v_star:
proj = self.intrinsic @ self.extrinsics[idx] @ np.array([*pprime_center, 1])
proj = (proj[:2] / proj[-1]).astype(np.int)
if (proj[1], proj[0]) in self.cells[idx]:
self.cells[idx][(proj[1], proj[0])].q_star.append(pprime)
pprime.cells.append(self.cells[idx][(proj[1], proj[0])])
print('processed one: {}, current queue length: {}'.format(len(self.patches), len(queue)))
colors = np.zeros((len(self.patches), 3))
vis = self.rgbs[0].copy()
for i, patch in enumerate(self.patches):
pts = patch.project_onto_img(self.intrinsic, self.extrinsics[0])
pt = pts[pts.shape[0] // 2].astype(np.int)
if pt[0] >= 0 and pt[1] >= 0 and pt[0] < self.grays[0].shape[1] and pt[1] < self.grays[0].shape[0]:
vis = cv2.circle(vis, (int(pt[0]), int(pt[1])), 1, color=(255, 0, 0), thickness=-1)
colors[i] = self.rgbs[0][pt[1], pt[0]]
cv2.imshow('patch', cv2.resize(vis[..., ::-1], (0, 0), fx=4, fy=4))
cv2.waitKey(10)
def matching(self):
feature_points_mask = [np.ones((self.feature_points[i].shape[0]), dtype=np.bool) for i in range(len(self.grays))]
for i in range(len(self.grays) - 1):
for camera_point in tqdm(self.camera_feature_points[i][feature_points_mask[i]]):
corrs = []
for j in range(len(self.grays)):
if i == j:
continue
epiline = self.intrinsic_inv.T @ self.Fs[(i, j)] @ camera_point
epiline /= np.linalg.norm(epiline[:2])
dist2epiline = np.abs(np.sum(self.feature_points[j][feature_points_mask[j]] * epiline[None], -1))
close_pts = self.camera_feature_points[j][feature_points_mask[j]][dist2epiline < 3]
for pt in close_pts:
c = np.asarray(opt_triangulate([self.extrinsics[i], self.extrinsics[j]], [[camera_point[0], camera_point[1], pt[0], pt[1]]])[0])
c = c[:3]
corrs.append((j, pt, c, np.linalg.norm(c - self.camera_centers[i])))
# if len(close_pts) > 10:
# img = self.rgbs[j].copy()
# img_ref = self.rgbs[i].copy()
# cv2.circle(img_ref, (self.feature_points[i][k][0], self.feature_points[i][k][1]), 3, color=(255, 0, 0), thickness=-1)
# for point in close_pts:
# img = cv2.circle(img, (int(point[0]), int(point[1])), 1, color=(255, 0, 0), thickness=-1)
# cv2.imshow('img', img[:, :, ::-1])
# cv2.imshow('img_ref', img_ref[:, :, ::-1])
# cv2.waitKey()
corrs.sort(key=lambda x: x[3])
for (j, pt, c, dist) in corrs:
patch = Patch(c, self.intrinsic_inv, self.extrinsics[i], i)
# first round
proj_i = patch.project_onto_img(self.intrinsic, self.extrinsics[i])
c = patch.cam_center + patch.k * patch.d
co = c[None] - np.stack(self.camera_centers)
co /= np.linalg.norm(co, axis=-1, keepdims=True)
cos = co @ sph2norm(patch.theta, patch.phi)
vp = list(np.where(cos > 0.5)[0])
v_star = []
for cand in vp:
proj_cand = patch.project_onto_img(self.intrinsic, self.extrinsics[cand])
zncc = calc_zncc(self.grays[i], self.grays[cand], proj_i, proj_cand)
if 1. - zncc < 0.6:
v_star.append(cand)
if len(v_star) < 2:
continue
def gp(x):
backup = patch.k, patch.theta, patch.phi
patch.k, patch.theta, patch.phi = x
res = self.gp(patch, patch.ref_idx, v_star)
patch.k, patch.theta, patch.phi = backup
return res
def gp_grad(x):
backup = patch.k, patch.theta, patch.phi
patch.k, patch.theta, patch.phi = x
res = self.gp_grad(patch, patch.ref_idx, v_star)
patch.k, patch.theta, patch.phi = backup
return res
# print(patch.k, patch.theta, patch.phi)
best_x = optimize.fmin_cg(gp, np.array([patch.k, patch.theta, patch.phi]), fprime=gp_grad, disp=False)
patch.k, patch.theta, patch.phi = best_x
# print(patch.k, patch.theta, patch.phi)
# conjugate gradient descent my implementation
# last_grad = None
# for it in range(100):
# if it == 0:
# last_grad = gp_grad()
# p = -last_grad
# last_p = p
# else:
# beta = np.dot(last_grad, last_grad) / np.dot(last_last_grad, last_last_grad)
# p = -last_grad + beta * last_p
# gp_before = gp()
# grad_before = gp_grad()
# alpha = 1.
# k, theta, phi = patch.k, patch.theta, patch.phi
# patch.k = float(k + alpha * p[0])
# patch.theta = float(theta + alpha * p[1])
# patch.phi = float(phi + alpha * p[2])
# patch.refresh()
# while gp() > gp_before + 1 / 2 * alpha * np.dot(p, grad_before):
# alpha *= 0.5
# patch.k = float(k + alpha * p[0])
# patch.theta = float(theta + alpha * p[1])
# patch.phi = float(phi + alpha * p[2])
# patch.refresh()
# last_p = p
# last_last_grad = last_grad
# last_grad = gp_grad()
# if np.any(np.isnan(last_grad)):
# import pdb; pdb.set_trace()
# if np.linalg.norm(last_grad) < 0.1:
# break
# second round
proj_i = patch.project_onto_img(self.intrinsic, self.extrinsics[i])
c = patch.cam_center + patch.k * patch.d
co = c[None] - np.stack(self.camera_centers)
co /= np.linalg.norm(co, axis=-1, keepdims=True)
cos = co @ sph2norm(patch.theta, patch.phi)
vp = list(np.where(cos > 0.5)[0])
v_star = []
for cand in vp:
proj_cand = patch.project_onto_img(self.intrinsic, self.extrinsics[cand])
zncc = calc_zncc(self.grays[i], self.grays[cand], proj_i, proj_cand)
if 1. - zncc < 0.3:
v_star.append(cand)
if len(v_star) < 2:
continue
patch.v = vp
patch.v_star = v_star
to_remove = []
for idx in vp:
proj = self.intrinsic @ self.extrinsics[idx] @ np.array([*c, 1])
proj = (proj[:2] / proj[-1]).astype(np.int)
if (proj[1], proj[0]) in self.cells[idx]:
self.cells[idx][(proj[1], proj[0])].q.append(patch)
for idx in v_star:
proj = self.intrinsic @ self.extrinsics[idx] @ np.array([*c, 1])
proj = (proj[:2] / proj[-1]).astype(np.int)
if (proj[1], proj[0]) in self.cells[idx]:
self.cells[idx][(proj[1], proj[0])].q_star.append(patch)
patch.cells.append(self.cells[idx][(proj[1], proj[0])])
for pt_idx in self.cells[idx][(proj[1], proj[0])].feature_points_idx:
to_remove.append((idx, pt_idx))
for (idx, pt_idx) in to_remove:
feature_points_mask[idx][pt_idx] = False
self.patches.append(patch)
colors = np.zeros((len(self.patches), 3))
vis = self.rgbs[0].copy()
for i, patch in enumerate(self.patches):
pts = patch.project_onto_img(self.intrinsic, self.extrinsics[0])
pt = pts[pts.shape[0] // 2].astype(np.int)
if pt[0] >= 0 and pt[1] >= 0 and pt[0] < self.grays[0].shape[1] and pt[1] < self.grays[0].shape[0]:
vis = cv2.circle(vis, (int(pt[0]), int(pt[1])), 1, color=(255, 0, 0), thickness=-1)
colors[i] = self.rgbs[0][pt[1], pt[0]]
cv2.imshow('patch', cv2.resize(vis[..., ::-1], (0, 0), fx=4, fy=4))
draw_patches(self.patches, colors)