feat(perception): add camera ego-motion evidence
This commit is contained in:
@@ -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(),
|
||||
|
||||
Reference in New Issue
Block a user