Exemple #1
0
 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)
Exemple #2
0
    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)