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)
py_workspace/pyproject.toml
+11
View File
@@ -21,6 +21,7 @@ dependencies = [
"pandas>=3.0.0",
"scipy>=1.17.0",
"typeguard>=4.4.4",
"matplotlib>=3.10.8",
]
[tool.uv.sources]
@@ -29,6 +30,7 @@ pyzed = { path = "libs/pyzed_pkg" }
[dependency-groups]
dev = [
"basedpyright>=1.37.4",
"pylint>=4.0.4",
"pytest>=9.0.2",
]
@@ -36,3 +38,12 @@ dev = [
testpaths = ["tests"]
norecursedirs = ["loguru", "tmp", "libs"]
[tool.basedpyright]
exclude = [
"libs",
"loguru",
"tmp",
".venv",
"ogl_viewer",
]
py_workspace/tests/test_alignment.py
+6 -5
View File
@@ -18,14 +18,14 @@ def test_compute_face_normal_valid_quad():
# normal = [1, 0, 0] x [0, 1, 0] = [0, 0, 1]
normal = compute_face_normal(corners)
np.testing.assert_allclose(normal, [0, 0, 1], atol=1e-10)
np.testing.assert_allclose(normal, np.array([0, 0, 1]), atol=1e-10)
def test_compute_face_normal_valid_triangle():
corners = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
normal = compute_face_normal(corners)
np.testing.assert_allclose(normal, [0, 0, 1], atol=1e-10)
np.testing.assert_allclose(normal, np.array([0, 0, 1]), atol=1e-10)
def test_compute_face_normal_degenerate():
@@ -104,7 +104,8 @@ def test_get_face_normal_from_geometry():
}
normal = get_face_normal_from_geometry("top", marker_geometry, face_marker_map)
np.testing.assert_allclose(normal, [0, 0, 1], atol=1e-10)
assert normal is not None
np.testing.assert_allclose(normal, np.array([0, 0, 1]), atol=1e-10)
# Missing face map
assert get_face_normal_from_geometry("top", marker_geometry, None) is None
@@ -145,14 +146,14 @@ def test_detect_ground_face():
assert res is not None
face_name, normal = res
assert face_name == "bottom"
np.testing.assert_allclose(normal, [0, -1, 0], atol=1e-10)
np.testing.assert_allclose(normal, np.array([0, -1, 0]), atol=1e-10)
# Only top visible
res = detect_ground_face({2}, marker_geometry, camera_up, face_marker_map)
assert res is not None
face_name, normal = res
assert face_name == "top"
np.testing.assert_allclose(normal, [0, 1, 0], atol=1e-10)
np.testing.assert_allclose(normal, np.array([0, 1, 0]), atol=1e-10)
# None visible
assert (
py_workspace/tests/test_depth_pool_integration.py
+13 -13
View File
@@ -64,10 +64,10 @@ def test_pool_size_1_equivalence(mock_dependencies):
# Run with pool_size=1
apply_depth_verify_refine_postprocess(
results=results,
verification_frames=verification_frames,
results=results, # type: ignore
verification_frames=verification_frames, # pyright: ignore
marker_geometry=marker_geometry,
camera_matrices=camera_matrices,
camera_matrices=camera_matrices, # pyright: ignore
verify_depth=True,
refine_depth=False,
use_confidence_weights=False,
@@ -122,10 +122,10 @@ def test_pool_size_5_integration(mock_dependencies):
# Run with pool_size=3
apply_depth_verify_refine_postprocess(
results=results,
verification_frames=verification_frames,
results=results, # type: ignore
verification_frames=verification_frames, # pyright: ignore
marker_geometry=marker_geometry,
camera_matrices=camera_matrices,
camera_matrices=camera_matrices, # pyright: ignore
verify_depth=True,
refine_depth=False,
use_confidence_weights=False,
@@ -148,8 +148,8 @@ def test_pool_size_5_integration(mock_dependencies):
# Verify metadata was added
assert "depth_pool" in results[serial]
assert results[serial]["depth_pool"]["pooled"] is True
assert results[serial]["depth_pool"]["pool_size_actual"] == 3
assert results[serial]["depth_pool"]["pooled"] is True # pyright: ignore
assert results[serial]["depth_pool"]["pool_size_actual"] == 3 # pyright: ignore
def test_pool_fallback_insufficient_valid(mock_dependencies):
@@ -222,10 +222,10 @@ def test_pool_fallback_insufficient_valid(mock_dependencies):
camera_matrices = {serial: np.eye(3)}
apply_depth_verify_refine_postprocess(
results=results,
verification_frames=verification_frames,
results=results, # type: ignore
verification_frames=verification_frames, # pyright: ignore
marker_geometry=marker_geometry,
camera_matrices=camera_matrices,
camera_matrices=camera_matrices, # pyright: ignore
verify_depth=True,
refine_depth=False,
use_confidence_weights=False,
@@ -246,8 +246,8 @@ def test_pool_fallback_insufficient_valid(mock_dependencies):
assert passed_depth_map is d1
# Verify metadata
assert results[serial]["depth_pool"]["pooled"] is False
assert results[serial]["depth_pool"]["pooled"] is False # pyright: ignore
assert (
results[serial]["depth_pool"]["fallback_reason"]
results[serial]["depth_pool"]["fallback_reason"] # pyright: ignore
== "insufficient_valid_points"
)