crosstyan/zed-playground
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: resolve basedpyright errors in aruco and tests

Browse Source
This commit is contained in:
crosstyan
2026-02-07 09:00:07 +00:00
parent 4fc8de4bdc
commit 8f6aee7f22
4 changed files with 59 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
py_workspace/aruco/pose_averaging.py
+29 -28
View File
@@ -2,11 +2,12 @@ import numpy as np
from typing import List, Dict, Tuple, Optional, Any
try:
from scipy.spatial.transform import Rotation as R
from scipy.spatial.transform import Rotation as R_scipy
HAS_SCIPY = True
has_scipy = True
except ImportError:
HAS_SCIPY = False
has_scipy = False
R_scipy = None # type: ignore
def rotation_angle_deg(Ra: np.ndarray, Rb: np.ndarray) -> float:
@@ -34,29 +35,29 @@ def matrix_to_quaternion(R_mat: np.ndarray) -> np.ndarray:
"""
tr = np.trace(R_mat)
if tr > 0:
S = np.sqrt(tr + 1.0) * 2
qw = 0.25 * S
qx = (R_mat[2, 1] - R_mat[1, 2]) / S
qy = (R_mat[0, 2] - R_mat[2, 0]) / S
qz = (R_mat[1, 0] - R_mat[0, 1]) / S
s = np.sqrt(tr + 1.0) * 2
qw = 0.25 * s
qx = (R_mat[2, 1] - R_mat[1, 2]) / s
qy = (R_mat[0, 2] - R_mat[2, 0]) / s
qz = (R_mat[1, 0] - R_mat[0, 1]) / s
elif (R_mat[0, 0] > R_mat[1, 1]) and (R_mat[0, 0] > R_mat[2, 2]):
S = np.sqrt(1.0 + R_mat[0, 0] - R_mat[1, 1] - R_mat[2, 2]) * 2
qw = (R_mat[2, 1] - R_mat[1, 2]) / S
qx = 0.25 * S
qy = (R_mat[0, 1] + R_mat[1, 0]) / S
qz = (R_mat[0, 2] + R_mat[2, 0]) / S
s = np.sqrt(1.0 + R_mat[0, 0] - R_mat[1, 1] - R_mat[2, 2]) * 2
qw = (R_mat[2, 1] - R_mat[1, 2]) / s
qx = 0.25 * s
qy = (R_mat[0, 1] + R_mat[1, 0]) / s
qz = (R_mat[0, 2] + R_mat[2, 0]) / s
elif R_mat[1, 1] > R_mat[2, 2]:
S = np.sqrt(1.0 + R_mat[1, 1] - R_mat[0, 0] - R_mat[2, 2]) * 2
qw = (R_mat[0, 2] - R_mat[2, 0]) / S
qx = (R_mat[0, 1] + R_mat[1, 0]) / S
qy = 0.25 * S
qz = (R_mat[1, 2] + R_mat[2, 1]) / S
s = np.sqrt(1.0 + R_mat[1, 1] - R_mat[0, 0] - R_mat[2, 2]) * 2
qw = (R_mat[0, 2] - R_mat[2, 0]) / s
qx = (R_mat[0, 1] + R_mat[1, 0]) / s
qy = 0.25 * s
qz = (R_mat[1, 2] + R_mat[2, 1]) / s
else:
S = np.sqrt(1.0 + R_mat[2, 2] - R_mat[0, 0] - R_mat[1, 1]) * 2
qw = (R_mat[1, 0] - R_mat[0, 1]) / S
qx = (R_mat[0, 2] + R_mat[2, 0]) / S
qy = (R_mat[1, 2] + R_mat[2, 1]) / S
qz = 0.25 * S
s = np.sqrt(1.0 + R_mat[2, 2] - R_mat[0, 0] - R_mat[1, 1]) * 2
qw = (R_mat[1, 0] - R_mat[0, 1]) / s
qx = (R_mat[0, 2] + R_mat[2, 0]) / s
qy = (R_mat[1, 2] + R_mat[2, 1]) / s
qz = 0.25 * s
return np.array([qw, qx, qy, qz])
@@ -211,19 +212,19 @@ class PoseAccumulator:
# Rotation: mean
rotations = [T[:3, :3] for T in inlier_poses]
if HAS_SCIPY:
r_objs = R.from_matrix(rotations)
if has_scipy and R_scipy is not None:
r_objs = R_scipy.from_matrix(rotations)
R_mean = r_objs.mean().as_matrix()
else:
# Quaternion eigen mean fallback
quats = np.array([matrix_to_quaternion(rot) for rot in rotations])
# Form the matrix M = sum(q_i * q_i^T)
M = np.zeros((4, 4))
M_mat = np.zeros((4, 4))
for i in range(n_inliers):
q = quats[i]
M += np.outer(q, q)
M_mat += np.outer(q, q)
# The average quaternion is the eigenvector corresponding to the largest eigenvalue
evals, evecs = np.linalg.eigh(M)
evals, evecs = np.linalg.eigh(M_mat)
q_mean = evecs[:, -1]
R_mean = quaternion_to_matrix(q_mean)