diff --git a/src/k1link/compute/lidar_local_surface_geometry.py b/src/k1link/compute/lidar_local_surface_geometry.py index 1ce166b..5e12291 100644 --- a/src/k1link/compute/lidar_local_surface_geometry.py +++ b/src/k1link/compute/lidar_local_surface_geometry.py @@ -166,6 +166,16 @@ def update_cache( profile: K1LocalSurfaceProfile, ) -> None: keys, points = cloud_cell_observations(cloud, profile) + update_cache_observations(cache, keys, points, session_seconds) + + +def update_cache_observations( + cache: dict[tuple[int, int], tuple[float, float]], + keys: npt.NDArray[np.int64], + points: npt.NDArray[np.float64], + session_seconds: float, +) -> None: + """Update a surface cache from already aggregated cell observations.""" for key, point in zip(keys, points, strict=True): cache[(int(key[0]), int(key[1]))] = (float(point[2]), session_seconds) diff --git a/src/k1link/compute/lidar_local_surface_shadow.py b/src/k1link/compute/lidar_local_surface_shadow.py index dedf325..88fe0ba 100644 --- a/src/k1link/compute/lidar_local_surface_shadow.py +++ b/src/k1link/compute/lidar_local_surface_shadow.py @@ -46,7 +46,7 @@ from .lidar_local_surface_geometry import ( step_candidate_keys as _step_candidate_keys, ) from .lidar_local_surface_geometry import ( - update_cache as _update_cache, + update_cache_observations as _update_cache_observations, ) from .live_perception import LatestWinsQueue @@ -489,7 +489,7 @@ class K1LocalSurfaceShadowEstimator: position, self.profile, ) - _, current_cell_points = _cloud_cell_observations( + current_cell_keys, current_cell_points = _cloud_cell_observations( local_cloud, self.profile, ) @@ -499,11 +499,11 @@ class K1LocalSurfaceShadowEstimator: position, self.profile, ) - _update_cache( + _update_cache_observations( self._cache, - local_cloud, + current_cell_keys, + current_cell_points, value.session_seconds, - self.profile, ) cell_keys, cell_points, cell_times = _local_cache_records( self._cache, diff --git a/tests/test_lidar_local_surface.py b/tests/test_lidar_local_surface.py index 4186733..c38eed0 100644 --- a/tests/test_lidar_local_surface.py +++ b/tests/test_lidar_local_surface.py @@ -26,6 +26,11 @@ from k1link.compute.lidar_local_surface import ( from k1link.compute.lidar_local_surface_geometry import ( DEFAULT_K1_LOCAL_SURFACE_PROFILE as WORKER_LOCAL_SURFACE_PROFILE, ) +from k1link.compute.lidar_local_surface_geometry import ( + cloud_cell_observations, + update_cache, + update_cache_observations, +) from k1link.web.lidar_api import build_lidar_router from k1link.web.lidar_local_surface_service import K1LocalSurfaceReadService @@ -48,6 +53,26 @@ def test_replay_and_minimal_worker_pin_the_same_local_surface_profile() -> None: assert WORKER_LOCAL_SURFACE_PROFILE.to_dict() == REPLAY_LOCAL_SURFACE_PROFILE.to_dict() +def test_precomputed_local_surface_observations_preserve_cache_update() -> None: + cloud = np.asarray( + [ + [0.1, 0.1, 0.8], + [0.2, 0.2, 0.2], + [1.1, 0.1, 0.5], + [-0.2, -0.2, -0.1], + ], + dtype=np.float64, + ) + baseline: dict[tuple[int, int], tuple[float, float]] = {} + optimized: dict[tuple[int, int], tuple[float, float]] = {} + + update_cache(baseline, cloud, 4.25, WORKER_LOCAL_SURFACE_PROFILE) + keys, points = cloud_cell_observations(cloud, WORKER_LOCAL_SURFACE_PROFILE) + update_cache_observations(optimized, keys, points, 4.25) + + assert optimized == baseline + + def _source_pack(root: Path) -> Path: frame_count = 8 available = np.asarray([True, True, True, True, True, True, True, False])