fix: resolve basedpyright errors in aruco and tests

This commit is contained in:
2026-02-07 09:00:07 +00:00
parent 4fc8de4bdc
commit 8f6aee7f22
4 changed files with 59 additions and 46 deletions
+29 -28
View File
@@ -2,11 +2,12 @@ import numpy as np
from typing import List, Dict, Tuple, Optional, Any from typing import List, Dict, Tuple, Optional, Any
try: 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: except ImportError:
HAS_SCIPY = False has_scipy = False
R_scipy = None # type: ignore
def rotation_angle_deg(Ra: np.ndarray, Rb: np.ndarray) -> float: 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) tr = np.trace(R_mat)
if tr > 0: if tr > 0:
S = np.sqrt(tr + 1.0) * 2 s = np.sqrt(tr + 1.0) * 2
qw = 0.25 * S qw = 0.25 * s
qx = (R_mat[2, 1] - R_mat[1, 2]) / S qx = (R_mat[2, 1] - R_mat[1, 2]) / s
qy = (R_mat[0, 2] - R_mat[2, 0]) / S qy = (R_mat[0, 2] - R_mat[2, 0]) / s
qz = (R_mat[1, 0] - R_mat[0, 1]) / 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]): 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 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 qw = (R_mat[2, 1] - R_mat[1, 2]) / s
qx = 0.25 * S qx = 0.25 * s
qy = (R_mat[0, 1] + R_mat[1, 0]) / S qy = (R_mat[0, 1] + R_mat[1, 0]) / s
qz = (R_mat[0, 2] + R_mat[2, 0]) / S qz = (R_mat[0, 2] + R_mat[2, 0]) / s
elif R_mat[1, 1] > R_mat[2, 2]: 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 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 qw = (R_mat[0, 2] - R_mat[2, 0]) / s
qx = (R_mat[0, 1] + R_mat[1, 0]) / S qx = (R_mat[0, 1] + R_mat[1, 0]) / s
qy = 0.25 * S qy = 0.25 * s
qz = (R_mat[1, 2] + R_mat[2, 1]) / S qz = (R_mat[1, 2] + R_mat[2, 1]) / s
else: else:
S = np.sqrt(1.0 + R_mat[2, 2] - R_mat[0, 0] - R_mat[1, 1]) * 2 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 qw = (R_mat[1, 0] - R_mat[0, 1]) / s
qx = (R_mat[0, 2] + R_mat[2, 0]) / S qx = (R_mat[0, 2] + R_mat[2, 0]) / s
qy = (R_mat[1, 2] + R_mat[2, 1]) / S qy = (R_mat[1, 2] + R_mat[2, 1]) / s
qz = 0.25 * S qz = 0.25 * s
return np.array([qw, qx, qy, qz]) return np.array([qw, qx, qy, qz])
@@ -211,19 +212,19 @@ class PoseAccumulator:
# Rotation: mean # Rotation: mean
rotations = [T[:3, :3] for T in inlier_poses] rotations = [T[:3, :3] for T in inlier_poses]
if HAS_SCIPY: if has_scipy and R_scipy is not None:
r_objs = R.from_matrix(rotations) r_objs = R_scipy.from_matrix(rotations)
R_mean = r_objs.mean().as_matrix() R_mean = r_objs.mean().as_matrix()
else: else:
# Quaternion eigen mean fallback # Quaternion eigen mean fallback
quats = np.array([matrix_to_quaternion(rot) for rot in rotations]) quats = np.array([matrix_to_quaternion(rot) for rot in rotations])
# Form the matrix M = sum(q_i * q_i^T) # 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): for i in range(n_inliers):
q = quats[i] 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 # 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] q_mean = evecs[:, -1]
R_mean = quaternion_to_matrix(q_mean) R_mean = quaternion_to_matrix(q_mean)
+11
View File
@@ -21,6 +21,7 @@ dependencies = [
"pandas>=3.0.0", "pandas>=3.0.0",
"scipy>=1.17.0", "scipy>=1.17.0",
"typeguard>=4.4.4", "typeguard>=4.4.4",
"matplotlib>=3.10.8",
] ]
[tool.uv.sources] [tool.uv.sources]
@@ -29,6 +30,7 @@ pyzed = { path = "libs/pyzed_pkg" }
[dependency-groups] [dependency-groups]
dev = [ dev = [
"basedpyright>=1.37.4", "basedpyright>=1.37.4",
"pylint>=4.0.4",
"pytest>=9.0.2", "pytest>=9.0.2",
] ]
@@ -36,3 +38,12 @@ dev = [
testpaths = ["tests"] testpaths = ["tests"]
norecursedirs = ["loguru", "tmp", "libs"] norecursedirs = ["loguru", "tmp", "libs"]
[tool.basedpyright]
exclude = [
"libs",
"loguru",
"tmp",
".venv",
"ogl_viewer",
]
+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 = [1, 0, 0] x [0, 1, 0] = [0, 0, 1]
normal = compute_face_normal(corners) 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(): def test_compute_face_normal_valid_triangle():
corners = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64) corners = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float64)
normal = compute_face_normal(corners) 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(): 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) 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 # Missing face map
assert get_face_normal_from_geometry("top", marker_geometry, None) is None 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 assert res is not None
face_name, normal = res face_name, normal = res
assert face_name == "bottom" 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 # Only top visible
res = detect_ground_face({2}, marker_geometry, camera_up, face_marker_map) res = detect_ground_face({2}, marker_geometry, camera_up, face_marker_map)
assert res is not None assert res is not None
face_name, normal = res face_name, normal = res
assert face_name == "top" 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 # None visible
assert ( assert (
@@ -64,10 +64,10 @@ def test_pool_size_1_equivalence(mock_dependencies):
# Run with pool_size=1 # Run with pool_size=1
apply_depth_verify_refine_postprocess( apply_depth_verify_refine_postprocess(
results=results, results=results, # type: ignore
verification_frames=verification_frames, verification_frames=verification_frames, # pyright: ignore
marker_geometry=marker_geometry, marker_geometry=marker_geometry,
camera_matrices=camera_matrices, camera_matrices=camera_matrices, # pyright: ignore
verify_depth=True, verify_depth=True,
refine_depth=False, refine_depth=False,
use_confidence_weights=False, use_confidence_weights=False,
@@ -122,10 +122,10 @@ def test_pool_size_5_integration(mock_dependencies):
# Run with pool_size=3 # Run with pool_size=3
apply_depth_verify_refine_postprocess( apply_depth_verify_refine_postprocess(
results=results, results=results, # type: ignore
verification_frames=verification_frames, verification_frames=verification_frames, # pyright: ignore
marker_geometry=marker_geometry, marker_geometry=marker_geometry,
camera_matrices=camera_matrices, camera_matrices=camera_matrices, # pyright: ignore
verify_depth=True, verify_depth=True,
refine_depth=False, refine_depth=False,
use_confidence_weights=False, use_confidence_weights=False,
@@ -148,8 +148,8 @@ def test_pool_size_5_integration(mock_dependencies):
# Verify metadata was added # Verify metadata was added
assert "depth_pool" in results[serial] assert "depth_pool" in results[serial]
assert results[serial]["depth_pool"]["pooled"] is True assert results[serial]["depth_pool"]["pooled"] is True # pyright: ignore
assert results[serial]["depth_pool"]["pool_size_actual"] == 3 assert results[serial]["depth_pool"]["pool_size_actual"] == 3 # pyright: ignore
def test_pool_fallback_insufficient_valid(mock_dependencies): 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)} camera_matrices = {serial: np.eye(3)}
apply_depth_verify_refine_postprocess( apply_depth_verify_refine_postprocess(
results=results, results=results, # type: ignore
verification_frames=verification_frames, verification_frames=verification_frames, # pyright: ignore
marker_geometry=marker_geometry, marker_geometry=marker_geometry,
camera_matrices=camera_matrices, camera_matrices=camera_matrices, # pyright: ignore
verify_depth=True, verify_depth=True,
refine_depth=False, refine_depth=False,
use_confidence_weights=False, use_confidence_weights=False,
@@ -246,8 +246,8 @@ def test_pool_fallback_insufficient_valid(mock_dependencies):
assert passed_depth_map is d1 assert passed_depth_map is d1
# Verify metadata # Verify metadata
assert results[serial]["depth_pool"]["pooled"] is False assert results[serial]["depth_pool"]["pooled"] is False # pyright: ignore
assert ( assert (
results[serial]["depth_pool"]["fallback_reason"] results[serial]["depth_pool"]["fallback_reason"] # pyright: ignore
== "insufficient_valid_points" == "insufficient_valid_points"
) )