新增原始数据一步导出到 combined：对齐 Lidar-IMU 导出入口，适配 H32/G90/N300
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -0,0 +1,371 @@
|
||||
"""Decode RoboSense H32 MSOP V2 .rscap into Cartesian frames (metres).
|
||||
|
||||
Angle / distance conventions follow ``RSLidarH32_3D_RawCaptureNet48``:
|
||||
azimuth = normalize(-(block_az + horizontal[ch])), altitude = vertical[ch],
|
||||
distance_mm = raw * distance_unit_mm, then:
|
||||
|
||||
x = d_m * cos(alt) * cos(az)
|
||||
y = d_m * cos(alt) * sin(az)
|
||||
z = d_m * sin(alt)
|
||||
|
||||
MSOP-only captures do not include DIFOP; vertical angles default to a uniform
|
||||
-16°…+16° fan, horizontal channel offsets default to 0.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
from capture_format_v2 import CaptureFile
|
||||
|
||||
PACKET_LENGTH = 1248
|
||||
DATA_START = 42
|
||||
BLOCKS = 12
|
||||
BLOCK_LENGTH = 100
|
||||
CHANNELS = 32
|
||||
MIN_FRAME_POINTS_DEFAULT = 100
|
||||
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||||
|
||||
|
||||
def ticks_to_unix_ns(ticks: int) -> int:
|
||||
return (ticks - DOTNET_UNIX_EPOCH_TICKS) * 100
|
||||
|
||||
|
||||
def default_vertical_deg() -> np.ndarray:
|
||||
return -16.0 + np.arange(CHANNELS, dtype=np.float64) * (32.0 / (CHANNELS - 1))
|
||||
|
||||
|
||||
def default_horizontal_deg() -> np.ndarray:
|
||||
return np.zeros(CHANNELS, dtype=np.float64)
|
||||
|
||||
|
||||
def read_u16_be(packet: bytes, index: int) -> int:
|
||||
return (packet[index] << 8) | packet[index + 1]
|
||||
|
||||
|
||||
def device_timestamp_ms(packet: bytes) -> int:
|
||||
seconds = int.from_bytes(packet[20:26], "big")
|
||||
microseconds = int.from_bytes(packet[26:30], "big")
|
||||
return seconds * 1000 + microseconds // 1000
|
||||
|
||||
|
||||
def distance_unit_mm(packet: bytes, *, auto: bool = True, fallback: float = 2.5) -> float:
|
||||
if not auto:
|
||||
return float(fallback)
|
||||
return 2.5 if packet[17] == 1 else 0.5
|
||||
|
||||
|
||||
def normalize_azimuth_deg(angle: float) -> float:
|
||||
while angle > 180.0:
|
||||
angle -= 360.0
|
||||
while angle < -180.0:
|
||||
angle += 360.0
|
||||
return angle
|
||||
|
||||
|
||||
@dataclass
|
||||
class LidarFrameExport:
|
||||
t_start_s: float
|
||||
t_end_s: float
|
||||
points_xyz: np.ndarray # (N, 3) metres
|
||||
|
||||
|
||||
@dataclass
|
||||
class LidarFramePolarExport:
|
||||
"""One H32 frame in the calibration ``points_raw`` polar contract.
|
||||
|
||||
Columns: ``d_mm, azimuth_deg, altitude_deg, intensity, progression``.
|
||||
Azimuth already includes the H32 channel horizontal offset and sign flip so
|
||||
``rigorous_calibration.load_npz_xyz`` reproduces the same Cartesian points.
|
||||
"""
|
||||
|
||||
t_start_s: float
|
||||
t_end_s: float
|
||||
points_raw: np.ndarray # (N, 5) float32
|
||||
host_receive_utc_ns: int
|
||||
|
||||
|
||||
def decode_packet_points(
|
||||
packet: bytes,
|
||||
vertical_deg: np.ndarray,
|
||||
horizontal_deg: np.ndarray,
|
||||
*,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
) -> tuple[list[float], np.ndarray]:
|
||||
"""Decode one MSOP packet into block azimuths and concatenated XYZ points."""
|
||||
|
||||
if len(packet) != PACKET_LENGTH:
|
||||
return [], np.zeros((0, 3), dtype=np.float64)
|
||||
unit = distance_unit_mm(packet)
|
||||
az_list: list[float] = []
|
||||
chunks: list[np.ndarray] = []
|
||||
idx = DATA_START
|
||||
for _block in range(BLOCKS):
|
||||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||||
break
|
||||
az = read_u16_be(packet, idx + 2) * 0.01
|
||||
az_list.append(az)
|
||||
pts = _block_points(
|
||||
packet,
|
||||
idx,
|
||||
az,
|
||||
unit,
|
||||
vertical_deg,
|
||||
horizontal_deg,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
)
|
||||
if pts.shape[0]:
|
||||
chunks.append(pts)
|
||||
idx += BLOCK_LENGTH
|
||||
if not chunks:
|
||||
return az_list, np.zeros((0, 3), dtype=np.float64)
|
||||
return az_list, np.vstack(chunks)
|
||||
|
||||
|
||||
def _block_points(
|
||||
packet: bytes,
|
||||
block_offset: int,
|
||||
az_deg: float,
|
||||
unit_mm: float,
|
||||
vertical_deg: np.ndarray,
|
||||
horizontal_deg: np.ndarray,
|
||||
*,
|
||||
min_range_m: float,
|
||||
max_range_m: float,
|
||||
) -> np.ndarray:
|
||||
xs: list[float] = []
|
||||
ys: list[float] = []
|
||||
zs: list[float] = []
|
||||
idx = block_offset + 4 # after FF EE + azimuth
|
||||
for ch in range(CHANNELS):
|
||||
raw = read_u16_be(packet, idx)
|
||||
idx += 3
|
||||
if raw == 0:
|
||||
continue
|
||||
d_m = (raw * unit_mm) * 0.001
|
||||
if d_m < min_range_m or d_m > max_range_m:
|
||||
continue
|
||||
az_ch = np.deg2rad(normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch]))))
|
||||
alt = np.deg2rad(float(vertical_deg[ch]))
|
||||
cos_alt = np.cos(alt)
|
||||
xs.append(d_m * cos_alt * np.cos(az_ch))
|
||||
ys.append(d_m * cos_alt * np.sin(az_ch))
|
||||
zs.append(d_m * np.sin(alt))
|
||||
if not xs:
|
||||
return np.zeros((0, 3), dtype=np.float64)
|
||||
return np.column_stack([xs, ys, zs]).astype(np.float64, copy=False)
|
||||
|
||||
|
||||
def _block_points_raw(
|
||||
packet: bytes,
|
||||
block_offset: int,
|
||||
az_deg: float,
|
||||
unit_mm: float,
|
||||
vertical_deg: np.ndarray,
|
||||
horizontal_deg: np.ndarray,
|
||||
*,
|
||||
min_range_m: float,
|
||||
max_range_m: float,
|
||||
) -> np.ndarray:
|
||||
"""Return polar ``points_raw`` rows compatible with ``load_npz_xyz``."""
|
||||
|
||||
rows: list[list[float]] = []
|
||||
idx = block_offset + 4
|
||||
for ch in range(CHANNELS):
|
||||
raw = read_u16_be(packet, idx)
|
||||
intensity = float(packet[idx + 2])
|
||||
idx += 3
|
||||
if raw == 0:
|
||||
continue
|
||||
d_mm = float(raw) * unit_mm
|
||||
d_m = d_mm * 0.001
|
||||
if d_m < min_range_m or d_m > max_range_m:
|
||||
continue
|
||||
az_ch = normalize_azimuth_deg(-(az_deg + float(horizontal_deg[ch])))
|
||||
rows.append([d_mm, az_ch, float(vertical_deg[ch]), intensity, float(ch)])
|
||||
if not rows:
|
||||
return np.zeros((0, 5), dtype=np.float32)
|
||||
return np.asarray(rows, dtype=np.float32)
|
||||
|
||||
|
||||
def iter_h32_frames_polar(
|
||||
capture: CaptureFile,
|
||||
*,
|
||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||
frame_stride: int = 1,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
max_points_per_frame: int | None = None,
|
||||
vertical_deg: np.ndarray | None = None,
|
||||
horizontal_deg: np.ndarray | None = None,
|
||||
) -> list[LidarFramePolarExport]:
|
||||
"""Assemble MSOP packets into polar frames for the RTK–LiDAR combined contract."""
|
||||
|
||||
vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
|
||||
horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
|
||||
if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
|
||||
raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)")
|
||||
|
||||
frames: list[LidarFramePolarExport] = []
|
||||
point_chunks: list[np.ndarray] = []
|
||||
t_start: float | None = None
|
||||
t_end: float | None = None
|
||||
host_ns = 0
|
||||
prev_az: float | None = None
|
||||
kept = 0
|
||||
stride = max(1, int(frame_stride))
|
||||
|
||||
def emit() -> None:
|
||||
nonlocal point_chunks, t_start, t_end, host_ns, kept
|
||||
if not point_chunks or t_start is None or t_end is None:
|
||||
point_chunks = []
|
||||
t_start = t_end = None
|
||||
return
|
||||
points = np.vstack(point_chunks)
|
||||
point_chunks = []
|
||||
start_s, end_s = t_start, t_end
|
||||
frame_host = host_ns
|
||||
t_start = t_end = None
|
||||
if points.shape[0] < min_frame_points:
|
||||
return
|
||||
if kept % stride != 0:
|
||||
kept += 1
|
||||
return
|
||||
kept += 1
|
||||
if max_points_per_frame is not None and points.shape[0] > max_points_per_frame:
|
||||
select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int)
|
||||
points = points[select]
|
||||
if end_s <= start_s:
|
||||
end_s = start_s + 0.1
|
||||
frames.append(
|
||||
LidarFramePolarExport(
|
||||
t_start_s=start_s,
|
||||
t_end_s=end_s,
|
||||
points_raw=points.astype(np.float32, copy=False),
|
||||
host_receive_utc_ns=int(frame_host),
|
||||
)
|
||||
)
|
||||
|
||||
for chunk in capture.chunks:
|
||||
packet = chunk.raw
|
||||
if len(packet) != PACKET_LENGTH:
|
||||
continue
|
||||
packet_t = device_timestamp_ms(packet) * 1e-3
|
||||
unit = distance_unit_mm(packet)
|
||||
chunk_host = ticks_to_unix_ns(chunk.receive_utc_ticks)
|
||||
idx = DATA_START
|
||||
for _block in range(BLOCKS):
|
||||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||||
break
|
||||
az = read_u16_be(packet, idx + 2) * 0.01
|
||||
if prev_az is not None and prev_az > 270.0 and az < 90.0:
|
||||
emit()
|
||||
prev_az = az
|
||||
pts = _block_points_raw(
|
||||
packet,
|
||||
idx,
|
||||
az,
|
||||
unit,
|
||||
vertical,
|
||||
horizontal,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
)
|
||||
if pts.shape[0]:
|
||||
if t_start is None:
|
||||
t_start = packet_t
|
||||
t_end = packet_t
|
||||
host_ns = chunk_host
|
||||
point_chunks.append(pts)
|
||||
idx += BLOCK_LENGTH
|
||||
|
||||
emit()
|
||||
return frames
|
||||
|
||||
|
||||
def iter_h32_frames(
|
||||
capture: CaptureFile,
|
||||
*,
|
||||
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
|
||||
frame_stride: int = 1,
|
||||
min_range_m: float = 0.3,
|
||||
max_range_m: float = 120.0,
|
||||
max_points_per_frame: int | None = None,
|
||||
vertical_deg: np.ndarray | None = None,
|
||||
horizontal_deg: np.ndarray | None = None,
|
||||
) -> list[LidarFrameExport]:
|
||||
"""Assemble MSOP packets into frames using the 270°→90° azimuth wrap."""
|
||||
|
||||
vertical = default_vertical_deg() if vertical_deg is None else np.asarray(vertical_deg, dtype=np.float64)
|
||||
horizontal = default_horizontal_deg() if horizontal_deg is None else np.asarray(horizontal_deg, dtype=np.float64)
|
||||
if vertical.shape != (CHANNELS,) or horizontal.shape != (CHANNELS,):
|
||||
raise ValueError(f"vertical/horizontal must have shape ({CHANNELS},)")
|
||||
|
||||
frames: list[LidarFrameExport] = []
|
||||
point_chunks: list[np.ndarray] = []
|
||||
t_start: float | None = None
|
||||
t_end: float | None = None
|
||||
prev_az: float | None = None
|
||||
kept = 0
|
||||
stride = max(1, int(frame_stride))
|
||||
|
||||
def emit() -> None:
|
||||
nonlocal point_chunks, t_start, t_end, kept
|
||||
if not point_chunks or t_start is None or t_end is None:
|
||||
point_chunks = []
|
||||
t_start = t_end = None
|
||||
return
|
||||
points = np.vstack(point_chunks)
|
||||
point_chunks = []
|
||||
start_s, end_s = t_start, t_end
|
||||
t_start = t_end = None
|
||||
if points.shape[0] < min_frame_points:
|
||||
return
|
||||
if kept % stride != 0:
|
||||
kept += 1
|
||||
return
|
||||
kept += 1
|
||||
if max_points_per_frame is not None and points.shape[0] > max_points_per_frame:
|
||||
select = np.linspace(0, points.shape[0] - 1, max_points_per_frame, dtype=int)
|
||||
points = points[select]
|
||||
if end_s <= start_s:
|
||||
end_s = start_s + 0.1
|
||||
frames.append(LidarFrameExport(t_start_s=start_s, t_end_s=end_s, points_xyz=points))
|
||||
|
||||
for chunk in capture.chunks:
|
||||
packet = chunk.raw
|
||||
if len(packet) != PACKET_LENGTH:
|
||||
continue
|
||||
packet_t = device_timestamp_ms(packet) * 1e-3
|
||||
unit = distance_unit_mm(packet)
|
||||
idx = DATA_START
|
||||
for _block in range(BLOCKS):
|
||||
if idx + BLOCK_LENGTH > PACKET_LENGTH or packet[idx] != 255 or packet[idx + 1] != 238:
|
||||
break
|
||||
az = read_u16_be(packet, idx + 2) * 0.01
|
||||
if prev_az is not None and prev_az > 270.0 and az < 90.0:
|
||||
emit()
|
||||
prev_az = az
|
||||
pts = _block_points(
|
||||
packet,
|
||||
idx,
|
||||
az,
|
||||
unit,
|
||||
vertical,
|
||||
horizontal,
|
||||
min_range_m=min_range_m,
|
||||
max_range_m=max_range_m,
|
||||
)
|
||||
if pts.shape[0]:
|
||||
if t_start is None:
|
||||
t_start = packet_t
|
||||
t_end = packet_t
|
||||
point_chunks.append(pts)
|
||||
idx += BLOCK_LENGTH
|
||||
|
||||
emit()
|
||||
return frames
|
||||
Reference in New Issue
Block a user