282 lines
9.8 KiB
Python
282 lines
9.8 KiB
Python
#!/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())
|