From d1e58245a6ceaaa55e5894a83451076bf8fcdc5e Mon Sep 17 00:00:00 2001 From: crosstyan Date: Wed, 4 Feb 2026 11:55:38 +0000 Subject: [PATCH] test: add pytest suite for pose utilities Ultraworked with [Sisyphus](https://github.com/code-yeongyu/oh-my-opencode) Co-authored-by: Sisyphus --- py_workspace/pyproject.toml | 5 ++ py_workspace/tests/conftest.py | 6 ++ py_workspace/tests/test_pose_averaging.py | 85 +++++++++++++++++++++++ py_workspace/tests/test_pose_math.py | 76 ++++++++++++++++++++ py_workspace/uv.lock | 42 +++++++++++ 5 files changed, 214 insertions(+) create mode 100644 py_workspace/tests/conftest.py create mode 100644 py_workspace/tests/test_pose_averaging.py create mode 100644 py_workspace/tests/test_pose_math.py diff --git a/py_workspace/pyproject.toml b/py_workspace/pyproject.toml index d56bda8..c896353 100644 --- a/py_workspace/pyproject.toml +++ b/py_workspace/pyproject.toml @@ -24,3 +24,8 @@ dependencies = [ [tool.uv.sources] pyzed = { path = "libs/pyzed_pkg" } +[dependency-groups] +dev = [ + "pytest>=9.0.2", +] + diff --git a/py_workspace/tests/conftest.py b/py_workspace/tests/conftest.py new file mode 100644 index 0000000..2545043 --- /dev/null +++ b/py_workspace/tests/conftest.py @@ -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__), ".."))) diff --git a/py_workspace/tests/test_pose_averaging.py b/py_workspace/tests/test_pose_averaging.py new file mode 100644 index 0000000..d10580a --- /dev/null +++ b/py_workspace/tests/test_pose_averaging.py @@ -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 diff --git a/py_workspace/tests/test_pose_math.py b/py_workspace/tests/test_pose_math.py new file mode 100644 index 0000000..b3484c2 --- /dev/null +++ b/py_workspace/tests/test_pose_math.py @@ -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 diff --git a/py_workspace/uv.lock b/py_workspace/uv.lock index ee8340a..3b1d116 100644 --- a/py_workspace/uv.lock +++ b/py_workspace/uv.lock @@ -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"