新增 N300/H32 rscap 到 V1 中间格式的导出工具与单元测试
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -0,0 +1,191 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Export N300 IMU + H32 MSOP V2 .rscap files to Lidar-IMU V1 intermediate format.
|
||||
|
||||
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.rscap_v2.capture_format_v2 import file_summary, read_capture
|
||||
from tools.rscap_v2.h32_msop import iter_h32_frames
|
||||
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,
|
||||
lidar_rscap: Path,
|
||||
out: Path,
|
||||
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:
|
||||
out.mkdir(parents=True, exist_ok=True)
|
||||
imu_capture = read_capture(imu_rscap)
|
||||
lidar_capture = read_capture(lidar_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)
|
||||
|
||||
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_dir = out / "lidar"
|
||||
lidar_stats = write_lidar_session(lidar_dir, frames)
|
||||
|
||||
summary = {
|
||||
"imu_rscap": str(imu_rscap),
|
||||
"lidar_rscap": str(lidar_rscap),
|
||||
"out": str(out),
|
||||
"timestamp_policy": {
|
||||
"imu": "n300_device_timestamp_us -> seconds",
|
||||
"lidar": "h32_msop_device_timestamp_ms -> seconds (t_start/t_end per frame)",
|
||||
"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,
|
||||
"capture": file_summary(lidar_capture),
|
||||
"angle_source": "default_msop_only_vertical_-16_to_16_deg",
|
||||
},
|
||||
"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")
|
||||
parser.add_argument("--lidar-rscap", type=Path, required=True, help="H32 MSOP V2 .rscap")
|
||||
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,
|
||||
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"],
|
||||
"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())
|
||||
Reference in New Issue
Block a user