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

新增原始数据一步导出到 combined：对齐 Lidar-IMU 导出入口，适配 H32/G90/N300

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-03 16:08:37 +08:00
co-authored by Cursor
parent 24eaa8508e
commit 13624b0be8
14 changed files with 1600 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/build_multisensor_npz.py
+136 -47
View File
@@ -1,9 +1,15 @@
#!/usr/bin/env python3
"""Build one LiDAR-centric NPZ per frame with matched RTK and an IMU window.
Inputs are LiDAR frame NPZ files from frontlidar_dlog_export.py and parsed
RTK/IMU JSONL files from parse_rtk_imu_v2.py. Raw .rscap files remain the
traceability source; this script never modifies them.
Inputs are LiDAR frame NPZ files from ``export_h32_rscap_station.py`` (or legacy
``frontlidar_dlog_export.py``) and parsed RTK/IMU JSONL from
``parse_rtk_imu_v2.py``. Raw ``.rscap`` files remain the traceability source;
this script never modifies them.
Position rows may be NMEA ``GGA`` or G90 ``PVTSLNA`` (both expose ``lat_deg`` /
``lon_deg`` / ``altitude_m``). Default time basis is LiDAR device time vs GNSS
week/TOW; ``--time-basis host`` keeps the legacy host-receive nearest-neighbour
association for old dlog datasets.
"""
from __future__ import annotations
@@ -18,6 +24,7 @@ import numpy as np
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
POSITION_TYPES = {"GGA", "PVTSLNA"}
def parse_named_path(text: str) -> tuple[str, Path]:
@@ -46,6 +53,12 @@ def parse_args() -> argparse.Namespace:
parser.add_argument("--imu-before-ms", type=float, default=100.0)
parser.add_argument("--imu-after-ms", type=float, default=100.0)
parser.add_argument("--gps-utc-leap-seconds", type=int, default=18)
parser.add_argument(
"--time-basis",
choices=("device_gnss", "host"),
default="device_gnss",
help="device_gnss: LiDAR unix_time_ns ↔ GNSS week/TOW; host: legacy host-receive association.",
)
parser.add_argument("--overwrite", action="store_true")
return parser.parse_args()
@@ -83,7 +96,7 @@ def nearest_index(times: np.ndarray, target: int) -> int:
def estimate_imu_times(rows: list[dict[str, Any]]) -> list[dict[str, Any]]:
"""Recover 100 Hz timing inside each serial chunk from device timestamps.
"""Recover timing inside each serial chunk from device timestamps.
A capture chunk has one host receive timestamp but may contain several IMU
frames. The last frame is anchored to the chunk receive time and earlier
@@ -119,6 +132,17 @@ def gnss_utc_ns(row: dict[str, Any], leap_seconds: int) -> int | None:
return GPS_EPOCH_UNIX_NS + int(round(seconds * 1_000_000_000))
def association_time_ns(row: dict[str, Any], time_basis: str, leap_seconds: int) -> int | None:
if time_basis == "host":
host = row.get("host_receive_utc_ns")
return int(host) if host is not None else None
device = gnss_utc_ns(row, leap_seconds)
if device is not None:
return device
host = row.get("host_receive_utc_ns")
return int(host) if host is not None else None
def numeric_array(rows: list[dict[str, Any]], key: str, dtype: Any, default: Any) -> np.ndarray:
return np.asarray([row.get(key, default) if row.get(key) is not None else default for row in rows], dtype=dtype)
@@ -168,34 +192,66 @@ def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
values["rtk_heading_gnss_utc_ns"] = np.asarray([0], dtype=np.int64)
values["rtk_heading_host_minus_gnss_ns"] = np.asarray([0], dtype=np.int64)
def main() -> int:
args = parse_args()
if args.out.exists() and any(args.out.iterdir()) and not args.overwrite:
raise FileExistsError(f"{args.out} is non-empty; pass --overwrite")
frames_out = args.out / "frames"
def build_combined(
lidar_segments: list[tuple[str, Path]],
rtk_paths: list[Path],
imu_paths: list[Path],
out: Path,
*,
rtk_max_dt_ms: float = 150.0,
imu_before_ms: float = 100.0,
imu_after_ms: float = 100.0,
gps_utc_leap_seconds: int = 18,
time_basis: str = "device_gnss",
overwrite: bool = False,
) -> dict[str, Any]:
"""Associate LiDAR frames with RTK/IMU and write ``out/`` combined package."""
if out.exists() and any(out.iterdir()) and not overwrite:
raise FileExistsError(f"{out} is non-empty; pass overwrite=True")
frames_out = out / "frames"
frames_out.mkdir(parents=True, exist_ok=True)
rtk_rows = load_jsonl(args.rtk)
gga = sorted(
[row for row in rtk_rows if row.get("type") == "GGA" and row.get("checksum_valid") and row.get("lat_deg") is not None],
key=lambda row: int(row["host_receive_utc_ns"]),
)
heading = sorted(
[row for row in rtk_rows if row.get("type") == "UNIHEADINGA" and row.get("checksum_valid") and row.get("heading_valid")],
key=lambda row: int(row["host_receive_utc_ns"]),
)
imu = estimate_imu_times(load_jsonl(args.imu))
gga_times = np.asarray([int(row["host_receive_utc_ns"]) for row in gga], dtype=np.int64)
heading_times = np.asarray([int(row["host_receive_utc_ns"]) for row in heading], dtype=np.int64)
rtk_rows = load_jsonl(rtk_paths)
positions = []
for row in rtk_rows:
if row.get("type") not in POSITION_TYPES or not row.get("checksum_valid"):
continue
if row.get("lat_deg") is None or row.get("lon_deg") is None:
continue
assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
if assoc is None:
continue
copied = dict(row)
copied["_assoc_time_ns"] = assoc
positions.append(copied)
positions.sort(key=lambda row: int(row["_assoc_time_ns"]))
heading = []
for row in rtk_rows:
if row.get("type") != "UNIHEADINGA" or not row.get("checksum_valid") or not row.get("heading_valid"):
continue
assoc = association_time_ns(row, time_basis, gps_utc_leap_seconds)
if assoc is None:
continue
copied = dict(row)
copied["_assoc_time_ns"] = assoc
heading.append(copied)
heading.sort(key=lambda row: int(row["_assoc_time_ns"]))
imu = estimate_imu_times(load_jsonl(imu_paths))
position_times = np.asarray([int(row["_assoc_time_ns"]) for row in positions], dtype=np.int64)
heading_times = np.asarray([int(row["_assoc_time_ns"]) for row in heading], dtype=np.int64)
imu_times = np.asarray([int(row["estimated_time_ns"]) for row in imu], dtype=np.int64)
manifest: list[dict[str, Any]] = []
global_index = 0
max_rtk_ns = int(args.rtk_max_dt_ms * 1_000_000)
before_ns = int(args.imu_before_ms * 1_000_000)
after_ns = int(args.imu_after_ms * 1_000_000)
max_rtk_ns = int(rtk_max_dt_ms * 1_000_000)
before_ns = int(imu_before_ms * 1_000_000)
after_ns = int(imu_after_ms * 1_000_000)
for segment_name, frame_dir in args.lidar:
for segment_name, frame_dir in lidar_segments:
frame_paths = sorted(frame_dir.glob("*.npz"))
if not frame_paths:
raise FileNotFoundError(f"no NPZ frames under {frame_dir}")
@@ -204,28 +260,31 @@ def main() -> int:
values = {key: np.asarray(frame[key]) for key in frame.files}
lidar_time_ns = int(scalar(values["unix_time_ns"]))
gga_index = nearest_index(gga_times, lidar_time_ns)
position_index = nearest_index(position_times, lidar_time_ns)
heading_index = nearest_index(heading_times, lidar_time_ns)
gga_row = gga[gga_index] if gga_index >= 0 else None
position_row = positions[position_index] if position_index >= 0 else None
heading_row = heading[heading_index] if heading_index >= 0 else None
gga_dt = int(gga_times[gga_index]) - lidar_time_ns if gga_index >= 0 else None
position_dt = int(position_times[position_index]) - lidar_time_ns if position_index >= 0 else None
heading_dt = int(heading_times[heading_index]) - lidar_time_ns if heading_index >= 0 else None
gga_ok = gga_row is not None and abs(gga_dt or 0) <= max_rtk_ns
position_ok = position_row is not None and abs(position_dt or 0) <= max_rtk_ns
heading_ok = heading_row is not None and abs(heading_dt or 0) <= max_rtk_ns
add_rtk(values, "rtk_gga", gga_row if gga_ok else None, gga_dt)
add_rtk(values, "rtk_gga", position_row if position_ok else None, position_dt)
add_rtk(values, "rtk_heading", heading_row if heading_ok else None, heading_dt)
initialize_rtk_measurements(values)
if gga_ok and gga_row:
if position_ok and position_row:
for key, dtype, default in (
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
("differential_age_s", np.float64, np.nan),
):
values[f"rtk_{key}"] = np.asarray([gga_row.get(key, default)], dtype=dtype)
values["rtk_gga_satellites"] = np.asarray([gga_row.get("satellites", -1)], dtype=np.int32)
values["rtk_fixed"] = np.asarray([int(gga_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8)
values[f"rtk_{key}"] = np.asarray([position_row.get(key, default)], dtype=dtype)
values["rtk_gga_satellites"] = np.asarray([position_row.get("satellites", -1)], dtype=np.int32)
if position_row.get("gnss_week") is not None:
values["rtk_gnss_week"] = np.asarray([position_row.get("gnss_week", -1)], dtype=np.int32)
values["rtk_gnss_tow_ms"] = np.asarray([position_row.get("gnss_tow_ms", -1)], dtype=np.int64)
values["rtk_fixed"] = np.asarray([int(position_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8)
if heading_ok and heading_row:
for key, dtype, default in (
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
@@ -237,7 +296,7 @@ def main() -> int:
values[f"rtk_{key}"] = np.asarray([heading_row.get(key, default)], dtype=dtype)
values["rtk_heading_satellites"] = np.asarray([heading_row.get("satellites", -1)], dtype=np.int32)
values["rtk_heading_solution_utf8"] = utf8_array(heading_row.get("heading_solution", ""))
device_ns = gnss_utc_ns(heading_row, args.gps_utc_leap_seconds)
device_ns = gnss_utc_ns(heading_row, gps_utc_leap_seconds)
values["rtk_heading_gnss_utc_ns"] = np.asarray([device_ns or 0], dtype=np.int64)
values["rtk_heading_host_minus_gnss_ns"] = np.asarray(
[int(heading_row["host_receive_utc_ns"]) - device_ns if device_ns is not None else 0], dtype=np.int64
@@ -263,7 +322,9 @@ def main() -> int:
raw_matrix, raw_lengths = raw_frame_matrix(window)
values["imu_raw_frame_bytes"] = raw_matrix
values["imu_raw_frame_length"] = raw_lengths
values["imu_source_files_json_utf8"] = utf8_array(json.dumps([str(path.resolve()) for path in args.imu], ensure_ascii=False))
values["imu_source_files_json_utf8"] = utf8_array(
json.dumps([str(path.resolve()) for path in imu_paths], ensure_ascii=False)
)
values["source_lidar_file_utf8"] = utf8_array(source.resolve())
values["segment_name_utf8"] = utf8_array(segment_name)
@@ -273,37 +334,65 @@ def main() -> int:
"global_index": global_index,
"segment": segment_name,
"segment_index": segment_index,
"output": str(output.relative_to(args.out)),
"output": str(output.relative_to(out)),
"source_lidar": str(source.resolve()),
"lidar_time_ns": lidar_time_ns,
"rtk_gga_dt_ns": gga_dt,
"rtk_gga_dt_ns": position_dt,
"rtk_heading_dt_ns": heading_dt,
"rtk_valid": gga_ok,
"rtk_valid": position_ok,
"heading_valid": heading_ok,
"rtk_fix_quality": gga_row.get("fix_quality") if gga_ok and gga_row else None,
"rtk_fixed": bool(gga_ok and gga_row and int(gga_row.get("fix_quality", -1)) in {4, 5}),
"rtk_fix_quality": position_row.get("fix_quality") if position_ok and position_row else None,
"rtk_fixed": bool(position_ok and position_row and int(position_row.get("fix_quality", -1)) in {4, 5}),
"imu_window_count": len(window),
})
global_index += 1
fields = sorted({key for row in manifest for key in row})
with (args.out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
with (out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
writer = csv.DictWriter(stream, fieldnames=fields)
writer.writeheader()
writer.writerows(manifest)
if time_basis == "device_gnss":
time_basis_text = (
"LiDAR MSOP/device unix_time_ns ↔ RTK GNSS week/TOW (fallback host receive); "
"IMU still windowed on host-anchored device deltas"
)
else:
time_basis_text = (
"LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement"
)
summary = {
"frames": len(manifest),
"segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in args.lidar},
"segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in lidar_segments},
"rtk_valid": sum(bool(row["rtk_valid"]) for row in manifest),
"heading_valid": sum(bool(row["heading_valid"]) for row in manifest),
"rtk_fixed": sum(bool(row["rtk_fixed"]) for row in manifest),
"imu_window_nonempty": sum(int(row["imu_window_count"]) > 0 for row in manifest),
"rtk_max_dt_ms": args.rtk_max_dt_ms,
"imu_window_ms": [-args.imu_before_ms, args.imu_after_ms],
"time_basis": "LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement",
"rtk_max_dt_ms": rtk_max_dt_ms,
"imu_window_ms": [-imu_before_ms, imu_after_ms],
"time_basis": time_basis_text,
"time_basis_mode": time_basis,
"position_message_types": sorted(POSITION_TYPES),
"imu_orientation_warning": "IMU values are in the raw IMU sensor frame; no LiDAR/body extrinsic is applied",
}
(args.out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
(out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
return summary
def main() -> int:
args = parse_args()
summary = build_combined(
args.lidar,
args.rtk,
args.imu,
args.out,
rtk_max_dt_ms=args.rtk_max_dt_ms,
imu_before_ms=args.imu_before_ms,
imu_after_ms=args.imu_after_ms,
gps_utc_leap_seconds=args.gps_utc_leap_seconds,
time_basis=args.time_basis,
overwrite=args.overwrite,
)
print(json.dumps(summary, ensure_ascii=False, indent=2))
return 0