支持 HI13/H32 主机 UTC 桥接对齐、多会话联合标定与 CAD 平移先验。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-10 13:26:32 +08:00
co-authored by Cursor
parent 30f7e66db3
commit 2237be77a4
20 changed files with 1830 additions and 347 deletions
+7 -1
View File
@@ -1,12 +1,18 @@
"""Medulla dlog readers for RSLidarH32_3D_DLogCaptureNet48 raw MSOP/DIFOP."""
from .difop import parse_difop_angles
from .dobject import discover_records, iter_payloads, resolve_dlog_root
from .dobject import discover_records, iter_payloads, open_dlog_source, resolve_dlog_root
from .load_session import H32DlogLidarSession, load_h32_dlog_lidar
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
from .timeutil import local_wall_to_dotnet_ticks
__all__ = [
"H32DlogLidarSession",
"discover_records",
"iter_payloads",
"load_h32_dlog_lidar",
"local_wall_to_dotnet_ticks",
"open_dlog_source",
"parse_difop_angles",
"parse_difop_payload",
"parse_msop_batch_payload",
+283 -81
View File
@@ -1,16 +1,24 @@
"""Index and read Medulla DObject recordings (dobject/ + dobject_recording/)."""
"""Index and read Medulla DObject recordings.
Supports:
- standard layout: ``dobject/**/*.log`` + ``dobject_recording/**/*.dorec``
- recovered layout: ``dobject/all/indices.log`` + ``dobject_recording/data.bin``
- either as an extracted directory or a zip containing those paths
"""
from __future__ import annotations
import re
import struct
import zipfile
from dataclasses import dataclass
from pathlib import Path
from typing import BinaryIO, Iterator
RECORD_RE = re.compile(
r"^\[(?P<log_time>[^]]+)\].*?DObject `(?P<name>[^`]+)` post "
r"^(?:\[(?P<log_time>[^]]+)\])?>?\s*DObject `(?P<name>[^`]+)` post "
r"len=(?P<len>\d+)B, id:(?P<id>[0-9A-Fa-f]+), tic:(?P<tic>\d+), "
r"@(?P<file>[^:]+):(?P<offset>\d+)"
)
@@ -29,37 +37,225 @@ class RecordRef:
dotnet_ticks: int
def resolve_dlog_root(value: Path | str) -> Path:
root = Path(value).expanduser().resolve()
if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir():
return root
child = root / "dlog"
if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
return child
raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording")
class _ZipStoredMemberIO:
"""Random-access reader for a ZIP_STORED member via the underlying zip file.
``ZipExtFile.seek`` on multi-GB members is far too slow for per-record reads.
"""
def __init__(self, zip_path: Path, member_name: str, data_offset: int, data_size: int):
self._path = zip_path
self._member_name = member_name
self._data_offset = data_offset
self._data_size = data_size
self._fh = zip_path.open("rb")
self._pos = 0
def seek(self, offset: int, whence: int = 0) -> int:
if whence == 0:
self._pos = offset
elif whence == 1:
self._pos += offset
elif whence == 2:
self._pos = self._data_size + offset
else:
raise ValueError(f"invalid whence: {whence}")
if self._pos < 0:
raise ValueError("negative seek")
return self._pos
def read(self, size: int = -1) -> bytes:
if size is None or size < 0:
size = self._data_size - self._pos
if size <= 0 or self._pos >= self._data_size:
return b""
size = min(size, self._data_size - self._pos)
self._fh.seek(self._data_offset + self._pos)
data = self._fh.read(size)
self._pos += len(data)
return data
def close(self) -> None:
self._fh.close()
def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
pending: list[tuple[str, str, str, int, int, str, int, str]] = []
for log_path in sorted((dlog_root / "dobject").rglob("*.log")):
relative_log = log_path.relative_to(dlog_root).as_posix()
with log_path.open("r", encoding="utf-8", errors="replace") as stream:
for line in stream:
match = RECORD_RE.search(line)
if not match or match.group("name").casefold() != object_name.casefold():
continue
pending.append(
(
match.group("name"),
match.group("log_time"),
relative_log,
int(match.group("offset")),
int(match.group("len")),
match.group("id").upper(),
int(match.group("tic")),
match.group("file"),
)
def _zip_stored_member_offset(zip_path: Path, info: zipfile.ZipInfo) -> int:
if info.compress_type != zipfile.ZIP_STORED:
raise RuntimeError(
f"member {info.filename!r} is compressed (type={info.compress_type}); "
"extract it first or store uncompressed"
)
with zip_path.open("rb") as handle:
handle.seek(info.header_offset)
header = handle.read(30)
if len(header) != 30 or header[:4] != b"PK\x03\x04":
raise RuntimeError(f"bad local zip header for {info.filename!r}")
name_len, extra_len = struct.unpack("<HH", header[26:30])
return info.header_offset + 30 + name_len + extra_len
@dataclass
class DlogSource:
"""Opened dlog directory or recovered zip."""
label: str
directory: Path | None = None
zip_path: Path | None = None
_zip: zipfile.ZipFile | None = None
_log_cache: dict[str, str] | None = None
_member_offsets: dict[str, tuple[int, int]] | None = None
def close(self) -> None:
if self._zip is not None:
self._zip.close()
self._zip = None
def __enter__(self) -> "DlogSource":
return self
def __exit__(self, exc_type, exc, tb) -> None:
self.close()
def iter_log_texts(self) -> Iterator[tuple[str, str]]:
if self.zip_path is not None:
assert self._zip is not None
if self._log_cache is None:
self._log_cache = {}
names = sorted(
name
for name in self._zip.namelist()
if name.replace("\\", "/").startswith("dobject/")
and name.replace("\\", "/").endswith(".log")
)
for name in names:
key = name.replace("\\", "/")
self._log_cache[key] = self._zip.read(name).decode("utf-8", errors="replace")
for name, text in self._log_cache.items():
yield name, text
return
assert self.directory is not None
for log_path in sorted((self.directory / "dobject").rglob("*.log")):
relative = log_path.relative_to(self.directory).as_posix()
yield relative, log_path.read_text(encoding="utf-8", errors="replace")
def open_recording(self, name: str) -> tuple[object, BinaryIO]:
"""Return (owner, binary stream) supporting seek/read of one recording member."""
base = Path(name).name
if self.zip_path is not None:
assert self._zip is not None
candidates = [
n
for n in self._zip.namelist()
if Path(n.replace("\\", "/")).name.casefold() == base.casefold()
and "dobject_recording/" in n.replace("\\", "/")
]
if not candidates:
alt = name.replace("\\", "/")
if alt in self._zip.namelist():
candidates = [alt]
elif f"dobject_recording/{base}" in self._zip.namelist():
candidates = [f"dobject_recording/{base}"]
if not candidates:
raise FileNotFoundError(f"missing recording in zip: {name}")
if len(candidates) > 1:
raise RuntimeError(f"ambiguous recording in zip {name}: {candidates}")
member = candidates[0].replace("\\", "/")
if self._member_offsets is None:
self._member_offsets = {}
if member not in self._member_offsets:
info = self._zip.getinfo(member)
self._member_offsets[member] = (
_zip_stored_member_offset(self.zip_path, info),
info.file_size,
)
data_offset, data_size = self._member_offsets[member]
stream = _ZipStoredMemberIO(self.zip_path, member, data_offset, data_size)
return stream, stream
assert self.directory is not None
index = index_dorec_files(self.directory)
if base.casefold() == "data.bin":
path = self.directory / "dobject_recording" / "data.bin"
if not path.is_file():
matches = list((self.directory / "dobject_recording").rglob("data.bin"))
if not matches:
raise FileNotFoundError(f"missing recording file: {name}")
path = matches[0]
stream = path.open("rb")
return stream, stream
path = choose_dorec(index, name)
stream = path.open("rb")
return stream, stream
def open_dlog_source(value: Path | str) -> DlogSource:
path = Path(value).expanduser().resolve()
if path.is_file() and path.suffix.lower() == ".zip":
zf = zipfile.ZipFile(path, "r")
names = {n.replace("\\", "/") for n in zf.namelist()}
has_log = any(n.startswith("dobject/") and n.endswith(".log") for n in names)
has_rec = any(n.startswith("dobject_recording/") for n in names)
if not (has_log and has_rec):
zf.close()
raise FileNotFoundError(f"{path} is not a recovered/standard dlog zip")
return DlogSource(label=str(path), zip_path=path, _zip=zf)
root = path
if not ((root / "dobject").is_dir() and (root / "dobject_recording").is_dir()):
child = root / "dlog"
if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
root = child
else:
raise FileNotFoundError(f"{path} does not contain dobject and dobject_recording")
return DlogSource(label=str(root), directory=root)
def resolve_dlog_root(value: Path | str) -> Path:
"""Backward-compatible helper: directory roots only (not zip)."""
source = open_dlog_source(value)
try:
if source.directory is None:
raise FileNotFoundError(
f"{value} is a zip; use open_dlog_source()/iter_payloads_from_source()"
)
return source.directory
finally:
source.close()
def discover_records_from_source(
source: DlogSource,
object_name: str,
*,
host_ticks_min: int | None = None,
host_ticks_max: int | None = None,
) -> list[RecordRef]:
pending: list[tuple[str, str, str, int, int, str, int, str]] = []
name_key = object_name.casefold()
for relative_log, text in source.iter_log_texts():
for line in text.splitlines():
match = RECORD_RE.search(line.strip())
if not match or match.group("name").casefold() != name_key:
continue
ticks = int(match.group("tic"))
if host_ticks_min is not None and ticks < host_ticks_min:
continue
if host_ticks_max is not None and ticks > host_ticks_max:
continue
pending.append(
(
match.group("name"),
match.group("log_time") or "",
relative_log,
int(match.group("offset")),
int(match.group("len")),
match.group("id").upper(),
ticks,
match.group("file"),
)
)
pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3]))
seen: set[tuple[str, int, int]] = set()
records: list[RecordRef] = []
@@ -84,10 +280,19 @@ def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
return records
def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
with open_dlog_source(dlog_root) as source:
return discover_records_from_source(source, object_name)
def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]:
result: dict[str, list[Path]] = {}
for path in (dlog_root / "dobject_recording").rglob("*.dorec"):
result.setdefault(path.name.casefold(), []).append(path)
recording = dlog_root / "dobject_recording"
if not recording.is_dir():
return result
for path in recording.rglob("*"):
if path.is_file() and path.suffix.lower() in {".dorec", ".bin"}:
result.setdefault(path.name.casefold(), []).append(path)
return result
@@ -107,17 +312,15 @@ def read_exact(stream: BinaryIO, size: int) -> bytes:
return data
def read_record_payload(path: Path, record: RecordRef) -> bytes:
with path.open("rb") as stream:
stream.seek(record.source_offset)
name_length = read_exact(stream, 1)[0]
name = read_exact(stream, name_length).decode("ascii")
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
id_length = read_exact(stream, 1)[0]
id_bytes = read_exact(stream, id_length)
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
payload = read_exact(stream, payload_length)
def _read_payload_at(stream: BinaryIO, record: RecordRef) -> bytes:
stream.seek(record.source_offset)
name_length = read_exact(stream, 1)[0]
name = read_exact(stream, name_length).decode("ascii")
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
id_length = read_exact(stream, 1)[0]
id_bytes = read_exact(stream, id_length)
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
payload = read_exact(stream, payload_length)
try:
record_id = id_bytes.decode("ascii")
except UnicodeDecodeError:
@@ -133,47 +336,46 @@ def read_record_payload(path: Path, record: RecordRef) -> bytes:
return payload
def iter_payloads(dlog_root: Path, object_name: str) -> Iterator[tuple[RecordRef, bytes]]:
root = resolve_dlog_root(dlog_root)
records = discover_records(root, object_name)
def iter_payloads_from_source(
source: DlogSource,
object_name: str,
*,
host_ticks_min: int | None = None,
host_ticks_max: int | None = None,
) -> Iterator[tuple[RecordRef, bytes]]:
records = discover_records_from_source(
source,
object_name,
host_ticks_min=host_ticks_min,
host_ticks_max=host_ticks_max,
)
if not records:
return
dorec_index = index_dorec_files(root)
open_files: dict[str, tuple[Path, BinaryIO]] = {}
open_files: dict[str, BinaryIO] = {}
try:
for record in records:
key = record.source_dorec.casefold()
handle = open_files.get(key)
if handle is None:
path = choose_dorec(dorec_index, record.source_dorec)
handle = (path, path.open("rb"))
open_files[key] = handle
path, stream = handle
stream.seek(record.source_offset)
name_length = read_exact(stream, 1)[0]
name = read_exact(stream, name_length).decode("ascii")
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
id_length = read_exact(stream, 1)[0]
id_bytes = read_exact(stream, id_length)
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
payload = read_exact(stream, payload_length)
try:
record_id = id_bytes.decode("ascii")
except UnicodeDecodeError:
record_id = id_bytes.hex().upper()
if name != record.object_name:
raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
if ticks != record.dotnet_ticks:
raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
if payload_length != record.payload_length:
raise ValueError(
f"payload mismatch: log={record.payload_length}, dorec={payload_length}"
)
if record_id.upper() != record.log_record_id.upper():
raise ValueError(
f"record id mismatch: log={record.log_record_id}, dorec={record_id}"
)
yield record, payload
key = Path(record.source_dorec).name.casefold()
stream = open_files.get(key)
if stream is None:
_owner, stream = source.open_recording(record.source_dorec)
open_files[key] = stream
yield record, _read_payload_at(stream, record)
finally:
for _path, stream in open_files.values():
for stream in open_files.values():
stream.close()
def iter_payloads(
dlog_root: Path | str,
object_name: str,
*,
host_ticks_min: int | None = None,
host_ticks_max: int | None = None,
) -> Iterator[tuple[RecordRef, bytes]]:
with open_dlog_source(dlog_root) as source:
yield from iter_payloads_from_source(
source,
object_name,
host_ticks_min=host_ticks_min,
host_ticks_max=host_ticks_max,
)
+98 -57
View File
@@ -10,16 +10,21 @@ import numpy as np
from tools.rscap_v2.h32_msop import default_horizontal_deg, default_vertical_deg
from .difop import DifopAngles, parse_difop_angles
from .dobject import discover_records, iter_payloads, resolve_dlog_root
from .dobject import (
discover_records_from_source,
iter_payloads_from_source,
open_dlog_source,
)
from .payload_v1 import parse_difop_payload, parse_msop_batch_payload
@dataclass
class H32DlogLidarSession:
dlog_root: Path
dlog_root: str
msop_object: str
difop_object: str
msop_packets: list[bytes]
msop_host_utc_ticks: list[int]
msop_batch_count: int
difop_record_count: int
angle_source: str
@@ -27,6 +32,8 @@ class H32DlogLidarSession:
horizontal_deg: np.ndarray
session_id: str | None = None
lidar_ip: str | None = None
host_ticks_min: int | None = None
host_ticks_max: int | None = None
def load_h32_dlog_lidar(
@@ -35,65 +42,99 @@ def load_h32_dlog_lidar(
msop_object: str = "frontlidar-msop-raw",
difop_object: str = "frontlidar-difop-raw",
require_difop: bool = False,
host_ticks_min: int | None = None,
host_ticks_max: int | None = None,
) -> H32DlogLidarSession:
root = resolve_dlog_root(dlog_root)
msop_packets: list[bytes] = []
batch_count = 0
session_id: str | None = None
lidar_ip: str | None = None
with open_dlog_source(dlog_root) as source:
# DIFOP angles: prefer packets inside the window, else any in the capture.
angles: DifopAngles | None = None
difop_count = 0
session_id: str | None = None
lidar_ip: str | None = None
for _record, payload in iter_payloads_from_source(
source,
difop_object,
host_ticks_min=host_ticks_min,
host_ticks_max=host_ticks_max,
):
difop = parse_difop_payload(payload)
difop_count += 1
try:
angles = parse_difop_angles(difop.raw)
except ValueError:
continue
if session_id is None:
session_id = difop.session_id
lidar_ip = difop.lidar_ip
for _record, payload in iter_payloads(root, msop_object):
batch = parse_msop_batch_payload(payload)
batch_count += 1
if session_id is None:
session_id = batch.session_id
lidar_ip = batch.lidar_ip
for item in batch.packets:
msop_packets.append(item.raw)
if angles is None:
for _record, payload in iter_payloads_from_source(source, difop_object):
difop = parse_difop_payload(payload)
difop_count += 1
try:
angles = parse_difop_angles(difop.raw)
except ValueError:
continue
if session_id is None:
session_id = difop.session_id
lidar_ip = difop.lidar_ip
if angles is not None:
break
angles: DifopAngles | None = None
difop_count = 0
for _record, payload in iter_payloads(root, difop_object):
difop = parse_difop_payload(payload)
difop_count += 1
try:
angles = parse_difop_angles(difop.raw)
except ValueError:
continue
if session_id is None:
session_id = difop.session_id
lidar_ip = difop.lidar_ip
msop_packets: list[bytes] = []
msop_host_utc_ticks: list[int] = []
batch_count = 0
for record, payload in iter_payloads_from_source(
source,
msop_object,
host_ticks_min=host_ticks_min,
host_ticks_max=host_ticks_max,
):
batch = parse_msop_batch_payload(payload)
batch_count += 1
if session_id is None:
session_id = batch.session_id
lidar_ip = batch.lidar_ip
for item in batch.packets:
msop_packets.append(item.raw)
# Per-packet UTC host receive from MSOP DLog payload only.
# Do NOT fall back to DObject tic (DateTime.Now / local).
msop_host_utc_ticks.append(int(item.host_receive_utc_ticks))
if not msop_packets:
msop_records = discover_records(root, msop_object)
raise RuntimeError(
f"no MSOP packets from DObject {msop_object!r} under {root} "
f"(log records={len(msop_records)})"
)
if angles is None:
if require_difop:
if not msop_packets:
msop_records = discover_records_from_source(source, msop_object)
raise RuntimeError(
f"no valid DIFOP calibration from DObject {difop_object!r} under {root}"
f"no MSOP packets from DObject {msop_object!r} under {source.label} "
f"(log records={len(msop_records)}, "
f"host_ticks=[{host_ticks_min}, {host_ticks_max}])"
)
vertical = default_vertical_deg()
horizontal = default_horizontal_deg()
angle_source = "default_msop_only_vertical_-16_to_16_deg"
else:
vertical = angles.vertical_deg
horizontal = angles.horizontal_deg
angle_source = "difop_channel_angles"
return H32DlogLidarSession(
dlog_root=root,
msop_object=msop_object,
difop_object=difop_object,
msop_packets=msop_packets,
msop_batch_count=batch_count,
difop_record_count=difop_count,
angle_source=angle_source,
vertical_deg=vertical,
horizontal_deg=horizontal,
session_id=session_id,
lidar_ip=lidar_ip,
)
if angles is None:
if require_difop:
raise RuntimeError(
f"no valid DIFOP calibration from DObject {difop_object!r} under {source.label}"
)
vertical = default_vertical_deg()
horizontal = default_horizontal_deg()
angle_source = "default_msop_only_vertical_-16_to_16_deg"
else:
vertical = angles.vertical_deg
horizontal = angles.horizontal_deg
angle_source = "difop_channel_angles"
return H32DlogLidarSession(
dlog_root=source.label,
msop_object=msop_object,
difop_object=difop_object,
msop_packets=msop_packets,
msop_host_utc_ticks=msop_host_utc_ticks,
msop_batch_count=batch_count,
difop_record_count=difop_count,
angle_source=angle_source,
vertical_deg=vertical,
horizontal_deg=horizontal,
session_id=session_id,
lidar_ip=lidar_ip,
host_ticks_min=host_ticks_min,
host_ticks_max=host_ticks_max,
)
+56
View File
@@ -0,0 +1,56 @@
"""Wall-clock helpers for Medulla tick filtering.
Two tick conventions appear in this dataset:
- LiDAR DObject ``tic`` / recovered ``indices.log``: ``DateTime.Now.Ticks`` (local)
- IMU / MSOP payload host receive fields: UTC ``DateTime.UtcNow.Ticks``
"""
from __future__ import annotations
from datetime import datetime, timedelta, timezone
TICKS_PER_SECOND = 10_000_000
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
def _parse_local_wall(text: str) -> datetime:
normalized = text.strip().replace(" ", "T")
if normalized.endswith("Z"):
raise ValueError("expected local wall time without Z; got UTC marker")
if "+" in normalized[10:]:
idx = normalized.find("+", 10)
normalized = normalized[:idx]
elif normalized.count("-") > 2:
# timezone like -08:00 after the date
idx = normalized.find("-", 10)
if idx > 0 and ":" in normalized[idx + 1 :]:
normalized = normalized[:idx]
return datetime.fromisoformat(normalized).replace(tzinfo=None)
def local_wall_to_dotnet_ticks(text: str) -> int:
"""Local wall time → ``DateTime.Now.Ticks`` (LiDAR DObject tic)."""
dt = _parse_local_wall(text)
delta = dt - datetime(1, 1, 1)
return int(delta.total_seconds() * TICKS_PER_SECOND)
def local_wall_to_utc_dotnet_ticks(text: str, *, tz_hours: float = 8.0) -> int:
"""Local wall time in ``tz_hours`` → UTC ``DateTime.UtcNow.Ticks`` (IMU host)."""
dt = _parse_local_wall(text).replace(tzinfo=timezone(timedelta(hours=tz_hours)))
unix = dt.timestamp()
return int(round(unix * TICKS_PER_SECOND)) + DOTNET_UNIX_EPOCH_TICKS
def dotnet_ticks_to_local_iso(ticks: int) -> str:
dt = datetime(1, 1, 1) + timedelta(microseconds=ticks / 10.0)
return dt.isoformat(timespec="milliseconds")
def utc_dotnet_ticks_to_unix_s(ticks: int) -> float:
"""UTC ``DateTime.UtcNow.Ticks`` → Unix seconds."""
return (float(ticks) - float(DOTNET_UNIX_EPOCH_TICKS)) / float(TICKS_PER_SECOND)