fix: resolve basedpyright errors in aruco and tests
This commit is contained in:
@@ -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)
|
||||||
|
|
||||||
|
|||||||
@@ -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",
|
||||||
|
]
|
||||||
|
|
||||||
|
|||||||
@@ -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"
|
||||||
)
|
)
|
||||||
|
|||||||
Reference in New Issue
Block a user