test: add pytest suite for pose utilities

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

Co-authored-by: Sisyphus <clio-agent@sisyphuslabs.ai>

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__), "..")))

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

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