"""Accumulates FAST-LIO's per-scan /cloud_registered into a running world-frame point cloud (voxel-deduped, capped at a hard point ceiling) and encodes decimated binary frames for the /ws/pointcloud stream. Binary frame layout (all little-endian, no padding): 4s magic b"PCF1" I frame_seq d timestamp (unix seconds) I pose_flag 1 if a pose follows, else 0 (uint32, not uint8 — keeps everything after it 4-byte aligned so the client can construct a Float32Array view directly on the xyz bytes below instead of copying) 7f pose x, y, z, qx, qy, qz, qw (only present if pose_flag) I point_count N Nx3 f4 xyz, world frame, contiguous [x0,y0,z0, x1,y1,z1, ...] Nx1 u1 intensity, normalized to 0-255 xyz and intensity are separate contiguous arrays (not an interleaved struct) so both ends can hand them straight to a typed array / numpy view. """ from __future__ import annotations import struct import threading import time from typing import Optional import numpy as np MAGIC = b"PCF1" class PointCloudAccumulator: def __init__(self, voxel_size: float, max_points: int): self.voxel_size = voxel_size self.max_points = max_points self._lock = threading.Lock() self._xyz = np.empty((0, 3), dtype=np.float32) self._intensity = np.empty((0,), dtype=np.float32) self._keys: list[int] = [] # packed voxel key per row, same order as _xyz self._key_set: set[int] = set() # membership test for incoming-scan dedup self._pose: Optional[tuple] = None # (x,y,z,qx,qy,qz,qw) self._frame_seq = 0 # ── ingest (called from the ROS-side worker thread) ───────────────── def _pack_keys(self, xyz: np.ndarray) -> np.ndarray: # Packs a voxel-quantized (ix,iy,iz) into one int64 so it's hashable # for the Python set below. OFFSET/BASE give each axis +-2^19 voxels # of range (with the default 5cm voxel that's +-26km), comfortably # covering FAST-LIO's 100m det_range with headroom to spare, while # staying well inside int64. OFFSET = 1 << 19 BASE = 1 << 20 idx = np.floor(xyz / self.voxel_size).astype(np.int64) + OFFSET return (idx[:, 0] * BASE + idx[:, 1]) * BASE + idx[:, 2] def add_scan(self, xyz: np.ndarray, intensity: np.ndarray) -> None: """xyz: (K,3) float32 world-frame points from one /cloud_registered message.""" if xyz.shape[0] == 0: return keys = self._pack_keys(xyz) # Dedup within this scan first (a downsampled scan can still put # multiple points in one voxel), then keep only voxels not already # in the accumulated buffer. _, first_idx = np.unique(keys, return_index=True) keys = keys[first_idx] xyz = xyz[first_idx] intensity = intensity[first_idx] with self._lock: novel = np.fromiter((k not in self._key_set for k in keys), dtype=bool, count=len(keys)) if not novel.any(): return new_xyz = xyz[novel] new_intensity = intensity[novel] new_keys = keys[novel] self._xyz = np.concatenate([self._xyz, new_xyz]) self._intensity = np.concatenate([self._intensity, new_intensity]) self._keys.extend(int(k) for k in new_keys) self._key_set.update(int(k) for k in new_keys) overflow = len(self._keys) - self.max_points if overflow > 0: # FIFO eviction — oldest points dropped first once over budget. evicted, self._keys = self._keys[:overflow], self._keys[overflow:] self._key_set.difference_update(evicted) self._xyz = self._xyz[overflow:] self._intensity = self._intensity[overflow:] def set_pose(self, x: float, y: float, z: float, qx: float, qy: float, qz: float, qw: float) -> None: with self._lock: self._pose = (x, y, z, qx, qy, qz, qw) def point_count(self) -> int: with self._lock: return self._xyz.shape[0] def reset(self) -> None: """Clears the accumulated map — call when a new recording starts so the previous scan's points don't linger into the next one.""" with self._lock: self._xyz = np.empty((0, 3), dtype=np.float32) self._intensity = np.empty((0,), dtype=np.float32) self._keys = [] self._key_set = set() self._pose = None self._frame_seq = 0 # ── output (called from the asyncio WS loop) ───────────────────────── def encode_frame(self, max_points: int) -> bytes: with self._lock: n_total = self._xyz.shape[0] if n_total <= max_points: xyz, intensity = self._xyz, self._intensity else: # Stride sample, not random — keeps the decimated view stable # frame-to-frame instead of sparkling with a fresh random # subset every send. idx = np.linspace(0, n_total - 1, max_points).astype(np.int64) xyz, intensity = self._xyz[idx], self._intensity[idx] pose = self._pose self._frame_seq += 1 seq = self._frame_seq return _pack_frame(seq, time.time(), pose, xyz, intensity) def _pack_frame(seq: int, timestamp: float, pose: Optional[tuple], xyz: np.ndarray, intensity: np.ndarray) -> bytes: n = xyz.shape[0] parts = [MAGIC, struct.pack("