fix(perception): reconstruct rolling occupancy from K1 increments
This commit is contained in:
DCCONSTRUCTIONS
@@ -0,0 +1,443 @@
|
||||
"""Bounded rolling occupancy reconstructed from registered map increments.
|
||||
|
||||
The K1 ``lio_pcl`` recording is a sequence of post-LIO map increments, not a
|
||||
complete scan at every timestamp. This provider preserves the exact current
|
||||
increment elsewhere and materializes only the still-valid *retained* cells
|
||||
which a later increment did not need to publish again. Missing points never
|
||||
clear occupancy.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import hashlib
|
||||
import json
|
||||
import math
|
||||
from collections import deque
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Final, Protocol
|
||||
|
||||
from .contracts import GridCell, HistorySample, MotionState, TemporalObstacle, TemporalState
|
||||
from .providers import SourcePacket
|
||||
|
||||
ROLLING_MAP_PROFILE_SCHEMA: Final = "missioncore.rolling-local-map-profile/v1"
|
||||
ROLLING_MAP_PROVIDER_ID: Final = "rolling-local-obstacle-map/v1"
|
||||
DEFAULT_ROLLING_MAP_PROFILE_PATH: Final = Path(
|
||||
"config/perception/m4-rolling-local-map-v1.json"
|
||||
)
|
||||
|
||||
|
||||
class RollingMapError(RuntimeError):
|
||||
"""The rolling map input, bounds or state is incompatible."""
|
||||
|
||||
|
||||
class ReplayPoseResolver(Protocol):
|
||||
def pose_values_for_frame(
|
||||
self,
|
||||
frame_id: str,
|
||||
) -> tuple[tuple[float, float, float], tuple[float, float, float, float]] | None: ...
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class RollingMapBounds:
|
||||
coordinate_frame: str
|
||||
voxel_size_m: float
|
||||
retention_seconds: float
|
||||
local_radius_m: float
|
||||
maximum_cells: int
|
||||
maximum_cells_per_component: int
|
||||
maximum_components: int
|
||||
neighbor_radius_cells: int
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
numeric = (
|
||||
self.voxel_size_m,
|
||||
self.retention_seconds,
|
||||
self.local_radius_m,
|
||||
)
|
||||
if any(not _positive_finite(value) for value in numeric):
|
||||
raise RollingMapError("rolling map numeric bound is invalid")
|
||||
integer = (
|
||||
self.maximum_cells,
|
||||
self.maximum_cells_per_component,
|
||||
self.maximum_components,
|
||||
self.neighbor_radius_cells,
|
||||
)
|
||||
if any(not _positive_integer(value) for value in integer):
|
||||
raise RollingMapError("rolling map integer bound is invalid")
|
||||
if self.maximum_cells_per_component > self.maximum_cells:
|
||||
raise RollingMapError("rolling component bound exceeds map capacity")
|
||||
|
||||
@property
|
||||
def retention_ns(self) -> int:
|
||||
return round(self.retention_seconds * 1_000_000_000)
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class RollingMapProfile:
|
||||
profile_id: str
|
||||
source_id: str
|
||||
session_id: str
|
||||
representation_id: str
|
||||
bounds: RollingMapBounds
|
||||
profile_sha256: str
|
||||
|
||||
|
||||
@dataclass(frozen=True, slots=True)
|
||||
class RollingMapSnapshot:
|
||||
voxel_size_m: float
|
||||
retention_ns: int
|
||||
local_radius_m: float
|
||||
maximum_cells: int
|
||||
input_frames: int
|
||||
current_increment_cells: int
|
||||
retained_component_publications: int
|
||||
retained_cell_publications: int
|
||||
time_evicted_cells: int
|
||||
radius_evicted_cells: int
|
||||
capacity_evicted_cells: int
|
||||
active_cells_at_end: int
|
||||
peak_active_cells: int
|
||||
peak_retained_components: int
|
||||
maximum_retained_age_ns: int
|
||||
|
||||
|
||||
@dataclass(slots=True)
|
||||
class _CellEvidence:
|
||||
first_hit_ns: int
|
||||
last_hit_ns: int
|
||||
last_frame_id: str
|
||||
hit_count: int
|
||||
|
||||
|
||||
class RollingLocalObstacleMapProvider:
|
||||
"""Accumulate occupied map cells without inventing scan-based clearing."""
|
||||
|
||||
provider_id: str = ROLLING_MAP_PROVIDER_ID
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
pose_resolver: ReplayPoseResolver,
|
||||
profile: RollingMapProfile,
|
||||
) -> None:
|
||||
self.pose_resolver = pose_resolver
|
||||
self.profile = profile
|
||||
self.config = profile.bounds
|
||||
self._cells: dict[GridCell, _CellEvidence] = {}
|
||||
self._previous_sequence: int | None = None
|
||||
self._previous_time_ns: int | None = None
|
||||
self._input_frames = 0
|
||||
self._current_increment_cells = 0
|
||||
self._retained_component_publications = 0
|
||||
self._retained_cell_publications = 0
|
||||
self._time_evicted_cells = 0
|
||||
self._radius_evicted_cells = 0
|
||||
self._peak_active_cells = 0
|
||||
self._peak_retained_components = 0
|
||||
self._maximum_retained_age_ns = 0
|
||||
|
||||
def update(
|
||||
self,
|
||||
packet: SourcePacket,
|
||||
temporal_obstacles: tuple[TemporalObstacle, ...],
|
||||
) -> tuple[TemporalObstacle, ...]:
|
||||
self._validate(packet)
|
||||
now_ns = packet.envelope.timestamps.source_ns
|
||||
current_cells = {
|
||||
cell
|
||||
for obstacle in temporal_obstacles
|
||||
if obstacle.state is TemporalState.CURRENT
|
||||
for cell in obstacle.cells
|
||||
}
|
||||
self._current_increment_cells += len(current_cells)
|
||||
for cell in current_cells:
|
||||
evidence = self._cells.get(cell)
|
||||
if evidence is None:
|
||||
self._cells[cell] = _CellEvidence(
|
||||
first_hit_ns=now_ns,
|
||||
last_hit_ns=now_ns,
|
||||
last_frame_id=packet.envelope.frame_id,
|
||||
hit_count=1,
|
||||
)
|
||||
else:
|
||||
evidence.last_hit_ns = now_ns
|
||||
evidence.last_frame_id = packet.envelope.frame_id
|
||||
evidence.hit_count += 1
|
||||
|
||||
self._evict_by_time(now_ns)
|
||||
self._evict_by_radius(packet)
|
||||
if len(self._cells) > self.config.maximum_cells:
|
||||
raise RollingMapError(
|
||||
"rolling map capacity exceeded; dropping occupied cells is forbidden"
|
||||
)
|
||||
retained_cells = set(self._cells) - current_cells
|
||||
components = self._components(retained_cells)
|
||||
if len(components) > self.config.maximum_components:
|
||||
raise RollingMapError("rolling map component bound exceeded")
|
||||
result = tuple(self._contract(packet, cells) for cells in components)
|
||||
self._retained_component_publications += len(result)
|
||||
self._retained_cell_publications += sum(len(item.cells) for item in result)
|
||||
self._peak_active_cells = max(self._peak_active_cells, len(self._cells))
|
||||
self._peak_retained_components = max(
|
||||
self._peak_retained_components,
|
||||
len(result),
|
||||
)
|
||||
self._previous_sequence = packet.envelope.sequence
|
||||
self._previous_time_ns = now_ns
|
||||
self._input_frames += 1
|
||||
return result
|
||||
|
||||
def _validate(self, packet: SourcePacket) -> None:
|
||||
envelope = packet.envelope
|
||||
if (
|
||||
envelope.source_id != self.profile.source_id
|
||||
or envelope.session_id != self.profile.session_id
|
||||
or envelope.representation_id != self.profile.representation_id
|
||||
):
|
||||
raise RollingMapError("packet escaped the rolling map source profile")
|
||||
now_ns = envelope.timestamps.source_ns
|
||||
if self._previous_sequence is not None and (
|
||||
envelope.sequence <= self._previous_sequence
|
||||
or self._previous_time_ns is None
|
||||
or now_ns <= self._previous_time_ns
|
||||
):
|
||||
raise RollingMapError("rolling map packet order is not monotonic")
|
||||
|
||||
def _evict_by_time(self, now_ns: int) -> None:
|
||||
expired = tuple(
|
||||
cell
|
||||
for cell, evidence in self._cells.items()
|
||||
if now_ns - evidence.last_hit_ns > self.config.retention_ns
|
||||
)
|
||||
for cell in expired:
|
||||
del self._cells[cell]
|
||||
self._time_evicted_cells += len(expired)
|
||||
|
||||
def _evict_by_radius(self, packet: SourcePacket) -> None:
|
||||
pose = self.pose_resolver.pose_values_for_frame(packet.envelope.frame_id)
|
||||
if pose is None:
|
||||
return
|
||||
x, y, _ = pose[0]
|
||||
radius_squared = self.config.local_radius_m**2
|
||||
evicted = tuple(
|
||||
cell
|
||||
for cell in self._cells
|
||||
if (
|
||||
((cell.x + 0.5) * self.config.voxel_size_m - x) ** 2
|
||||
+ ((cell.y + 0.5) * self.config.voxel_size_m - y) ** 2
|
||||
> radius_squared
|
||||
)
|
||||
)
|
||||
for cell in evicted:
|
||||
del self._cells[cell]
|
||||
self._radius_evicted_cells += len(evicted)
|
||||
|
||||
def _components(self, cells: set[GridCell]) -> tuple[frozenset[GridCell], ...]:
|
||||
remaining = set(cells)
|
||||
components: list[frozenset[GridCell]] = []
|
||||
radius = self.config.neighbor_radius_cells
|
||||
offsets = tuple(
|
||||
(dx, dy, dz)
|
||||
for dx in range(-radius, radius + 1)
|
||||
for dy in range(-radius, radius + 1)
|
||||
for dz in range(-radius, radius + 1)
|
||||
if dx or dy or dz
|
||||
)
|
||||
while remaining:
|
||||
start = min(remaining, key=_cell_key)
|
||||
remaining.remove(start)
|
||||
connected = {start}
|
||||
queue = deque((start,))
|
||||
while queue:
|
||||
cell = queue.popleft()
|
||||
for dx, dy, dz in offsets:
|
||||
neighbor = GridCell(cell.x + dx, cell.y + dy, cell.z + dz)
|
||||
if neighbor not in remaining:
|
||||
continue
|
||||
remaining.remove(neighbor)
|
||||
connected.add(neighbor)
|
||||
queue.append(neighbor)
|
||||
if len(connected) > self.config.maximum_cells_per_component:
|
||||
raise RollingMapError("rolling map component cell bound exceeded")
|
||||
components.append(frozenset(connected))
|
||||
return tuple(sorted(components, key=lambda value: _cell_key(min(value, key=_cell_key))))
|
||||
|
||||
def _contract(
|
||||
self,
|
||||
packet: SourcePacket,
|
||||
cells: frozenset[GridCell],
|
||||
) -> TemporalObstacle:
|
||||
now_ns = packet.envelope.timestamps.source_ns
|
||||
last_hit_ns = max(self._cells[cell].last_hit_ns for cell in cells)
|
||||
age_ns = now_ns - last_hit_ns
|
||||
if not 0 < age_ns <= self.config.retention_ns:
|
||||
raise RollingMapError("retained component escaped rolling bounds")
|
||||
centers = tuple(
|
||||
(
|
||||
(cell.x + 0.5) * self.config.voxel_size_m,
|
||||
(cell.y + 0.5) * self.config.voxel_size_m,
|
||||
(cell.z + 0.5) * self.config.voxel_size_m,
|
||||
)
|
||||
for cell in cells
|
||||
)
|
||||
centroid = (
|
||||
sum(point[0] for point in centers) / len(centers),
|
||||
sum(point[1] for point in centers) / len(centers),
|
||||
sum(point[2] for point in centers) / len(centers),
|
||||
)
|
||||
digest = hashlib.sha256(
|
||||
";".join(
|
||||
f"{cell.x},{cell.y},{cell.z}"
|
||||
for cell in sorted(cells, key=_cell_key)
|
||||
).encode()
|
||||
).hexdigest()[:24]
|
||||
self._maximum_retained_age_ns = max(self._maximum_retained_age_ns, age_ns)
|
||||
latest_frame = min(
|
||||
evidence.last_frame_id
|
||||
for cell in cells
|
||||
if (evidence := self._cells[cell]).last_hit_ns == last_hit_ns
|
||||
)
|
||||
return TemporalObstacle(
|
||||
component_id=f"rolling-{digest}",
|
||||
identity_scope="ephemeral",
|
||||
state=TemporalState.RETAINED,
|
||||
ttl_ns=self.config.retention_ns,
|
||||
last_hit_ns=last_hit_ns,
|
||||
age_ns=age_ns,
|
||||
association_basis="registered-map-increment-retention",
|
||||
history=(
|
||||
HistorySample(
|
||||
frame_id=latest_frame,
|
||||
evidence_time_ns=last_hit_ns,
|
||||
centroid_xyz_m=centroid,
|
||||
),
|
||||
),
|
||||
cells=tuple(sorted(cells, key=_cell_key)),
|
||||
coordinate_frame=self.config.coordinate_frame,
|
||||
last_centroid_xyz_m=centroid,
|
||||
motion=MotionState.UNKNOWN,
|
||||
motion_confidence=0.0,
|
||||
motion_reason="retained-map-increment-no-current-motion",
|
||||
semantic_hint=None,
|
||||
)
|
||||
|
||||
def snapshot(self) -> RollingMapSnapshot:
|
||||
return RollingMapSnapshot(
|
||||
voxel_size_m=self.config.voxel_size_m,
|
||||
retention_ns=self.config.retention_ns,
|
||||
local_radius_m=self.config.local_radius_m,
|
||||
maximum_cells=self.config.maximum_cells,
|
||||
input_frames=self._input_frames,
|
||||
current_increment_cells=self._current_increment_cells,
|
||||
retained_component_publications=self._retained_component_publications,
|
||||
retained_cell_publications=self._retained_cell_publications,
|
||||
time_evicted_cells=self._time_evicted_cells,
|
||||
radius_evicted_cells=self._radius_evicted_cells,
|
||||
capacity_evicted_cells=0,
|
||||
active_cells_at_end=len(self._cells),
|
||||
peak_active_cells=self._peak_active_cells,
|
||||
peak_retained_components=self._peak_retained_components,
|
||||
maximum_retained_age_ns=self._maximum_retained_age_ns,
|
||||
)
|
||||
|
||||
|
||||
def load_rolling_map_profile(path: Path) -> RollingMapProfile:
|
||||
resolved = path.resolve(strict=True)
|
||||
if resolved.is_symlink() or not resolved.is_file():
|
||||
raise RollingMapError("rolling map profile is not a regular file")
|
||||
raw = resolved.read_bytes()
|
||||
try:
|
||||
document = _object(json.loads(raw), "rolling map profile")
|
||||
except json.JSONDecodeError as exc:
|
||||
raise RollingMapError("rolling map profile JSON is invalid") from exc
|
||||
_exact_keys(
|
||||
document,
|
||||
{"schema_version", "profile_id", "provider_id", "source", "bounds", "policy", "authority"},
|
||||
"rolling map profile",
|
||||
)
|
||||
if (
|
||||
document["schema_version"] != ROLLING_MAP_PROFILE_SCHEMA
|
||||
or document["provider_id"] != ROLLING_MAP_PROVIDER_ID
|
||||
):
|
||||
raise RollingMapError("rolling map profile identity is incompatible")
|
||||
source = _object(document["source"], "rolling map source")
|
||||
bounds = _object(document["bounds"], "rolling map bounds")
|
||||
_exact_keys(source, {"source_id", "session_id", "representation_id"}, "rolling map source")
|
||||
_exact_keys(bounds, set(RollingMapBounds.__dataclass_fields__), "rolling map bounds")
|
||||
if document["policy"] != {
|
||||
"input_is_complete_scan": False,
|
||||
"input_is_registered_map_increment": True,
|
||||
"absence_of_republication_means_free": False,
|
||||
"clearing_from_missing_points": False,
|
||||
"retained_occupancy_can_assert_threat": True,
|
||||
"retained_motion_claimed": False,
|
||||
"local_radius_eviction": True,
|
||||
"time_bound_eviction": True,
|
||||
"capacity_eviction_allowed": False,
|
||||
}:
|
||||
raise RollingMapError("rolling map policy is incompatible")
|
||||
if document["authority"] != {
|
||||
"ground_truth": False,
|
||||
"physical_live": False,
|
||||
"commands_enabled": False,
|
||||
"actuation_allowed": False,
|
||||
"navigation_or_safety_accepted": False,
|
||||
}:
|
||||
raise RollingMapError("rolling map authority is incompatible")
|
||||
return RollingMapProfile(
|
||||
profile_id=_string(document, "profile_id"),
|
||||
source_id=_string(source, "source_id"),
|
||||
session_id=_string(source, "session_id"),
|
||||
representation_id=_string(source, "representation_id"),
|
||||
bounds=RollingMapBounds(**bounds), # type: ignore[arg-type]
|
||||
profile_sha256=hashlib.sha256(raw).hexdigest(),
|
||||
)
|
||||
|
||||
|
||||
def _cell_key(cell: GridCell) -> tuple[int, int, int]:
|
||||
return cell.x, cell.y, cell.z
|
||||
|
||||
|
||||
def _positive_finite(value: object) -> bool:
|
||||
return (
|
||||
isinstance(value, (int, float))
|
||||
and not isinstance(value, bool)
|
||||
and math.isfinite(float(value))
|
||||
and float(value) > 0.0
|
||||
)
|
||||
|
||||
|
||||
def _positive_integer(value: object) -> bool:
|
||||
return isinstance(value, int) and not isinstance(value, bool) and value > 0
|
||||
|
||||
|
||||
def _object(value: object, label: str) -> dict[str, object]:
|
||||
if not isinstance(value, dict) or any(not isinstance(key, str) for key in value):
|
||||
raise RollingMapError(f"{label} must be an object")
|
||||
return value
|
||||
|
||||
|
||||
def _exact_keys(document: dict[str, object], expected: set[str], label: str) -> None:
|
||||
if set(document) != expected:
|
||||
raise RollingMapError(f"{label} fields are incompatible")
|
||||
|
||||
|
||||
def _string(document: dict[str, object], key: str) -> str:
|
||||
value = document.get(key)
|
||||
if not isinstance(value, str) or not value:
|
||||
raise RollingMapError(f"{key} must be a nonempty string")
|
||||
return value
|
||||
|
||||
|
||||
__all__ = [
|
||||
"DEFAULT_ROLLING_MAP_PROFILE_PATH",
|
||||
"ROLLING_MAP_PROFILE_SCHEMA",
|
||||
"ROLLING_MAP_PROVIDER_ID",
|
||||
"RollingLocalObstacleMapProvider",
|
||||
"RollingMapBounds",
|
||||
"RollingMapError",
|
||||
"RollingMapProfile",
|
||||
"RollingMapSnapshot",
|
||||
"load_rolling_map_profile",
|
||||
]
|
||||
Reference in New Issue
Block a user