Spaces:
Runtime error
Runtime error
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 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
| 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)
|