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

完善 LiDAR–双天线 RTK 手眼标定仓库：补充旧式及多传感器数据导出、small_gicp/Open3D GICP 标定、结果复核与3D可视化流程，并整理三批数据和标定结果说明。

Browse Source
This commit is contained in:
lichun.qu
2026-07-23 18:55:50 +08:00
parent 6b7844a977
commit f72fcb71cc
91 changed files with 39561 additions and 503 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/build_multisensor_npz.py
+312
View File
@@ -0,0 +1,312 @@
#!/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.
"""
from __future__ import annotations
import argparse
import csv
import json
from pathlib import Path
from typing import Any
import numpy as np
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
def parse_named_path(text: str) -> tuple[str, Path]:
if "=" not in text:
raise argparse.ArgumentTypeError("expected NAME=PATH")
name, raw_path = text.split("=", 1)
if not name.strip():
raise argparse.ArgumentTypeError("segment name is empty")
return name.strip(), Path(raw_path)
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser()
parser.add_argument(
"--lidar",
type=parse_named_path,
action="append",
required=True,
metavar="NAME=FRAMES_DIR",
help="Repeat for each LiDAR segment; directory contains exported *.npz frames.",
)
parser.add_argument("--rtk", type=Path, action="append", required=True, help="Parsed rtk.jsonl; repeat per session.")
parser.add_argument("--imu", type=Path, action="append", required=True, help="Parsed imu.jsonl; repeat per session.")
parser.add_argument("--out", type=Path, required=True)
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
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("--overwrite", action="store_true")
return parser.parse_args()
def load_jsonl(paths: list[Path]) -> list[dict[str, Any]]:
rows: list[dict[str, Any]] = []
for source_index, path in enumerate(paths):
source_file = str(path.resolve())
with path.open("r", encoding="utf-8") as stream:
for line_number, line in enumerate(stream, start=1):
if not line.strip():
continue
row = json.loads(line)
row["_source_file"] = source_file
row["_source_index"] = source_index
row["_source_line"] = line_number
rows.append(row)
return rows
def utf8_array(value: Any) -> np.ndarray:
return np.frombuffer(str(value if value is not None else "").encode("utf-8"), dtype=np.uint8)
def scalar(array: np.ndarray) -> Any:
return array.reshape(-1)[0].item()
def nearest_index(times: np.ndarray, target: int) -> int:
if not len(times):
return -1
right = int(np.searchsorted(times, target, side="left"))
candidates = [index for index in (right - 1, right) if 0 <= index < len(times)]
return min(candidates, key=lambda index: abs(int(times[index]) - target))
def estimate_imu_times(rows: list[dict[str, Any]]) -> list[dict[str, Any]]:
"""Recover 100 Hz 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
frames are moved backwards by their device timestamp difference.
"""
groups: dict[tuple[int, int], list[dict[str, Any]]] = {}
for row in rows:
if not row.get("crc_valid") or row.get("device_timestamp_ms") is None:
continue
key = (int(row["_source_index"]), int(row.get("source_chunk_sequence_last", -1)))
groups.setdefault(key, []).append(row)
result: list[dict[str, Any]] = []
for group in groups.values():
group.sort(key=lambda row: (int(row["device_timestamp_ms"]), int(row["_source_line"])))
last_device = int(group[-1]["device_timestamp_ms"])
host_ns = int(group[-1]["host_receive_utc_ns"])
for row in group:
delta_ms = (last_device - int(row["device_timestamp_ms"])) & 0xFFFFFFFF
if delta_ms > 60_000:
delta_ms = 0
copied = dict(row)
copied["estimated_time_ns"] = host_ns - delta_ms * 1_000_000
result.append(copied)
result.sort(key=lambda row: int(row["estimated_time_ns"]))
return result
def gnss_utc_ns(row: dict[str, Any], leap_seconds: int) -> int | None:
week, tow_ms = row.get("gnss_week"), row.get("gnss_tow_ms")
if week is None or tow_ms is None:
return None
seconds = int(week) * 604800 + float(tow_ms) / 1000.0 - leap_seconds
return GPS_EPOCH_UNIX_NS + int(round(seconds * 1_000_000_000))
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)
def raw_frame_matrix(rows: list[dict[str, Any]]) -> tuple[np.ndarray, np.ndarray]:
frames = [bytes.fromhex(str(row.get("raw_frame_hex", ""))) for row in rows]
lengths = np.asarray([len(frame) for frame in frames], dtype=np.int32)
width = max(lengths, default=0)
matrix = np.zeros((len(frames), width), dtype=np.uint8)
for index, frame in enumerate(frames):
matrix[index, : len(frame)] = np.frombuffer(frame, dtype=np.uint8)
return matrix, lengths
def add_rtk(values: dict[str, np.ndarray], prefix: str, row: dict[str, Any] | None, dt_ns: int | None) -> None:
values[f"{prefix}_valid"] = np.asarray([row is not None], dtype=np.uint8)
values[f"{prefix}_dt_ns"] = np.asarray([dt_ns or 0], dtype=np.int64)
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([0], dtype=np.int64)
values[f"{prefix}_raw_utf8"] = utf8_array("")
values[f"{prefix}_source_file_utf8"] = utf8_array("")
values[f"{prefix}_source_raw_file_offset"] = np.asarray([-1], dtype=np.int64)
values[f"{prefix}_source_raw_byte_length"] = np.asarray([0], dtype=np.int32)
if row is None:
return
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([row.get("host_receive_utc_ns", 0)], dtype=np.int64)
values[f"{prefix}_raw_utf8"] = utf8_array(row.get("raw_line", ""))
values[f"{prefix}_source_file_utf8"] = utf8_array(row.get("_source_file", ""))
values[f"{prefix}_source_raw_file_offset"] = np.asarray([row.get("source_raw_file_offset", -1)], dtype=np.int64)
values[f"{prefix}_source_raw_byte_length"] = np.asarray([row.get("source_raw_byte_length", 0)], dtype=np.int32)
def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
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),
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
("pitch_stddev_deg", np.float64, np.nan), ("heading_satellites", np.int32, -1),
("solution_satellites", np.int32, -1),
):
values[f"rtk_{key}"] = np.asarray([default], dtype=dtype)
values["rtk_fixed"] = np.asarray([0], dtype=np.uint8)
values["rtk_heading_solution_utf8"] = utf8_array("")
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"
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)
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)
for segment_name, frame_dir in args.lidar:
frame_paths = sorted(frame_dir.glob("*.npz"))
if not frame_paths:
raise FileNotFoundError(f"no NPZ frames under {frame_dir}")
for segment_index, source in enumerate(frame_paths):
with np.load(source, allow_pickle=False) as frame:
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)
heading_index = nearest_index(heading_times, lidar_time_ns)
gga_row = gga[gga_index] if gga_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
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
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_heading", heading_row if heading_ok else None, heading_dt)
initialize_rtk_measurements(values)
if gga_ok and gga_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)
if heading_ok and heading_row:
for key, dtype, default in (
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
("pitch_stddev_deg", np.float64, np.nan),
("solution_satellites", np.int32, -1),
):
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)
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
)
left = int(np.searchsorted(imu_times, lidar_time_ns - before_ns, side="left"))
right = int(np.searchsorted(imu_times, lidar_time_ns + after_ns, side="right"))
window = imu[left:right]
values["imu_window_count"] = np.asarray([len(window)], dtype=np.int32)
values["imu_valid"] = np.asarray([bool(window)], dtype=np.uint8)
values["imu_time_ns"] = numeric_array(window, "estimated_time_ns", np.int64, 0)
values["imu_host_receive_utc_ns"] = numeric_array(window, "host_receive_utc_ns", np.int64, 0)
for key in ("device_timestamp_ms", "pps_sync_stamp_ms", "tag"):
values[f"imu_{key}"] = numeric_array(window, key, np.int64, -1)
for key in (
"temperature_c", "air_pressure_pa", "accel_x_mps2", "accel_y_mps2", "accel_z_mps2",
"gyro_x_radps", "gyro_y_radps", "gyro_z_radps", "mag_x_ut", "mag_y_ut", "mag_z_ut",
"roll_deg", "pitch_deg", "yaw_deg", "quaternion_w", "quaternion_x", "quaternion_y", "quaternion_z",
):
values[f"imu_{key}"] = numeric_array(window, key, np.float64, np.nan)
values["imu_source_index"] = numeric_array(window, "_source_index", np.int32, -1)
values["imu_source_raw_file_offset"] = numeric_array(window, "source_raw_file_offset", np.int64, -1)
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["source_lidar_file_utf8"] = utf8_array(source.resolve())
values["segment_name_utf8"] = utf8_array(segment_name)
output = frames_out / f"{segment_name}_{segment_index:06d}.npz"
np.savez_compressed(output, **values)
manifest.append({
"global_index": global_index,
"segment": segment_name,
"segment_index": segment_index,
"output": str(output.relative_to(args.out)),
"source_lidar": str(source.resolve()),
"lidar_time_ns": lidar_time_ns,
"rtk_gga_dt_ns": gga_dt,
"rtk_heading_dt_ns": heading_dt,
"rtk_valid": gga_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}),
"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:
writer = csv.DictWriter(stream, fieldnames=fields)
writer.writeheader()
writer.writerows(manifest)
summary = {
"frames": len(manifest),
"segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in args.lidar},
"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",
"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")
print(json.dumps(summary, ensure_ascii=False, indent=2))
return 0
if __name__ == "__main__":
raise SystemExit(main())