112 lines
3.6 KiB
Python
112 lines
3.6 KiB
Python
from __future__ import annotations
|
|
|
|
from pathlib import Path
|
|
|
|
import pytest
|
|
|
|
from k1link.device_plugins.xgrids_k1.calibration_schema import (
|
|
FactoryCalibrationSchemaError,
|
|
parse_k1_factory_calibration,
|
|
)
|
|
|
|
FIXTURE_ROOT = Path(__file__).parent / "fixtures" / "k1" / "calibration"
|
|
|
|
|
|
def _documents() -> tuple[bytes, bytes]:
|
|
return (
|
|
(FIXTURE_ROOT / "camera.yaml").read_bytes(),
|
|
(FIXTURE_ROOT / "extrinsic_camera_lidar.yaml").read_bytes(),
|
|
)
|
|
|
|
|
|
def test_factory_calibration_normalizes_main_camera_mapping_and_transforms() -> None:
|
|
calibration = parse_k1_factory_calibration(*_documents())
|
|
profile = calibration.normalized_profile()
|
|
streams = profile["stream_bindings"]
|
|
assert isinstance(streams, dict)
|
|
|
|
left = streams["sensor.camera.left"]
|
|
right = streams["sensor.camera.right"]
|
|
assert isinstance(left, dict)
|
|
assert isinstance(right, dict)
|
|
assert left["calibration_slot"] == "camera_0"
|
|
assert right["calibration_slot"] == "camera_1"
|
|
assert left["rtsp_path"] == "/live/chn_left_main"
|
|
assert right["rtsp_path"] == "/live/chn_right_main"
|
|
assert left["native_resolution"] == [4000, 3000]
|
|
assert left["admitted_resolution"] == [800, 600]
|
|
assert left["admitted_intrinsic_fx_fy_cx_cy"] == pytest.approx(
|
|
[193.6, 193.6, 400.0, 300.0]
|
|
)
|
|
assert right["admitted_intrinsic_fx_fy_cx_cy"] == pytest.approx(
|
|
[194.0, 194.2, 396.0, 302.0]
|
|
)
|
|
assert profile["transform_notation"] == "T_destination_from_source"
|
|
|
|
right_transform = calibration.t_camera_from_lidar("camera_1")
|
|
assert right_transform[0] == pytest.approx((-1.0, 0.0, 0.0, 0.007))
|
|
assert right_transform[1] == pytest.approx((0.0, 0.0, -1.0, -0.0948))
|
|
assert right_transform[2] == pytest.approx((0.0, -1.0, 0.0, -0.0328))
|
|
|
|
|
|
@pytest.mark.parametrize(
|
|
("mutation", "message"),
|
|
[
|
|
(
|
|
lambda data: data.replace(
|
|
b"calibrated: true\n",
|
|
b"calibrated: true\ncalibrated: true\n",
|
|
1,
|
|
),
|
|
"safe valid YAML",
|
|
),
|
|
(
|
|
lambda data: data.replace(
|
|
b"camera_pose: [1, 0, 0, 0,",
|
|
b"camera_pose: &pose [1, 0, 0, 0,",
|
|
1,
|
|
),
|
|
"anchors",
|
|
),
|
|
(
|
|
lambda data: data.replace(b"intrinsic: [968,", b"intrinsic: [.nan,", 1),
|
|
"finite",
|
|
),
|
|
(
|
|
lambda data: data.replace(b"image_width: 4000", b"image_width: 3999", 1),
|
|
"resolution",
|
|
),
|
|
(
|
|
lambda data: data.replace(
|
|
b"camera_model: kb4",
|
|
b"camera_model: kb4\n unexpected: true",
|
|
1,
|
|
),
|
|
"unexpected key set",
|
|
),
|
|
],
|
|
)
|
|
def test_factory_calibration_rejects_unsafe_or_nonprofile_camera_yaml(
|
|
mutation: object,
|
|
message: str,
|
|
) -> None:
|
|
camera, extrinsic = _documents()
|
|
mutate = mutation
|
|
assert callable(mutate)
|
|
with pytest.raises(FactoryCalibrationSchemaError, match=message):
|
|
parse_k1_factory_calibration(mutate(camera), extrinsic)
|
|
|
|
|
|
def test_factory_calibration_rejects_version_or_rigid_transform_mismatch() -> None:
|
|
camera, extrinsic = _documents()
|
|
mismatched = extrinsic.replace(b"V2.2.0_alpha", b"V2.2.1_alpha")
|
|
with pytest.raises(FactoryCalibrationSchemaError, match="versions do not match"):
|
|
parse_k1_factory_calibration(camera, mismatched)
|
|
|
|
invalid_rotation = extrinsic.replace(
|
|
b"transform: [1, 0, 0,",
|
|
b"transform: [2, 0, 0,",
|
|
)
|
|
with pytest.raises(FactoryCalibrationSchemaError, match="not orthonormal"):
|
|
parse_k1_factory_calibration(camera, invalid_rotation)
|