431 lines
16 KiB
Python
431 lines
16 KiB
Python
#!/usr/bin/env python3
|
||
"""Export IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format.
|
||
|
||
IMU sources:
|
||
- ``--imu-kind hi13`` (HI13R4 / HI91) or ``n300`` or ``auto``
|
||
- one or more ``--imu-rscap`` files (concatenated)
|
||
|
||
LiDAR sources (exactly one):
|
||
- ``--lidar-dlog``: Medulla dlog dir **or recovered zip** (MSOP + DIFOP)
|
||
- ``--lidar-rscap``: legacy H32 MSOP V2 ``.rscap``
|
||
|
||
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/
|
||
frames_index.csv
|
||
frames/frame_XXXXX.npz
|
||
export_summary.json
|
||
|
||
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
|
||
|
||
import argparse
|
||
import csv
|
||
import json
|
||
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 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.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, 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",
|
||
"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"{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,
|
||
]
|
||
)
|
||
|
||
|
||
def write_lidar_session(root: Path, frames) -> dict:
|
||
frames_dir = root / "frames"
|
||
frames_dir.mkdir(parents=True, exist_ok=True)
|
||
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",
|
||
"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,
|
||
"t_start": float(frames[0].t_start_s) if frames else None,
|
||
"t_end": float(frames[-1].t_end_s) if frames else None,
|
||
}
|
||
|
||
|
||
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: 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,
|
||
max_range_m: float = 120.0,
|
||
min_frame_points: int = 100,
|
||
) -> dict:
|
||
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)
|
||
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, samples)
|
||
imu_host_ok = sum(1 for sample in samples if sample.host_receive_utc_ticks > 0)
|
||
|
||
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,
|
||
max_range_m=max_range_m,
|
||
max_points_per_frame=max_points_per_frame,
|
||
vertical_deg=session.vertical_deg,
|
||
horizontal_deg=session.horizontal_deg,
|
||
)
|
||
lidar_meta = {
|
||
"source": "dlog",
|
||
"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": (
|
||
"device: h32_msop_device_timestamp -> seconds; "
|
||
"host: MSOP HostReceiveUtcTicks -> unix seconds"
|
||
),
|
||
}
|
||
else:
|
||
assert lidar_rscap is not None
|
||
lidar_capture = read_capture(lidar_rscap)
|
||
frames = iter_h32_frames(
|
||
lidar_capture,
|
||
min_frame_points=min_frame_points,
|
||
frame_stride=frame_stride,
|
||
min_range_m=min_range_m,
|
||
max_range_m=max_range_m,
|
||
max_points_per_frame=max_points_per_frame,
|
||
)
|
||
lidar_meta = {
|
||
"source": "rscap_v2",
|
||
"lidar_rscap": str(lidar_rscap),
|
||
"capture": file_summary(lidar_capture),
|
||
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
|
||
"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(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_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,
|
||
"captures": imu_captures,
|
||
},
|
||
"lidar": {
|
||
**lidar_stats,
|
||
"frame_stride": int(frame_stride),
|
||
"max_points_per_frame": max_points_per_frame,
|
||
**lidar_meta,
|
||
},
|
||
"outputs": {
|
||
"imu_csv": str(imu_csv),
|
||
"lidar_session": str(lidar_dir),
|
||
},
|
||
}
|
||
(out / "export_summary.json").write_text(
|
||
json.dumps(summary, indent=2, ensure_ascii=False) + "\n",
|
||
encoding="utf-8",
|
||
)
|
||
return summary
|
||
|
||
|
||
def main() -> int:
|
||
parser = argparse.ArgumentParser(description=__doc__)
|
||
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 dlog directory or recovered zip (indices.log + data.bin)",
|
||
)
|
||
lidar.add_argument(
|
||
"--lidar-rscap",
|
||
type=Path,
|
||
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)
|
||
args = parser.parse_args()
|
||
max_points = None if args.max_points_per_frame <= 0 else args.max_points_per_frame
|
||
summary = export_session(
|
||
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,
|
||
min_range_m=args.min_range_m,
|
||
max_range_m=args.max_range_m,
|
||
min_frame_points=args.min_frame_points,
|
||
)
|
||
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"),
|
||
},
|
||
ensure_ascii=False,
|
||
indent=2,
|
||
)
|
||
)
|
||
if summary["imu"]["samples"] == 0:
|
||
raise SystemExit("no valid IMU samples decoded in window")
|
||
if summary["lidar"]["frames"] == 0:
|
||
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
|
||
|
||
|
||
if __name__ == "__main__":
|
||
raise SystemExit(main())
|