L'électron rare
e11f54eb4b
Task 9 of the ICP LiDAR plan: integrate the FusionWorker built in earlier tasks into the live data_only_viz pipeline without disturbing the existing ARKit pelvis fuse path or the Multi-HMR worker thread. A new IcpFusionThread pulls LiDAR frames from LidarTCPReader, stages them into State, and applies in-place ICP registration on state.persons_smplx[*].vertices_3d. It runs as a separate daemon thread parallel to MultiHMRWorker rather than inline per frame — the autonomous-worker architecture didn't fit the plan's per-frame call site, so we adapted to a polling thread at 8 Hz. Activation is opt-in via ICP_FUSION=1 plus ICP_LIDAR_HOST; the default code path is untouched. Shutdown wired through applicationWillTerminate_. MultiHMRWorker.predict_once is added as a documented stub (NotImplementedError) because the existing PyTorch run loop is too coupled to the camera and MPS lifecycle for a clean single-shot extraction. calibrate_lidar.py keeps its placeholder until a follow-up refactor extracts a pure _infer(rgb) helper.
73 lines
2.7 KiB
Python
73 lines
2.7 KiB
Python
"""Threaded wrapper that polls State and calls FusionWorker.run_once.
|
|
|
|
ICP fusion runs as a background thread parallel to the autonomous
|
|
Multi-HMR worker. It pulls the latest LiDAR frame from a
|
|
LidarTCPReader, stages it into State, and applies in-place ICP
|
|
registration to ``state.persons_smplx[*].vertices_3d``.
|
|
|
|
Opt-in via ``ICP_FUSION=1`` from main.py.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import logging
|
|
import threading
|
|
import time
|
|
from typing import Optional
|
|
|
|
from .icp_fusion import FusionWorker, IcpConfig
|
|
from .lidar_calib import load_extrinsic
|
|
from .lidar_receiver import LidarTCPReader
|
|
|
|
_LOG = logging.getLogger(__name__)
|
|
|
|
|
|
class IcpFusionThread:
|
|
"""Background thread: pull LiDAR frames, run FusionWorker on state."""
|
|
|
|
def __init__(self, state, host: str, port: int,
|
|
target_hz: float = 8.0) -> None:
|
|
self._state = state
|
|
self._reader = LidarTCPReader(host=host, port=port)
|
|
self._worker = FusionWorker(extrinsic=load_extrinsic(),
|
|
config=IcpConfig())
|
|
self._period_s = 1.0 / max(target_hz, 0.5)
|
|
self._stop = threading.Event()
|
|
self._thread: Optional[threading.Thread] = None
|
|
|
|
def start(self) -> None:
|
|
if self._thread is not None:
|
|
return
|
|
self._reader.start()
|
|
self._thread = threading.Thread(
|
|
target=self._run, name="icp-fusion", daemon=True)
|
|
self._thread.start()
|
|
_LOG.info("icp-fusion thread started")
|
|
|
|
def stop(self) -> None:
|
|
self._stop.set()
|
|
self._reader.stop()
|
|
if self._thread is not None:
|
|
self._thread.join(timeout=2.0)
|
|
self._thread = None
|
|
|
|
def _run(self) -> None:
|
|
while not self._stop.is_set():
|
|
t0 = time.monotonic()
|
|
frame = self._reader.latest()
|
|
if frame is not None and self._state.persons_smplx:
|
|
# State doesn't expose a fine-grained lock for these
|
|
# fields here; rely on FusionWorker.run_once being
|
|
# write-only on persons_smplx[*].vertices_3d (replace in
|
|
# place) and the readers being tolerant of mid-update.
|
|
self._state.lidar_points = frame.points
|
|
self._state.lidar_timestamp_ns = frame.timestamp_ns
|
|
try:
|
|
self._state.icp_metadata = self._worker.run_once(
|
|
self._state)
|
|
except Exception as exc: # noqa: BLE001
|
|
_LOG.warning("icp fusion failed: %s", exc)
|
|
self._state.icp_metadata = None
|
|
elapsed = time.monotonic() - t0
|
|
if self._stop.wait(max(0.0, self._period_s - elapsed)):
|
|
return
|