lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
tools/export_rscap_to_v1.py
+212 -63
View File
@@ -1,13 +1,18 @@
#!/usr/bin/env python3
"""Export N300 IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format.
"""Export IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format.
Supported LiDAR sources (exactly one required):
IMU sources:
- ``--imu-kind hi13`` (HI13R4 / HI91) or ``n300`` or ``auto``
- one or more ``--imu-rscap`` files (concatenated)
- ``--lidar-dlog``: Medulla dlog from ``RSLidarH32_3D_DLogCaptureNet48``
(raw MSOP + DIFOP DObjects; preferred for new recordings)
- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap`` (MSOP-only defaults for angles)
LiDAR sources (exactly one):
- ``--lidar-dlog``: Medulla dlog dir **or recovered zip** (MSOP + DIFOP)
- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap``
Output layout under --out:
Optional host-time window (local wall clock, DateTime.Now.Ticks convention):
- ``--host-start`` / ``--host-end`` e.g. ``2026-08-08T17:40:05``
Output under ``--out``:
imu.csv
lidar/
@@ -15,8 +20,9 @@ Output layout under --out:
frames/frame_XXXXX.npz
export_summary.json
Timestamps written into the intermediate format are **device times**
(N300 device_timestamp_us, H32 MSOP device timestamp), not host receive time.
Device times stay in ``t`` / ``t_start``/``t_end``. Host UTC receive times are
also written so LiDAR–IMU alignment can bridge clocks without forcing first-frame
device coincidence.
"""
from __future__ import annotations
@@ -34,26 +40,48 @@ if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from tools.h32_dlog.load_session import load_h32_dlog_lidar
from tools.h32_dlog.timeutil import (
local_wall_to_dotnet_ticks,
local_wall_to_utc_dotnet_ticks,
utc_dotnet_ticks_to_unix_s,
)
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
from tools.rscap_v2.h32_msop import iter_h32_frames, iter_h32_frames_from_packets
from tools.rscap_v2.n300_imu import iter_n300_imu_samples, samples_to_arrays
from tools.rscap_v2.hi13_imu import iter_hi13_imu_samples
from tools.rscap_v2.n300_imu import ImuSample, iter_n300_imu_samples, samples_to_arrays
def write_imu_csv(path: Path, t: np.ndarray, gyro: np.ndarray, accel: np.ndarray) -> None:
def write_imu_csv(path: Path, samples: list[ImuSample]) -> None:
path.parent.mkdir(parents=True, exist_ok=True)
with path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.writer(handle)
writer.writerow(["t", "gx", "gy", "gz", "ax", "ay", "az"])
for index in range(t.shape[0]):
writer.writerow(
[
"t",
"gx",
"gy",
"gz",
"ax",
"ay",
"az",
"t_host_utc_s",
"receive_utc_ticks",
]
)
for sample in samples:
ticks = int(sample.host_receive_utc_ticks)
t_host = utc_dotnet_ticks_to_unix_s(ticks) if ticks > 0 else float("nan")
writer.writerow(
[
f"{t[index]:.9f}",
f"{gyro[index, 0]:.12g}",
f"{gyro[index, 1]:.12g}",
f"{gyro[index, 2]:.12g}",
f"{accel[index, 0]:.12g}",
f"{accel[index, 1]:.12g}",
f"{accel[index, 2]:.12g}",
f"{sample.t_s:.9f}",
f"{sample.gyro_rad_s[0]:.12g}",
f"{sample.gyro_rad_s[1]:.12g}",
f"{sample.gyro_rad_s[2]:.12g}",
f"{sample.accel_m_s2[0]:.12g}",
f"{sample.accel_m_s2[1]:.12g}",
f"{sample.accel_m_s2[2]:.12g}",
f"{t_host:.9f}" if ticks > 0 else "",
ticks,
]
)
@@ -64,22 +92,45 @@ def write_lidar_session(root: Path, frames) -> dict:
index_path = root / "frames_index.csv"
with index_path.open("w", newline="", encoding="utf-8") as handle:
writer = csv.writer(handle)
writer.writerow(["frame_id", "filename", "t_start", "t_end"])
writer.writerow(
[
"frame_id",
"filename",
"t_start",
"t_end",
"host_receive_utc_ticks",
"t_host_utc_s",
"host_receive_utc_end_ticks",
"t_host_utc_end_s",
]
)
point_counts = []
host_ok = 0
for index, frame in enumerate(frames):
rel = f"frames/frame_{index:05d}.npz"
np.savez_compressed(root / rel, points=np.asarray(frame.points_xyz, dtype=np.float32))
h0 = int(getattr(frame, "host_receive_utc_ticks_start", 0) or 0)
h1 = int(getattr(frame, "host_receive_utc_ticks_end", 0) or 0)
t_host0 = utc_dotnet_ticks_to_unix_s(h0) if h0 > 0 else float("nan")
t_host1 = utc_dotnet_ticks_to_unix_s(h1) if h1 > 0 else float("nan")
if h0 > 0:
host_ok += 1
writer.writerow(
[
index,
rel,
f"{frame.t_start_s:.9f}",
f"{frame.t_end_s:.9f}",
h0,
f"{t_host0:.9f}" if h0 > 0 else "",
h1,
f"{t_host1:.9f}" if h1 > 0 else "",
]
)
point_counts.append(int(frame.points_xyz.shape[0]))
return {
"frames": len(frames),
"frames_with_host_utc": host_ok,
"points_min": int(min(point_counts)) if point_counts else 0,
"points_max": int(max(point_counts)) if point_counts else 0,
"points_mean": float(np.mean(point_counts)) if point_counts else 0.0,
@@ -88,15 +139,63 @@ def write_lidar_session(root: Path, frames) -> dict:
}
def detect_imu_kind(paths: list[Path], explicit: str) -> str:
if explicit != "auto":
return explicit
joined = " ".join(path.name.lower() for path in paths)
if "hi13" in joined or "hipnuc" in joined:
return "hi13"
if "n300" in joined or "wheeltec" in joined:
return "n300"
return "hi13"
def load_imu_samples(
paths: list[Path],
*,
kind: str,
host_ticks_min: int | None,
host_ticks_max: int | None,
) -> tuple[list[ImuSample], list[dict], str]:
samples: list[ImuSample] = []
captures_meta: list[dict] = []
for path in paths:
capture = read_capture(path)
captures_meta.append(file_summary(capture))
if kind == "hi13":
part = iter_hi13_imu_samples(
capture,
host_utc_ticks_min=host_ticks_min,
host_utc_ticks_max=host_ticks_max,
)
elif kind == "n300":
part = iter_n300_imu_samples(capture)
if host_ticks_min is not None or host_ticks_max is not None:
part = [
sample
for sample in part
if (host_ticks_min is None or sample.host_receive_utc_ticks >= host_ticks_min)
and (host_ticks_max is None or sample.host_receive_utc_ticks <= host_ticks_max)
]
else:
raise ValueError(f"unsupported imu kind: {kind}")
samples.extend(part)
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
return samples, captures_meta, kind
def export_session(
*,
imu_rscap: Path,
imu_rscap: list[Path] | Path,
out: Path,
lidar_rscap: Path | None = None,
lidar_dlog: Path | None = None,
imu_kind: str = "auto",
msop_object: str = "frontlidar-msop-raw",
difop_object: str = "frontlidar-difop-raw",
require_difop: bool = False,
host_start: str | None = None,
host_end: str | None = None,
frame_stride: int = 1,
max_points_per_frame: int | None = 80000,
min_range_m: float = 0.3,
@@ -106,24 +205,41 @@ def export_session(
if (lidar_rscap is None) == (lidar_dlog is None):
raise ValueError("provide exactly one of lidar_rscap or lidar_dlog")
imu_paths = [imu_rscap] if isinstance(imu_rscap, Path) else list(imu_rscap)
if not imu_paths:
raise ValueError("at least one --imu-rscap is required")
# LiDAR DObject tic uses DateTime.Now; IMU/MSOP host fields use UTC.
lidar_ticks_min = local_wall_to_dotnet_ticks(host_start) if host_start else None
lidar_ticks_max = local_wall_to_dotnet_ticks(host_end) if host_end else None
imu_ticks_min = local_wall_to_utc_dotnet_ticks(host_start) if host_start else None
imu_ticks_max = local_wall_to_utc_dotnet_ticks(host_end) if host_end else None
kind = detect_imu_kind(imu_paths, imu_kind)
out.mkdir(parents=True, exist_ok=True)
imu_capture = read_capture(imu_rscap)
samples = iter_n300_imu_samples(imu_capture)
t, gyro, accel = samples_to_arrays(samples)
samples, imu_captures, kind = load_imu_samples(
imu_paths,
kind=kind,
host_ticks_min=imu_ticks_min,
host_ticks_max=imu_ticks_max,
)
t, _gyro, _accel = samples_to_arrays(samples)
imu_csv = out / "imu.csv"
write_imu_csv(imu_csv, t, gyro, accel)
write_imu_csv(imu_csv, samples)
imu_host_ok = sum(1 for sample in samples if sample.host_receive_utc_ticks > 0)
lidar_meta: dict
if lidar_dlog is not None:
session = load_h32_dlog_lidar(
lidar_dlog,
msop_object=msop_object,
difop_object=difop_object,
require_difop=require_difop,
host_ticks_min=lidar_ticks_min,
host_ticks_max=lidar_ticks_max,
)
frames = iter_h32_frames_from_packets(
session.msop_packets,
host_utc_ticks=session.msop_host_utc_ticks,
min_frame_points=min_frame_points,
frame_stride=frame_stride,
min_range_m=min_range_m,
@@ -134,16 +250,20 @@ def export_session(
)
lidar_meta = {
"source": "dlog",
"lidar_dlog": str(session.dlog_root),
"lidar_dlog": session.dlog_root,
"msop_object": session.msop_object,
"difop_object": session.difop_object,
"msop_packets": len(session.msop_packets),
"msop_packets_with_host_utc": sum(1 for ticks in session.msop_host_utc_ticks if ticks > 0),
"msop_batches": session.msop_batch_count,
"difop_records": session.difop_record_count,
"session_id": session.session_id,
"lidar_ip": session.lidar_ip,
"angle_source": session.angle_source,
"timestamp_note": "h32_msop_device_timestamp -> seconds (from MSOP bytes)",
"timestamp_note": (
"device: h32_msop_device_timestamp -> seconds; "
"host: MSOP HostReceiveUtcTicks -> unix seconds"
),
}
else:
assert lidar_rscap is not None
@@ -161,25 +281,46 @@ def export_session(
"lidar_rscap": str(lidar_rscap),
"capture": file_summary(lidar_capture),
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
"timestamp_note": "h32_msop_device_timestamp_ms -> seconds",
"timestamp_note": (
"device: h32_msop_device_timestamp_ms -> seconds; "
"host: rscap receive_utc_ticks -> unix seconds"
),
}
lidar_dir = out / "lidar"
lidar_stats = write_lidar_session(lidar_dir, frames)
imu_time_note = (
"hi13_device_timestamp_ms -> seconds"
if kind == "hi13"
else "n300_device_timestamp_us -> seconds"
)
summary = {
"imu_rscap": str(imu_rscap),
"imu_rscap": [str(path) for path in imu_paths],
"imu_kind": kind,
"out": str(out),
"host_window": {
"host_start": host_start,
"host_end": host_end,
"lidar_ticks_min": lidar_ticks_min,
"lidar_ticks_max": lidar_ticks_max,
"imu_ticks_min": imu_ticks_min,
"imu_ticks_max": imu_ticks_max,
"note": "local wall cut; lidar DObject tic=DateTime.Now, IMU/MSOP host=UTC",
},
"timestamp_policy": {
"imu": "n300_device_timestamp_us -> seconds",
"lidar": lidar_meta["timestamp_note"],
"host_utc": "not used as calibration timeline",
"imu_device": imu_time_note,
"imu_host": "rscap receive_utc_ticks -> t_host_utc_s",
"lidar_device": "MSOP device timestamp -> t_start/t_end",
"lidar_host": "MSOP HostReceiveUtcTicks -> t_host_utc_s",
"calibration_align": "bridge via host UTC; do not force first device samples to coincide",
},
"imu": {
"samples": int(t.shape[0]),
"samples_with_host_utc": imu_host_ok,
"t_start": float(t[0]) if t.size else None,
"t_end": float(t[-1]) if t.size else None,
"capture": file_summary(imu_capture),
"captures": imu_captures,
},
"lidar": {
**lidar_stats,
@@ -201,41 +342,38 @@ def export_session(
def main() -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--imu-rscap", type=Path, required=True, help="N300 V2 .rscap")
parser.add_argument(
"--imu-rscap",
type=Path,
action="append",
required=True,
help="IMU V2 .rscap (repeatable)",
)
parser.add_argument(
"--imu-kind",
choices=("auto", "hi13", "n300"),
default="auto",
help="IMU decoder (default: auto from filename)",
)
lidar = parser.add_mutually_exclusive_group(required=True)
lidar.add_argument(
"--lidar-dlog",
type=Path,
help="H32 Medulla dlog root (dobject/ + dobject_recording/), preferred",
help="H32 dlog directory or recovered zip (indices.log + data.bin)",
)
lidar.add_argument(
"--lidar-rscap",
type=Path,
help="Legacy H32 MSOP V2 .rscap (no DIFOP; default vertical angles)",
)
parser.add_argument(
"--msop-object",
default="frontlidar-msop-raw",
help="DObject name for raw MSOP batches (dlog path)",
)
parser.add_argument(
"--difop-object",
default="frontlidar-difop-raw",
help="DObject name for raw DIFOP packets (dlog path)",
)
parser.add_argument(
"--require-difop",
action="store_true",
help="Fail if dlog has no valid DIFOP channel angles",
)
parser.add_argument("--out", type=Path, required=True, help="Output session directory")
parser.add_argument("--frame-stride", type=int, default=1, help="Keep every N-th LiDAR frame")
parser.add_argument(
"--max-points-per-frame",
type=int,
default=80000,
help="Uniform downsample cap per frame; 0 disables",
help="Legacy H32 MSOP V2 .rscap",
)
parser.add_argument("--msop-object", default="frontlidar-msop-raw")
parser.add_argument("--difop-object", default="frontlidar-difop-raw")
parser.add_argument("--require-difop", action="store_true")
parser.add_argument("--host-start", type=str, default=None, help="Local wall start, e.g. 2026-08-08T17:40:05")
parser.add_argument("--host-end", type=str, default=None, help="Local wall end, e.g. 2026-08-08T17:45:15")
parser.add_argument("--out", type=Path, required=True)
parser.add_argument("--frame-stride", type=int, default=1)
parser.add_argument("--max-points-per-frame", type=int, default=80000)
parser.add_argument("--min-range-m", type=float, default=0.3)
parser.add_argument("--max-range-m", type=float, default=120.0)
parser.add_argument("--min-frame-points", type=int, default=100)
@@ -245,9 +383,12 @@ def main() -> int:
imu_rscap=args.imu_rscap,
lidar_rscap=args.lidar_rscap,
lidar_dlog=args.lidar_dlog,
imu_kind=args.imu_kind,
msop_object=args.msop_object,
difop_object=args.difop_object,
require_difop=args.require_difop,
host_start=args.host_start,
host_end=args.host_end,
out=args.out,
frame_stride=args.frame_stride,
max_points_per_frame=max_points,
@@ -258,10 +399,14 @@ def main() -> int:
print(
json.dumps(
{
"imu_kind": summary["imu_kind"],
"imu_samples": summary["imu"]["samples"],
"imu_host_utc": summary["imu"]["samples_with_host_utc"],
"lidar_frames": summary["lidar"]["frames"],
"lidar_host_utc": summary["lidar"]["frames_with_host_utc"],
"lidar_source": summary["lidar"]["source"],
"angle_source": summary["lidar"]["angle_source"],
"host_window": summary["host_window"],
"imu_csv": summary["outputs"]["imu_csv"],
"lidar_session": summary["outputs"]["lidar_session"],
"export_summary": str(Path(args.out) / "export_summary.json"),
@@ -271,9 +416,13 @@ def main() -> int:
)
)
if summary["imu"]["samples"] == 0:
raise SystemExit("no valid N300 IMU samples decoded")
raise SystemExit("no valid IMU samples decoded in window")
if summary["lidar"]["frames"] == 0:
raise SystemExit("no valid H32 frames decoded")
raise SystemExit("no valid H32 frames decoded in window")
if summary["lidar"]["frames_with_host_utc"] == 0:
raise SystemExit("no LiDAR frames with MSOP HostReceiveUtcTicks; cannot host-bridge align")
if summary["imu"]["samples_with_host_utc"] == 0:
raise SystemExit("no IMU samples with host receive UTC; cannot host-bridge align")
return 0
tools/export_usable_20260808_windows.py
+125
View File
@@ -0,0 +1,125 @@
#!/usr/bin/env python3
"""Export priority LiDAR–IMU windows from calibration_usable_20260808.
Does not push anything; writes local V1 sessions under --out-root.
"""
from __future__ import annotations
import argparse
import json
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from tools.export_rscap_to_v1 import export_session
DEFAULT_DATA = Path(r"D:\data\calibration_usable_20260808")
LIDAR_ZIP = "lidar_dlog/dorec_recovered_20260808_171438_181422.zip"
IMU_MAIN = "imu_rscap/hi13r4-imu_20260808-092827.638_39783edb-e46e-4b28-a5e8-427b981c2fce.rscap"
IMU_TAIL = "imu_rscap/hi13r4-imu_20260808-101022.036_87ea5edc-cd3d-4192-809a-469fbc8cac01.rscap"
# From usable-segment chart (local wall clock).
WINDOWS = [
{
"name": "priority_174005_174515",
"host_start": "2026-08-08T17:40:05",
"host_end": "2026-08-08T17:45:15",
"imu": [IMU_MAIN],
"priority": True,
},
{
"name": "priority_174905_175450",
"host_start": "2026-08-08T17:49:05",
"host_end": "2026-08-08T17:54:50",
"imu": [IMU_MAIN],
"priority": True,
},
{
"name": "priority_175910_180530",
"host_start": "2026-08-08T17:59:10",
"host_end": "2026-08-08T18:05:30",
"imu": [IMU_MAIN],
"priority": True,
},
{
"name": "usable_181035_181050",
"host_start": "2026-08-08T18:10:35",
"host_end": "2026-08-08T18:10:50",
"imu": [IMU_TAIL],
"priority": False,
},
{
"name": "usable_181225_181300",
"host_start": "2026-08-08T18:12:25",
"host_end": "2026-08-08T18:13:00",
"imu": [IMU_TAIL],
"priority": False,
},
{
"name": "usable_181350_181410",
"host_start": "2026-08-08T18:13:50",
"host_end": "2026-08-08T18:14:10",
"imu": [IMU_TAIL],
"priority": False,
},
]
def main() -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--data-root", type=Path, default=DEFAULT_DATA)
parser.add_argument(
"--out-root",
type=Path,
default=DEFAULT_DATA / "sessions_v1",
)
parser.add_argument("--priority-only", action="store_true", default=True)
parser.add_argument("--all-windows", action="store_true")
parser.add_argument("--frame-stride", type=int, default=5)
parser.add_argument("--max-points-per-frame", type=int, default=40000)
args = parser.parse_args()
priority_only = not args.all_windows
lidar = args.data_root / LIDAR_ZIP
if not lidar.is_file():
raise SystemExit(f"missing lidar zip: {lidar}")
selected = [w for w in WINDOWS if (not priority_only) or w["priority"]]
results = []
for window in selected:
out = args.out_root / window["name"]
imu_paths = [args.data_root / rel for rel in window["imu"]]
print(f"=== exporting {window['name']} ===", flush=True)
summary = export_session(
imu_rscap=imu_paths,
lidar_dlog=lidar,
imu_kind="hi13",
require_difop=True,
host_start=window["host_start"],
host_end=window["host_end"],
out=out,
frame_stride=args.frame_stride,
max_points_per_frame=args.max_points_per_frame,
)
brief = {
"name": window["name"],
"imu_samples": summary["imu"]["samples"],
"lidar_frames": summary["lidar"]["frames"],
"angle_source": summary["lidar"]["angle_source"],
"out": str(out),
}
results.append(brief)
print(json.dumps(brief, ensure_ascii=False, indent=2), flush=True)
manifest = args.out_root / "export_windows_manifest.json"
args.out_root.mkdir(parents=True, exist_ok=True)
manifest.write_text(json.dumps(results, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
print(f"manifest: {manifest}")
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/h32_dlog/__init__.py
+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",
tools/h32_dlog/dobject.py
+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,
)
tools/h32_dlog/load_session.py
+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,
)
tools/h32_dlog/timeutil.py
+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)
tools/rscap_v2/h32_msop.py
+41 -6
View File
@@ -15,7 +15,7 @@ and horizontal channel offsets default to 0.
from __future__ import annotations
from dataclasses import dataclass
from typing import Iterable
from typing import Iterable, Sequence
import numpy as np
@@ -66,6 +66,8 @@ class LidarFrameExport:
t_start_s: float
t_end_s: float
points_xyz: np.ndarray # (N, 3) metres
host_receive_utc_ticks_start: int = 0
host_receive_utc_ticks_end: int = 0
def decode_packet_points(
@@ -144,6 +146,7 @@ def _block_points(
def iter_h32_frames_from_packets(
packets: Iterable[bytes],
*,
host_utc_ticks: Sequence[int] | None = None,
min_frame_points: int = MIN_FRAME_POINTS_DEFAULT,
frame_stride: int = 1,
min_range_m: float = 0.3,
@@ -152,31 +155,49 @@ def iter_h32_frames_from_packets(
vertical_deg: np.ndarray | None = None,
horizontal_deg: np.ndarray | None = None,
) -> list[LidarFrameExport]:
"""Assemble raw MSOP packets into frames using the 270°→90° azimuth wrap."""
"""Assemble raw MSOP packets into frames using the 270°→90° azimuth wrap.
``host_utc_ticks`` is optional per-packet ``HostReceiveUtcTicks`` from the
MSOP DLog payload (UTC DateTime ticks). When provided, each emitted frame
carries host receive start/end ticks from the first/last contributing packet.
"""
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},)")
packet_list = list(packets)
host_list = list(host_utc_ticks) if host_utc_ticks is not None else None
if host_list is not None and len(host_list) != len(packet_list):
raise ValueError(
f"host_utc_ticks length {len(host_list)} != packets length {len(packet_list)}"
)
frames: list[LidarFrameExport] = []
point_chunks: list[np.ndarray] = []
t_start: float | None = None
t_end: float | None = None
host_start: int | None = None
host_end: int | 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
nonlocal point_chunks, t_start, t_end, host_start, host_end, kept
if not point_chunks or t_start is None or t_end is None:
point_chunks = []
t_start = t_end = None
host_start = host_end = None
return
points = np.vstack(point_chunks)
point_chunks = []
start_s, end_s = t_start, t_end
h0 = int(host_start or 0)
h1 = int(host_end or 0)
t_start = t_end = None
host_start = host_end = None
if points.shape[0] < min_frame_points:
return
if kept % stride != 0:
@@ -188,12 +209,21 @@ def iter_h32_frames_from_packets(
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))
frames.append(
LidarFrameExport(
t_start_s=start_s,
t_end_s=end_s,
points_xyz=points,
host_receive_utc_ticks_start=h0,
host_receive_utc_ticks_end=h1,
)
)
for packet in packets:
for index, packet in enumerate(packet_list):
if len(packet) != PACKET_LENGTH:
continue
packet_t = device_timestamp_ms(packet) * 1e-3
packet_host = int(host_list[index]) if host_list is not None else 0
unit = distance_unit_mm(packet)
idx = DATA_START
for _block in range(BLOCKS):
@@ -216,7 +246,9 @@ def iter_h32_frames_from_packets(
if pts.shape[0]:
if t_start is None:
t_start = packet_t
host_start = packet_host
t_end = packet_t
host_end = packet_host
point_chunks.append(pts)
idx += BLOCK_LENGTH
@@ -237,8 +269,11 @@ def iter_h32_frames(
) -> list[LidarFrameExport]:
"""Assemble MSOP packets from a V2 .rscap capture into frames."""
packets = [chunk.raw for chunk in capture.chunks]
host_ticks = [chunk.receive_utc_ticks for chunk in capture.chunks]
return iter_h32_frames_from_packets(
(chunk.raw for chunk in capture.chunks),
packets,
host_utc_ticks=host_ticks,
min_frame_points=min_frame_points,
frame_stride=frame_stride,
min_range_m=min_range_m,
tools/rscap_v2/hi13_imu.py
+136
View File
@@ -0,0 +1,136 @@
"""Decode Hipnuc / HI13 (HI91/HI92) IMU frames from a V2 .rscap capture.
Matches ``EcarSensorMinimal/RawSerialImu/Hi13Protocol.cs``:
sync ``5A A5``, CRC16 over header[0:4]+payload, tag ``0x91`` / ``0x92``.
HI91 (preferred for calibration):
- accel: float32 in g → m/s² (* 9.80665)
- gyro: float32 in deg/s → rad/s
- device time: uint32 ms at frame offset 14 → ``t_s = ms * 1e-3``
"""
from __future__ import annotations
import struct
import numpy as np
from .capture_format_v2 import CaptureFile
from .n300_imu import ImuSample, samples_to_arrays
G0 = 9.80665
DEG2RAD = np.pi / 180.0
def crc16_hi13(frame: bytes, payload_length: int) -> int:
crc = 0
for value in frame[:4]:
crc = _update_crc16(crc, value)
for value in frame[6 : 6 + payload_length]:
crc = _update_crc16(crc, value)
return crc & 0xFFFF
def _update_crc16(crc: int, value: int) -> int:
crc ^= (value & 0xFF) << 8
for _ in range(8):
if crc & 0x8000:
crc = ((crc << 1) ^ 0x1021) & 0xFFFF
else:
crc = (crc << 1) & 0xFFFF
return crc
def parse_hi91_frame(raw: bytes) -> tuple[tuple[float, float, float], tuple[float, float, float], int] | None:
"""Return (gyro_rad_s, accel_m_s2, device_timestamp_ms) for a CRC-valid HI91 frame."""
if len(raw) < 6 + 76:
return None
payload_length = raw[2] | (raw[3] << 8)
if payload_length < 76 or len(raw) < 6 + payload_length:
return None
if raw[6] != 0x91:
return None
expected = raw[4] | (raw[5] << 8)
if crc16_hi13(raw, payload_length) != expected:
return None
device_ms = struct.unpack_from("<I", raw, 14)[0]
ax, ay, az = struct.unpack_from("<fff", raw, 18)
gx, gy, gz = struct.unpack_from("<fff", raw, 30)
gyro = (gx * DEG2RAD, gy * DEG2RAD, gz * DEG2RAD)
accel = (ax * G0, ay * G0, az * G0)
return gyro, accel, int(device_ms)
def iter_hi13_imu_samples(
capture: CaptureFile,
*,
host_utc_ticks_min: int | None = None,
host_utc_ticks_max: int | None = None,
) -> list[ImuSample]:
"""Return CRC-valid HI91 samples sorted by device timestamp.
Streams chunk-by-chunk (no giant join) and can skip whole chunks outside the
host UTC receive window before parsing.
"""
samples: list[ImuSample] = []
carry = b""
for chunk in capture.chunks:
if host_utc_ticks_min is not None and chunk.receive_utc_ticks < host_utc_ticks_min:
carry = b""
continue
if host_utc_ticks_max is not None and chunk.receive_utc_ticks > host_utc_ticks_max:
# chunks are time-ordered; remaining ones are later
if chunk.receive_utc_ticks > host_utc_ticks_max:
break
stream = carry + chunk.raw
cursor = 0
while cursor + 6 < len(stream):
sync = stream.find(b"\x5A\xA5", cursor)
if sync < 0:
carry = b""
break
if sync + 6 > len(stream):
carry = stream[sync:]
break
payload_length = stream[sync + 2] | (stream[sync + 3] << 8)
if payload_length < 1 or payload_length > 512:
cursor = sync + 1
continue
end = sync + 6 + payload_length
if end > len(stream):
carry = stream[sync:]
break
parsed = parse_hi91_frame(stream[sync:end])
cursor = end
if parsed is None:
continue
gyro, accel, device_ms = parsed
host_ticks = chunk.receive_utc_ticks
if host_utc_ticks_min is not None and host_ticks < host_utc_ticks_min:
continue
if host_utc_ticks_max is not None and host_ticks > host_utc_ticks_max:
continue
samples.append(
ImuSample(
t_s=float(device_ms) * 1e-3,
gyro_rad_s=gyro,
accel_m_s2=accel,
host_receive_utc_ticks=host_ticks,
device_timestamp_us=int(device_ms) * 1000,
)
)
else:
carry = b""
samples.sort(key=lambda sample: (sample.t_s, sample.device_timestamp_us))
return samples
__all__ = [
"ImuSample",
"crc16_hi13",
"iter_hi13_imu_samples",
"parse_hi91_frame",
"samples_to_arrays",
]
tools/run_joint_three_windows_full_se3.py
+50
View File
@@ -0,0 +1,50 @@
#!/usr/bin/env python3
"""Joint full_se3 on the three host-aligned priority windows."""
from __future__ import annotations
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from imu_lidar.cli import main as cli_main
ALIGNED = Path(r"D:\data\calibration_usable_20260808\sessions_v1_host_aligned")
SESSIONS = [
"priority_174005_174515",
"priority_174905_175450",
"priority_175910_180530",
]
def main() -> int:
out = ALIGNED / "joint_full_se3"
argv = [
"run",
"--vehicle-config",
str(ROOT / "config" / "vehicle_hi13_h32_20260808.yaml"),
"--output",
str(out),
"--mode",
"full_se3",
"--time-offset-search-s",
"0.5",
"--min-pair-rotation-deg",
"2.0",
]
for name in SESSIONS:
session = ALIGNED / name
if not (session / "imu.csv").is_file() or not (session / "lidar" / "frames_index.csv").is_file():
raise SystemExit(f"missing host-aligned session: {session}")
argv.extend(["--session-id", name])
argv.extend(["--imu", str(session / "imu.csv")])
argv.extend(["--lidar", str(session / "lidar")])
print("argv:", " ".join(argv), flush=True)
return cli_main(argv)
if __name__ == "__main__":
raise SystemExit(main())
tools/run_priority_windows_calibration.py
+292
View File
@@ -0,0 +1,292 @@
#!/usr/bin/env python3
"""Align HI13/H32 via host-UTC bridge, then run rotation_only.
Device clocks (HI13 boot ms vs H32 absolute) must NOT be forced to share a
first-sample epoch. Instead map each LiDAR frame onto the IMU device timeline
by interpolating IMU device time at the frame's MSOP HostReceiveUtcTicks.
Optional |ω| correlation then refines residual host/path delay.
"""
from __future__ import annotations
import argparse
import csv
import json
import shutil
import sys
from pathlib import Path
import numpy as np
ROOT = Path(__file__).resolve().parents[1]
if str(ROOT) not in sys.path:
sys.path.insert(0, str(ROOT))
from imu_lidar.cli import main as cli_main
from imu_lidar.geometry import so3_log
from imu_lidar.imu_io import load_imu_samples
from imu_lidar.lidar_io import load_lidar_frames
from imu_lidar.registration import estimate_frame_rotations
from imu_lidar.time_offset import _correlate_offset, _magnitude_series
SESSIONS = [
"priority_174005_174515",
"priority_174905_175450",
"priority_175910_180530",
]
def _read_imu_host_table(imu_csv: Path) -> tuple[np.ndarray, np.ndarray]:
rows = list(csv.DictReader(imu_csv.open(encoding="utf-8")))
if not rows:
raise RuntimeError(f"empty IMU csv: {imu_csv}")
if "t_host_utc_s" not in rows[0] or not rows[0].get("t_host_utc_s"):
raise RuntimeError(
f"{imu_csv} missing t_host_utc_s; re-export with HostReceiveUtcTicks support"
)
t_dev = np.asarray([float(row["t"]) for row in rows], dtype=np.float64)
t_host = np.asarray([float(row["t_host_utc_s"]) for row in rows], dtype=np.float64)
order = np.argsort(t_host)
return t_host[order], t_dev[order]
def _imu_device_at_host(t_host_query: np.ndarray, imu_host: np.ndarray, imu_dev: np.ndarray) -> np.ndarray:
"""Map host UTC seconds → IMU device seconds (linear interp, edge clamp)."""
return np.interp(t_host_query, imu_host, imu_dev)
def rewrite_lidar_index_host_bridge(
src_index: Path,
dst_index: Path,
*,
imu_host: np.ndarray,
imu_dev: np.ndarray,
residual_delta_s: float = 0.0,
) -> dict:
"""Rewrite LiDAR times onto IMU device clock via host UTC bridge.
For each frame:
t_host_mid = mid of MSOP host receive window
t_imu_mid = interp(IMU device @ t_host_mid) + residual_delta
keep device duration: t_start/t_end centered on t_imu_mid
"""
rows = list(csv.DictReader(src_index.open(encoding="utf-8")))
if not rows:
raise RuntimeError(f"empty frames_index: {src_index}")
if "t_host_utc_s" not in rows[0]:
raise RuntimeError(
f"{src_index} missing t_host_utc_s; re-export DLog with MSOP HostReceiveUtcTicks"
)
dst_index.parent.mkdir(parents=True, exist_ok=True)
offsets: list[float] = []
with dst_index.open("w", newline="", encoding="utf-8") as handle:
writer = csv.writer(handle)
writer.writerow(["frame_id", "filename", "t_start", "t_end"])
for row in rows:
t0 = float(row["t_start"])
t1 = float(row["t_end"])
host0 = row.get("t_host_utc_s") or ""
host1 = row.get("t_host_utc_end_s") or ""
if not host0:
raise RuntimeError(f"frame {row.get('frame_id')} missing t_host_utc_s")
h0 = float(host0)
h1 = float(host1) if host1 else h0
host_mid = 0.5 * (h0 + h1)
imu_mid = float(_imu_device_at_host(np.asarray([host_mid]), imu_host, imu_dev)[0])
imu_mid += residual_delta_s
duration = max(t1 - t0, 1e-3)
new0 = imu_mid - 0.5 * duration
new1 = imu_mid + 0.5 * duration
offsets.append(imu_mid - 0.5 * (t0 + t1))
writer.writerow(
[
row["frame_id"],
row["filename"],
f"{new0:.9f}",
f"{new1:.9f}",
]
)
arr = np.asarray(offsets, dtype=np.float64)
return {
"frames": len(offsets),
"bridge_offset_median_s": float(np.median(arr)),
"bridge_offset_mean_s": float(np.mean(arr)),
"bridge_offset_std_s": float(np.std(arr)),
"bridge_offset_min_s": float(np.min(arr)),
"bridge_offset_max_s": float(np.max(arr)),
"residual_delta_s": float(residual_delta_s),
}
def estimate_residual_delta(session_dir: Path, *, search_s: float = 5.0) -> tuple[float, float]:
imu = load_imu_samples(session_dir / "imu.csv")
frames = load_lidar_frames(session_dir / "lidar")
# Short pairs only — large stride anti-correlates with IMU |gyro|.
stride = 1 if len(frames) < 80 else 2
rotations, pair_times = estimate_frame_rotations(frames, stride=stride)
if len(rotations) < 8:
rotations, pair_times = estimate_frame_rotations(frames, stride=1)
lidar_t = []
lidar_w = []
for (t_a, t_b), rotation in zip(pair_times, rotations):
dt_pair = max(t_b - t_a, 1e-3)
omega = so3_log(rotation) / dt_pair
lidar_t.append(0.5 * (t_a + t_b))
lidar_w.append(omega)
imu_t, imu_mag = _magnitude_series(imu.t_s, imu.gyro_rad_s)
lidar_t_arr, lidar_mag = _magnitude_series(np.asarray(lidar_t), np.asarray(lidar_w))
delta, peak = _correlate_offset(
imu_t,
imu_mag,
lidar_t_arr,
lidar_mag,
search_s=search_s,
sample_hz=20.0,
)
return float(delta), float(peak)
def align_session(src: Path, dst: Path, *, residual_search_s: float = 5.0) -> dict:
if dst.exists():
shutil.rmtree(dst)
dst.mkdir(parents=True)
shutil.copy2(src / "imu.csv", dst / "imu.csv")
shutil.copytree(src / "lidar" / "frames", dst / "lidar" / "frames")
imu_host, imu_dev = _read_imu_host_table(src / "imu.csv")
bridge = rewrite_lidar_index_host_bridge(
src / "lidar" / "frames_index.csv",
dst / "lidar" / "frames_index.csv",
imu_host=imu_host,
imu_dev=imu_dev,
residual_delta_s=0.0,
)
residual_delta, residual_peak = estimate_residual_delta(dst, search_s=residual_search_s)
# Only apply residual when correlation is clearly positive; otherwise the
# host-UTC bridge alone is the trusted alignment (weak peaks are noise).
apply_residual = residual_peak >= 0.5 and abs(residual_delta) <= residual_search_s
applied = float(residual_delta) if apply_residual else 0.0
if apply_residual:
bridge = rewrite_lidar_index_host_bridge(
src / "lidar" / "frames_index.csv",
dst / "lidar" / "frames_index.csv",
imu_host=imu_host,
imu_dev=imu_dev,
residual_delta_s=applied,
)
meta = {
"source": str(src),
"aligned": str(dst),
"method": "host_utc_bridge",
"imu_host_span_s": [float(imu_host[0]), float(imu_host[-1])],
"imu_device_span_s": [float(imu_dev[0]), float(imu_dev[-1])],
"bridge": bridge,
"residual_delta_s": residual_delta,
"residual_peak": residual_peak,
"residual_applied_s": applied,
"residual_applied": apply_residual,
"note": (
"LiDAR t_* rewritten onto IMU device clock via MSOP/IMU HostReceiveUtc; "
"not first-device-sample coincidence. residual |omega| shift applied only if peak>=0.5."
),
}
(dst / "align_meta.json").write_text(
json.dumps(meta, indent=2, ensure_ascii=False) + "\n", encoding="utf-8"
)
return meta
def run_one(session_dir: Path, vehicle: Path, search_s: float) -> dict:
output = session_dir / "out"
if output.exists():
shutil.rmtree(output)
argv = [
"run",
"--session-id",
session_dir.name,
"--imu",
str(session_dir / "imu.csv"),
"--lidar",
str(session_dir / "lidar"),
"--vehicle-config",
str(vehicle),
"--output",
str(output),
"--mode",
"rotation_only",
"--time-offset-search-s",
str(search_s),
"--min-pair-rotation-deg",
"2.0",
]
code = cli_main(argv)
summary_path = output / "summary.json"
summary = {}
if summary_path.is_file():
summary = json.loads(summary_path.read_text(encoding="utf-8"))
t_block = summary.get("T_IMU_lidar") or {}
return {
"session": session_dir.name,
"exit_code": code,
"status": summary.get("status"),
"message": summary.get("message"),
"time_offset_s": summary.get("time_offset_s"),
"rotation_deg": t_block.get("rotation_deg") if isinstance(t_block, dict) else None,
"summary": str(summary_path) if summary_path.is_file() else None,
}
def main() -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument(
"--sessions-root",
type=Path,
default=Path(r"D:\data\calibration_usable_20260808\sessions_v1"),
)
parser.add_argument(
"--aligned-root",
type=Path,
default=Path(r"D:\data\calibration_usable_20260808\sessions_v1_host_aligned"),
)
parser.add_argument(
"--vehicle-config",
type=Path,
default=ROOT / "config" / "vehicle_hi13_h32_20260808.yaml",
)
parser.add_argument(
"--residual-search-s",
type=float,
default=5.0,
help="|ω| residual search after host bridge (seconds)",
)
parser.add_argument("--time-offset-search-s", type=float, default=1.0)
args = parser.parse_args()
results = []
for name in SESSIONS:
src = args.sessions_root / name
if not src.is_dir():
raise SystemExit(f"missing session: {src}")
aligned = args.aligned_root / name
print(f"=== align {name} ===", flush=True)
meta = align_session(src, aligned, residual_search_s=args.residual_search_s)
print(json.dumps(meta, ensure_ascii=False, indent=2), flush=True)
print(f"=== calibrate {name} ===", flush=True)
result = run_one(aligned, args.vehicle_config, args.time_offset_search_s)
results.append({"align": meta, **result})
print(json.dumps(result, ensure_ascii=False, indent=2), flush=True)
manifest = args.aligned_root / "calibration_manifest.json"
args.aligned_root.mkdir(parents=True, exist_ok=True)
manifest.write_text(json.dumps(results, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
print(f"manifest: {manifest}")
return 0
if __name__ == "__main__":
raise SystemExit(main())