gardentech
f34817d5b6
FastAPI backend + vanilla JS frontend for LiDAR/camera startup, camera settings, and rosbag recording, unifying the previous scan_gui/scan_gui_dual/ scan_gui_triple desktop tools into one web app.
146 lines
6.3 KiB
Python
146 lines
6.3 KiB
Python
"""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("<Id", seq, timestamp)]
|
|
if pose is not None:
|
|
parts.append(struct.pack("<I", 1))
|
|
parts.append(struct.pack("<7f", *pose))
|
|
else:
|
|
parts.append(struct.pack("<I", 0))
|
|
parts.append(struct.pack("<I", n))
|
|
parts.append(np.ascontiguousarray(xyz, dtype="<f4").tobytes())
|
|
# Livox reflectivity/intensity isn't guaranteed to sit in 0-255, but in
|
|
# practice runs close to it — plain clip+cast is a fine first cut, tune
|
|
# once real field values are observed.
|
|
intensity_u8 = np.clip(intensity, 0, 255).astype(np.uint8)
|
|
parts.append(np.ascontiguousarray(intensity_u8).tobytes())
|
|
return b"".join(parts)
|