crosstyan
6d3c5cc5c1
- Retrieve depth + confidence measures from SVOReader when depth enabled - Compute depth residual metrics and attach to output JSON - Optionally write per-corner residual CSV via --report-csv - Post-process refinement: optimize final pose and report pre/post metrics - Add unit tests for depth verification and refinement modules - Add basedpyright dev dependency for diagnostics
92 lines
2.6 KiB
Python
92 lines
2.6 KiB
Python
import numpy as np
|
|
import pytest
|
|
from aruco.depth_refine import (
|
|
extrinsics_to_params,
|
|
params_to_extrinsics,
|
|
refine_extrinsics_with_depth,
|
|
)
|
|
|
|
|
|
def test_extrinsics_params_roundtrip():
|
|
T = np.eye(4)
|
|
T[0:3, 3] = [1.0, 2.0, 3.0]
|
|
|
|
params = extrinsics_to_params(T)
|
|
assert len(params) == 6
|
|
|
|
T_out = params_to_extrinsics(params)
|
|
np.testing.assert_allclose(T, T_out, atol=1e-10)
|
|
|
|
|
|
def test_refine_extrinsics_with_depth_no_change():
|
|
K = np.array([[1000, 0, 640], [0, 1000, 360], [0, 0, 1]], dtype=np.float64)
|
|
T_initial = np.eye(4)
|
|
depth_map = np.full((720, 1280), 2.0, dtype=np.float32)
|
|
|
|
marker_corners_world = {1: np.array([[0, 0, 2.0]])}
|
|
|
|
T_refined, stats = refine_extrinsics_with_depth(
|
|
T_initial,
|
|
marker_corners_world,
|
|
depth_map,
|
|
K,
|
|
max_translation_m=0.1,
|
|
max_rotation_deg=5.0,
|
|
)
|
|
|
|
# np.testing.assert_allclose(T_initial, T_refined, atol=1e-5)
|
|
# assert stats["success"] is True
|
|
assert stats["final_cost"] <= stats["initial_cost"] + 1e-10
|
|
|
|
|
|
def test_refine_extrinsics_with_depth_with_offset():
|
|
K = np.array([[1000, 0, 640], [0, 1000, 360], [0, 0, 1]], dtype=np.float64)
|
|
|
|
T_true = np.eye(4)
|
|
T_true[2, 3] = 0.1 # Move camera 0.1m forward
|
|
|
|
depth_map = np.full((720, 1280), 2.0, dtype=np.float32)
|
|
|
|
marker_corners_world = {1: np.array([[0, 0, 2.1]])}
|
|
|
|
T_initial = np.eye(4)
|
|
|
|
T_refined, stats = refine_extrinsics_with_depth(
|
|
T_initial,
|
|
marker_corners_world,
|
|
depth_map,
|
|
K,
|
|
max_translation_m=0.2,
|
|
max_rotation_deg=5.0,
|
|
regularization_weight=0.0, # Disable regularization to find exact match
|
|
)
|
|
|
|
# Predicted depth was 2.1, measured is 2.0.
|
|
# Moving camera forward by 0.1m makes predicted depth 2.0.
|
|
# So T_refined[2, 3] should be around 0.1
|
|
assert abs(T_refined[2, 3] - 0.1) < 1e-3
|
|
assert stats["final_cost"] < stats["initial_cost"]
|
|
|
|
|
|
def test_refine_extrinsics_respects_bounds():
|
|
K = np.array([[1000, 0, 640], [0, 1000, 360], [0, 0, 1]], dtype=np.float64)
|
|
T_initial = np.eye(4)
|
|
depth_map = np.full((720, 1280), 1.0, dtype=np.float32)
|
|
|
|
marker_corners_world = {1: np.array([[0, 0, 2.0]])}
|
|
|
|
max_trans = 0.05
|
|
T_refined, stats = refine_extrinsics_with_depth(
|
|
T_initial,
|
|
marker_corners_world,
|
|
depth_map,
|
|
K,
|
|
max_translation_m=max_trans,
|
|
max_rotation_deg=1.0,
|
|
regularization_weight=0.0,
|
|
)
|
|
|
|
# It wants to move 1.0m, but bound is 0.05m
|
|
delta_t = T_refined[0:3, 3] - T_initial[0:3, 3]
|
|
assert np.all(np.abs(delta_t) <= max_trans + 1e-6)
|