#!/usr/bin/env python3 """Export N300 IMU + H32 LiDAR captures to Lidar-IMU V1 intermediate format. Supported LiDAR sources (exactly one required): - ``--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) Output layout under --out: imu.csv lidar/ frames_index.csv 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. """ 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.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 def write_imu_csv(path: Path, t: np.ndarray, gyro: np.ndarray, accel: np.ndarray) -> 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( [ 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}", ] ) 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"]) point_counts = [] 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)) writer.writerow( [ index, rel, f"{frame.t_start_s:.9f}", f"{frame.t_end_s:.9f}", ] ) point_counts.append(int(frame.points_xyz.shape[0])) return { "frames": len(frames), "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 export_session( *, imu_rscap: Path, out: Path, lidar_rscap: Path | None = None, lidar_dlog: Path | None = None, msop_object: str = "frontlidar-msop-raw", difop_object: str = "frontlidar-difop-raw", require_difop: bool = False, 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") 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) imu_csv = out / "imu.csv" write_imu_csv(imu_csv, t, gyro, accel) 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, ) frames = iter_h32_frames_from_packets( session.msop_packets, 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": str(session.dlog_root), "msop_object": session.msop_object, "difop_object": session.difop_object, "msop_packets": len(session.msop_packets), "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)", } 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": "h32_msop_device_timestamp_ms -> seconds", } lidar_dir = out / "lidar" lidar_stats = write_lidar_session(lidar_dir, frames) summary = { "imu_rscap": str(imu_rscap), "out": str(out), "timestamp_policy": { "imu": "n300_device_timestamp_us -> seconds", "lidar": lidar_meta["timestamp_note"], "host_utc": "not used as calibration timeline", }, "imu": { "samples": int(t.shape[0]), "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), }, "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, required=True, help="N300 V2 .rscap") lidar = parser.add_mutually_exclusive_group(required=True) lidar.add_argument( "--lidar-dlog", type=Path, help="H32 Medulla dlog root (dobject/ + dobject_recording/), preferred", ) 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", ) 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, msop_object=args.msop_object, difop_object=args.difop_object, require_difop=args.require_difop, 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_samples": summary["imu"]["samples"], "lidar_frames": summary["lidar"]["frames"], "lidar_source": summary["lidar"]["source"], "angle_source": summary["lidar"]["angle_source"], "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 N300 IMU samples decoded") if summary["lidar"]["frames"] == 0: raise SystemExit("no valid H32 frames decoded") return 0 if __name__ == "__main__": raise SystemExit(main())