yakvrz commited on
Commit
42a8e2e
·
1 Parent(s): 736cb2a

Make pose alignment optional without evo dependency

Browse files
src/depth_anything_3/utils/pose_align.py CHANGED
@@ -15,7 +15,11 @@
15
  from typing import List
16
  import numpy as np
17
  import torch
18
- from evo.core.trajectory import PosePath3D
 
 
 
 
19
 
20
  from depth_anything_3.utils.geometry import affine_inverse, affine_inverse_np
21
 
@@ -82,6 +86,26 @@ def _poses_from_ext(ext_ref, ext_est):
82
 
83
 
84
  def _umeyama_sim3_from_paths(pose_ref, pose_est):
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
85
  path_ref = PosePath3D(poses_se3=pose_ref.copy())
86
  path_est = PosePath3D(poses_se3=pose_est.copy())
87
  r, t, s = path_est.align(path_ref, correct_scale=True)
 
15
  from typing import List
16
  import numpy as np
17
  import torch
18
+
19
+ try: # Optional dependency; HF Spaces may not have evo installed.
20
+ from evo.core.trajectory import PosePath3D
21
+ except Exception: # pragma: no cover
22
+ PosePath3D = None
23
 
24
  from depth_anything_3.utils.geometry import affine_inverse, affine_inverse_np
25
 
 
86
 
87
 
88
  def _umeyama_sim3_from_paths(pose_ref, pose_est):
89
+ if PosePath3D is None:
90
+ # Lightweight Umeyama Sim(3) without evo dependency
91
+ P = pose_ref[:, :3, 3]
92
+ Q = pose_est[:, :3, 3]
93
+ n = max(1, P.shape[0])
94
+ mu_P = P.mean(axis=0)
95
+ mu_Q = Q.mean(axis=0)
96
+ X = Q - mu_Q
97
+ Y = P - mu_P
98
+ cov = (X.T @ Y) / n
99
+ U, D, Vt = np.linalg.svd(cov)
100
+ S = np.eye(3)
101
+ if np.linalg.det(U @ Vt) < 0:
102
+ S[2, 2] = -1.0
103
+ R = U @ S @ Vt
104
+ var_Q = np.mean(np.sum(X * X, axis=1)) + 1e-8
105
+ s = float(np.trace(np.diag(D) @ S) / var_Q)
106
+ t = mu_P - s * (R @ mu_Q)
107
+ pose_est_aligned = _apply_sim3_to_poses(pose_est, R, t, s)
108
+ return R, t, s, pose_est_aligned
109
  path_ref = PosePath3D(poses_se3=pose_ref.copy())
110
  path_est = PosePath3D(poses_se3=pose_est.copy())
111
  r, t, s = path_est.align(path_ref, correct_scale=True)