feat(perception): add camera ego-motion evidence

This commit is contained in:
DCCONSTRUCTIONS
2026-07-24 13:37:39 +03:00
parent 230cba4b21
commit 7fba39a629
11 changed files with 2348 additions and 0 deletions
@@ -13,6 +13,7 @@ from k1link.device_plugins.xgrids_k1.analyze.calibrated_projection import (
map_points_to_lidar,
project_map_points_kb4,
quaternion_xyzw_to_rotation_matrix,
unproject_pixels_kb4,
)
from k1link.device_plugins.xgrids_k1.calibration_schema import (
parse_k1_factory_calibration,
@@ -68,6 +69,19 @@ def test_kb4_projection_uses_theta_polynomial_and_rejects_behind_camera() -> Non
assert projected.source_indices.tolist() == [0, 1]
def test_kb4_unprojection_round_trips_camera_directions() -> None:
profile = _identity_profile()
directions = unproject_pixels_kb4(
[[400.0, 300.0], [400.0 + 100.0 * math.pi / 4.0, 300.0]],
profile=profile,
)
np.testing.assert_allclose(
directions,
[[0.0, 0.0, 1.0], [math.sqrt(0.5), 0.0, math.sqrt(0.5)]],
atol=1e-12,
)
def test_factory_profile_binds_right_main_camera_and_scales_intrinsics() -> None:
calibration = parse_k1_factory_calibration(
(FIXTURES / "camera.yaml").read_bytes(),