del far

handcrafted_solution.py

@@ -190,19 +190,6 @@ def get_SfM_depth(points3D, depth_np, gest_seg_np, K, R, t, dilate_r = 5):
           if (sfm_depth_np[j, i]!=0 and z < sfm_depth_np[j, i]) or (sfm_depth_np[j, i]==0):
             sfm_depth_np[j, i] = z
             sfm_color_np[j, i] = c
-  '''            
-  for pt, c in zip(xyz, rgb):
-    x, y, z = pt
-    u, v = x/z, y/z
-    u = u.astype(np.int32)    
-    v = v.astype(np.int32)
-    for i in range(max(0, u - dilate_r), min(W, u + dilate_r)):
-      for j in range(max(0, v - dilate_r), min(H, v + dilate_r)):        
-        if z > 0: 
-          if (sfm_depth_np[j, i]!=0 and z < sfm_depth_np[j, i]) or (sfm_depth_np[j, i]==0):
-            sfm_depth_np[j, i] = z
-            sfm_color_np[j, i] = c
-  '''  
 
   if DUMP_IMG:
     filename_sfm_depth = 'sfm_depth.png'
@@ -472,7 +459,75 @@ def uv_to_v3d(uv, depth_vert, K, R, t):
   vertices_3d = cv2.convertPointsFromHomogeneous(vertices_3d).reshape(-1, 3)
   return vertices_3d
 
-def predict(entry, visualize=False, depth_scale=2.5, ) -> Tuple[np.ndarray, List[int]]:
+def delete_one_vert(vertices, vertices_3d, connections, vert_to_del):
+  i = np.where(np.all(abs(vertices_3d - vert_to_del) < 0.01, axis=1))
+  #print(i)
+  #print(len(i[0]))
+  if len(i[0])==0:
+    if vertices:
+      return vertices, vertices_3d, connections
+    else:
+        return vertices, vertices_3d, connections
+
+  #print('to del idx=', i[0])
+  idx = i[0]#[0]
+  if vertices:
+    vertices = np.delete(vertices, idx)
+  vertices_3d = np.delete(vertices_3d, idx, axis=0)
+  conn_to_del = []
+  for i, c in enumerate(connections):
+    if c[0] == idx or c[1] == idx:
+      conn_to_del.append(i)
+    if c[0] >= idx:   
+      connections[i] = (connections[i][0]-1, connections[i][1])
+    if c[1] >= idx:      
+      connections[i] = (connections[i][0], connections[i][1]-1)
+  
+  #print(f'del {len(conn_to_del)} connections')  
+  connections = np.delete(connections, (conn_to_del), axis=0)
+  #print(vertices, vertices_3d, connections)
+  if vertices:
+    return vertices, vertices_3d, connections
+  else:
+    return vertices_3d, connections
+
+def prune_far(all_3d_vertices, connections_3d, prune_dist_thr=3000):
+    '''Prune vertices that are far away from any the other vertices'''
+    if (len(all_3d_vertices) < 2) or len(connections_3d) < 1:
+      return all_3d_vertices, connections_3d
+    
+    isolated = []
+    distmat = cdist(all_3d_vertices, all_3d_vertices)    
+    for i, v in enumerate(distmat):
+      exclude_self = np.array([x for idx,x in enumerate(v) if idx!=i])
+      #print('excluded:', exclude_self)
+      #if np.any(exclude_self > prune_dist_thr):
+      exclude_self = abs(exclude_self)
+      if min(exclude_self) > prune_dist_thr:
+        #print('del a pt w/ dist = ', min(exclude_self))
+        isolated.append(i)
+        break
+
+    while isolated:
+      isolated_pt = isolated.pop()
+      #print('isolated:', isolated_pt)
+      pt_to_del = all_3d_vertices[isolated_pt]
+      all_3d_vertices, connections_3d = delete_one_vert([], all_3d_vertices, connections_3d, pt_to_del)
+
+      distmat = cdist(all_3d_vertices, all_3d_vertices)    
+      for i, v in enumerate(distmat):
+        exclude_self = np.array([x for idx,x in enumerate(v) if idx!=i])
+        #if np.any(exclude_self > prune_dist_thr):
+        exclude_self = abs(exclude_self)
+        if min(exclude_self) > prune_dist_thr:
+          #print('del a pt w/ dist = ', min(exclude_self))
+          isolated.append(i)
+          break
+    
+    return all_3d_vertices, connections_3d
+    
+
+def predict(entry, visualize=False, prune_dist_thr=600, depth_scale=2.5, ) -> Tuple[np.ndarray, List[int]]:
     good_entry = convert_entry_to_human_readable(entry)
     points3D = good_entry['points3d']
     vert_edge_per_image = {}
@@ -514,12 +569,20 @@ def predict(entry, visualize=False, depth_scale=2.5, ) -> Tuple[np.ndarray, List
 
         vertices_3d = uv_to_v3d(uv, depth_vert, K, R, t)        
         
+        '''
+        print('before del, ', len(vertices), len(vertices_3d), len(connections))
+        vert_to_del = np.array([-2807.275, -4999.645, 1284.803])
+        vertices, vertices_3d, connections = delete_one_vert(vertices, vertices_3d, connections, vert_to_del)        
+        print('after del, ', len(vertices), len(vertices_3d), len(connections))
+        '''     
+        
         vert_edge_per_image[i] = vertices, connections, vertices_3d
     all_3d_vertices, connections_3d = merge_vertices_3d(vert_edge_per_image, 3.0) # TODO: 3cm looks too small    
     #print(f'after merge, {len(all_3d_vertices)} 3d vertices and {len(connections_3d)} 3d connections')
     #all_3d_vertices_clean, connections_3d_clean  = prune_not_connected(all_3d_vertices, connections_3d)
-    all_3d_vertices_clean, connections_3d_clean  = all_3d_vertices, connections_3d # don't prune -> cost:2.0
-    #print(f'after pruning, {len(all_3d_vertices_clean)} 3d clean vertices and {len(connections_3d_clean)} 3d clean connections')
+    all_3d_vertices_clean, connections_3d_clean = prune_far(all_3d_vertices, connections_3d, prune_dist_thr=prune_dist_thr)
+    #all_3d_vertices_clean, connections_3d_clean  = all_3d_vertices, connections_3d # don't prune -> cost:2.0
+    #print(f'after pruning, {len(all_3d_vertices_clean)} 3d clean vertices and {len(connections_3d_clean)} 3d clean connections')    
     if (len(all_3d_vertices_clean) < 2) or len(connections_3d_clean) < 1:
         print (f'Not enough vertices or connections in the 3D vertices')
         return (good_entry['__key__'], *empty_solution())