fix: Clean pose_utils

pose_utils.py

@@ -25,29 +25,6 @@ def sRT_to_4x4(scale, R, T, device):
 def to_numpy(tensor):
     return tensor.cpu().numpy() if isinstance(tensor, torch.Tensor) else tensor
 
-def calculate_depth_map(pts3d, R, T):
-    """
-    Calculate ray depths directly using camera center and 3D points.
-    
-    Args:
-    pts3d (np.array): 3D points in world coordinates, shape (H, W, 3)
-    R (np.array): Rotation matrix, shape (3, 3)
-    T (np.array): Translation vector, shape (3, 1)
-    
-    Returns:
-    np.array: Depth map of shape (H, W)
-    """
-    # Camera center in world coordinates is simply -T
-    C = -T.ravel()
-    
-    # Calculate ray vectors
-    ray_vectors = pts3d - C
-    
-    # Calculate ray depths
-    depth_map = np.linalg.norm(ray_vectors, axis=2)
-    
-    return depth_map
-
 def fast_pnp(pts3d, focal, msk, device, pp=None, niter_PnP=10):
     # extract camera poses and focals with RANSAC-PnP
     if msk.sum() < 4:
@@ -69,7 +46,7 @@ def fast_pnp(pts3d, focal, msk, device, pp=None, niter_PnP=10):
     else:
         pp = to_numpy(pp)
 
-    best = 0, None, None, None, None
+    best = 0,
     for focal in tentative_focals:
         K = np.float32([(focal, 0, pp[0]), (0, focal, pp[1]), (0, 0, 1)])
 
@@ -81,20 +58,16 @@ def fast_pnp(pts3d, focal, msk, device, pp=None, niter_PnP=10):
 
         score = len(inliers)
         if success and score > best[0]:
-            depth_map = calculate_depth_map(pts3d, R, T)
-            best = score, R, T, focal, depth_map
+            best = score, R, T, focal
 
     if not best[0]:
         return None
 
-    _, R, T, best_focal, depth_map = best
+    _, R, T, best_focal = best
     R = cv2.Rodrigues(R)[0]  # world to cam
     R, T = map(torch.from_numpy, (R, T))
-    depth_map = torch.from_numpy(depth_map).to(device)
-
-    cam_to_world = inv(sRT_to_4x4(1, R, T, device))  # cam to world
 
-    return best_focal, cam_to_world, depth_map
+    return best_focal, inv(sRT_to_4x4(1, R, T, device))  # cam to world
 
 def solve_cemara(pts3d, msk, device, focal=None, pp=None):
     # Estimate focal length
@@ -105,9 +78,9 @@ def solve_cemara(pts3d, msk, device, focal=None, pp=None):
     result = fast_pnp(pts3d, focal, msk, device, pp)
     
     if result is None:
-        return None, focal, None
+        return None, focal
     
-    best_focal, camera_to_world, depth_map = result
+    best_focal, camera_to_world = result
     
     # Construct K matrix
     H, W, _ = pts3d.shape
@@ -123,4 +96,4 @@ def solve_cemara(pts3d, msk, device, focal=None, pp=None):
     camera_parameters.intrinsic = intrinsic
     camera_parameters.extrinsic = torch.inverse(camera_to_world).cpu().numpy()
 
-    return camera_parameters, best_focal, depth_map
+    return camera_parameters, best_focal