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

test: add pytest suite for pose utilities

Browse Source 
Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-opencode)

Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>
This commit is contained in:
crosstyan
2026-02-04 11:55:38 +00:00
parent 69a3d1e8bf
commit d1e58245a6
5 changed files with 214 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
py_workspace/pyproject.toml
+5
View File
@@ -24,3 +24,8 @@ dependencies = [
[tool.uv.sources]
pyzed = { path = "libs/pyzed_pkg" }
[dependency-groups]
dev = [
"pytest>=9.0.2",
]
py_workspace/tests/conftest.py
+6
View File
@@ -0,0 +1,6 @@
import sys
import os
# The pytest console script does not always add the current working directory to sys.path.
# This ensures that the 'aruco' module and other local packages are importable during tests.
sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..")))
py_workspace/tests/test_pose_averaging.py
+85
View File
@@ -0,0 +1,85 @@
import numpy as np
import pytest
from aruco.pose_averaging import PoseAccumulator
from aruco.pose_math import rvec_tvec_to_matrix
def test_pose_accumulator_basic():
acc = PoseAccumulator()
T = np.eye(4)
acc.add_pose(T, 0.1, 0)
assert len(acc.poses) == 1
assert acc.reproj_errors[0] == 0.1
assert acc.frame_ids[0] == 0
def test_filter_by_reproj():
acc = PoseAccumulator()
acc.add_pose(np.eye(4), 0.1, 0)
acc.add_pose(np.eye(4), 0.5, 1)
acc.add_pose(np.eye(4), 1.0, 2)
indices = acc.filter_by_reproj(0.6)
assert indices == [0, 1]
def test_ransac_filter_translation_outlier():
acc = PoseAccumulator()
# Reference pose
T_ref = np.eye(4)
acc.add_pose(T_ref, 0.1, 0)
# Inlier (small shift)
T_inlier = np.eye(4)
T_inlier[0, 3] = 0.01
acc.add_pose(T_inlier, 0.1, 1)
# Outlier (large shift)
T_outlier = np.eye(4)
T_outlier[0, 3] = 1.0
acc.add_pose(T_outlier, 0.1, 2)
inliers = acc.ransac_filter(trans_thresh_m=0.1)
assert 0 in inliers
assert 1 in inliers
assert 2 not in inliers
def test_compute_robust_mean():
acc = PoseAccumulator()
# Two identical poses
T = np.eye(4)
T[0, 3] = 1.0
acc.add_pose(T, 0.1, 0)
acc.add_pose(T, 0.1, 1)
T_mean, stats = acc.compute_robust_mean()
np.testing.assert_allclose(T_mean, T, atol=1e-10)
assert stats["n_inliers"] == 2
assert stats["median_reproj_error"] == 0.1
def test_compute_robust_mean_with_outlier():
acc = PoseAccumulator()
T1 = np.eye(4)
T1[0, 3] = 1.0
T2 = np.eye(4)
T2[0, 3] = 1.1
T_outlier = np.eye(4)
T_outlier[0, 3] = 10.0
acc.add_pose(T1, 0.1, 0)
acc.add_pose(T2, 0.2, 1)
acc.add_pose(T_outlier, 0.5, 2)
# Filter out the outlier manually or via RANSAC
inliers = acc.ransac_filter(trans_thresh_m=0.5)
T_mean, stats = acc.compute_robust_mean(inliers)
assert stats["n_inliers"] == 2
# Median of 1.0 and 1.1 is 1.05
assert abs(T_mean[0, 3] - 1.05) < 1e-10
py_workspace/tests/test_pose_math.py
+76
View File
@@ -0,0 +1,76 @@
import numpy as np
import cv2
import pytest
from aruco.pose_math import (
rvec_tvec_to_matrix,
matrix_to_rvec_tvec,
invert_transform,
compose_transforms,
compute_reprojection_error,
)
def test_rvec_tvec_roundtrip():
rvec = np.array([0.1, 0.2, 0.3])
tvec = np.array([1.0, 2.0, 3.0])
T = rvec_tvec_to_matrix(rvec, tvec)
rvec_out, tvec_out = matrix_to_rvec_tvec(T)
np.testing.assert_allclose(rvec, rvec_out, atol=1e-10)
np.testing.assert_allclose(tvec, tvec_out, atol=1e-10)
def test_invert_transform():
rvec = np.array([0.5, -0.2, 0.1])
tvec = np.array([10.0, -5.0, 2.0])
T = rvec_tvec_to_matrix(rvec, tvec)
T_inv = invert_transform(T)
I_test = T @ T_inv
np.testing.assert_allclose(I_test, np.eye(4), atol=1e-10)
def test_compose_transforms():
T1 = rvec_tvec_to_matrix(np.array([0.1, 0, 0]), np.array([1, 0, 0]))
T2 = rvec_tvec_to_matrix(np.array([0, 0.2, 0]), np.array([0, 2, 0]))
T_res = compose_transforms(T1, T2)
T_expected = T1 @ T2
np.testing.assert_allclose(T_res, T_expected, atol=1e-10)
def test_compute_reprojection_error_zero():
# Setup camera
K = np.array([[1000, 0, 640], [0, 1000, 360], [0, 0, 1]], dtype=np.float64)
dist = np.zeros(5)
# Setup pose
rvec = np.array([0.1, -0.2, 0.3])
tvec = np.array([0.0, 0.0, 2.0])
# Create object points
obj_pts = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [1, 1, 0]], dtype=np.float64)
# Project them to get "perfect" image points
img_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, K, dist)
# Error should be near zero
error = compute_reprojection_error(obj_pts, img_pts, rvec, tvec, K, dist)
assert error < 1e-10
def test_compute_reprojection_error_nonzero():
K = np.array([[1000, 0, 640], [0, 1000, 360], [0, 0, 1]], dtype=np.float64)
rvec = np.zeros(3, dtype=np.float64)
tvec = np.array([0, 0, 1], dtype=np.float64)
obj_pts = np.array([[0, 0, 0]], dtype=np.float64)
# Projected point should be at (640, 360)
# Let's provide an image point at (641, 360)
img_pts = np.array([[641, 360]], dtype=np.float64)
error = compute_reprojection_error(obj_pts, img_pts, rvec, tvec, K)
assert abs(error - 1.0) < 1e-10
py_workspace/uv.lock
Generated
+42
View File
@@ -492,6 +492,11 @@ dependencies = [
{ name = "pyzed" },
]
[package.dev-dependencies]
dev = [
{ name = "pytest" },
]
[package.metadata]
requires-dist = [
{ name = "awkward", specifier = ">=2.8.12" },
@@ -510,6 +515,9 @@ requires-dist = [
{ name = "pyzed", directory = "libs/pyzed_pkg" },
]
[package.metadata.requires-dev]
dev = [{ name = "pytest", specifier = ">=9.0.2" }]
[[package]]
name = "executing"
version = "2.2.1"
@@ -612,6 +620,15 @@ wheels = [
{ url = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/packages/0e/61/66938bbb5fc52dbdf84594873d5b51fb1f7c7794e9c0f5bd885f30bc507b/idna-3.11-py3-none-any.whl", hash = "sha256:771a87f49d9defaf64091e6e6fe9c18d4833f140bd19464795bc32d966ca37ea", size = 71008, upload-time = "2025-10-12T14:55:18.883Z" },
]
[[package]]
name = "iniconfig"
version = "2.3.0"
source = { registry = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/simple/" }
sdist = { url = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/packages/72/34/14ca021ce8e5dfedc35312d08ba8bf51fdd999c576889fc2c24cb97f4f10/iniconfig-2.3.0.tar.gz", hash = "sha256:c76315c77db068650d49c5b56314774a7804df16fee4402c1f19d6d15d8c4730", size = 20503, upload-time = "2025-10-18T21:55:43.219Z" }
wheels = [
{ url = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/packages/cb/b1/3846dd7f199d53cb17f49cba7e651e9ce294d8497c8c150530ed11865bb8/iniconfig-2.3.0-py3-none-any.whl", hash = "sha256:f631c04d2c48c52b84d0d0549c99ff3859c98df65b3101406327ecc7d53fbf12", size = 7484, upload-time = "2025-10-18T21:55:41.639Z" },
]
[[package]]
name = "ipykernel"
version = "7.1.0"
@@ -1289,6 +1306,15 @@ wheels = [
{ url = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/packages/cb/28/3bfe2fa5a7b9c46fe7e13c97bda14c895fb10fa2ebf1d0abb90e0cea7ee1/platformdirs-4.5.1-py3-none-any.whl", hash = "sha256:d03afa3963c806a9bed9d5125c8f4cb2fdaf74a55ab60e5d59b3fde758104d31", size = 18731, upload-time = "2025-12-05T13:52:56.823Z" },
]
[[package]]
name = "pluggy"
version = "1.6.0"
source = { registry = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/simple/" }
sdist = { url = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/packages/f9/e2/3e91f31a7d2b083fe6ef3fa267035b518369d9511ffab804f839851d2779/pluggy-1.6.0.tar.gz", hash = "sha256:7dcc130b76258d33b90f61b658791dede3486c3e6bfb003ee5c9bfb396dd22f3", size = 69412, upload-time = "2025-05-15T12:30:07.975Z" }
wheels = [
{ url = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/packages/54/20/4d324d65cc6d9205fabedc306948156824eb9f0ee1633355a8f7ec5c66bf/pluggy-1.6.0-py3-none-any.whl", hash = "sha256:e920276dd6813095e9377c0bc5566d94c932c33b27a3e3945d8389c374dd4746", size = 20538, upload-time = "2025-05-15T12:30:06.134Z" },
]
[[package]]
name = "prometheus-client"
version = "0.24.1"
@@ -1454,6 +1480,22 @@ wheels = [
{ url = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/packages/87/97/6312caecc721cf4e4fdac2a156dc57e403e3d9a17553d72e86653cbc92ef/pyopengl_accelerate-3.1.10-cp314-cp314t-win_amd64.whl", hash = "sha256:a2866cb65c45b013c2bf9995010824cc1b50ae91a4166746beb9ce241803e62a", size = 484776, upload-time = "2025-08-18T02:33:51.342Z" },
]
[[package]]
name = "pytest"
version = "9.0.2"
source = { registry = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/simple/" }
dependencies = [
{ name = "colorama", marker = "sys_platform == 'win32'" },
{ name = "iniconfig" },
{ name = "packaging" },
{ name = "pluggy" },
{ name = "pygments" },
]
sdist = { url = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/packages/d1/db/7ef3487e0fb0049ddb5ce41d3a49c235bf9ad299b6a25d5780a89f19230f/pytest-9.0.2.tar.gz", hash = "sha256:75186651a92bd89611d1d9fc20f0b4345fd827c41ccd5c299a868a05d70edf11", size = 1568901, upload-time = "2025-12-06T21:30:51.014Z" }
wheels = [
{ url = "https://mirrors.tuna.tsinghua.edu.cn/pypi/web/packages/3b/ab/b3226f0bd7cdcf710fbede2b3548584366da3b19b5021e74f5bde2a8fa3f/pytest-9.0.2-py3-none-any.whl", hash = "sha256:711ffd45bf766d5264d487b917733b453d917afd2b0ad65223959f59089f875b", size = 374801, upload-time = "2025-12-06T21:30:49.154Z" },
]
[[package]]
name = "python-dateutil"
version = "2.9.0.post0"