lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
8477935ad2cccc0fdf658feaf907459c4d1d2520
calibration/tools/frontlidar_dlog_export.py
T

1964 lines
75 KiB
Python
Raw Normal View History

更新README并添加标定工具
2026-07-22 17:09:20 +08:00
#!/usr/bin/env python3
"""Export Medulla DObject lidar + RTK recordings to sharded NumPy NPZ / pickle.
The default NPZ path uses only the Python standard library; NumPy is required
only when consuming the exported files, not while exporting them.
"""
from __future__ import annotations
import argparse
import array
import bisect
import csv
import io
import json
import math
import os
import pickle
import re
import struct
import sys
import time
import zipfile
from contextlib import nullcontext
from dataclasses import asdict, dataclass
from datetime import datetime, timedelta, timezone
from pathlib import Path
from typing import BinaryIO, Iterable, Iterator, Sequence
RECORD_RE = re.compile(
r"^\[(?P<log_time>[^]]+)\].*?DObject `(?P<name>[^`]+)` post "
r"len=(?P<len>\d+)B, id:(?P<id>[0-9A-Fa-f]+), tic:(?P<tic>\d+), "
r"@(?P<file>[^:]+):(?P<offset>\d+)"
)
GPS_TEXT_RE = re.compile(
r"^\[(?P<log_time>[^]]+)\].*?\$GPS-POST-Z>text\)>(?P<body>.+)$"
)
POINT_STRUCT = struct.Struct("<5f")
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
TICKS_PER_SECOND = 10_000_000
TICKS_PER_DAY = 864_000_000_000
NS_PER_TICK = 100
FORMAT_VERSION = "medulla-lidar3d-rtk-v2"
RTK_OBJECT_NAMES = ("GPS-POST-Z", "rtk", "GPS-POST")
@dataclass(frozen=True)
class RecordRef:
sequence: int
object_name: str
log_time: str
source_log: str
source_dorec: str
source_offset: int
payload_length: int
log_record_id: str
dotnet_ticks: int
@dataclass
class RtkSample:
index: int
source: str
object_name: str
dotnet_ticks: int
timestamp_iso_local: str
unix_time_ns: int
log_time: str
record_id: str
source_log: str
source_dorec: str
source_offset: int
payload_length: int
counter: int | None = None
lat: float | None = None
lon: float | None = None
alt_m: float | None = None
raw_heading_deg: float | None = None
vehicle_heading_deg: float | None = None
fix: int | None = None
sat: int | None = None
position_valid: bool | None = None
heading_valid: bool | None = None
heading_solution: str | None = None
position_time: str | None = None
heading_time: str | None = None
last_line: str | None = None
device_name: str | None = None
device_stamp_hex: str | None = None
raw_text: str | None = None
parse_error: str | None = None
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(
description=(
"Export Medulla frontlidar dlog records with matched RTK/GPS-POST-Z "
"to per-frame NPZ or pickle files."
)
)
parser.add_argument("--dlog", required=True, help="Directory containing dobject/ and dobject_recording/.")
parser.add_argument("--out", required=True, help="Output dataset directory.")
parser.add_argument("--object", default="frontlidar", help="Lidar DObject name; default: frontlidar.")
parser.add_argument("--format", choices=("npz", "pickle"), default="npz")
parser.add_argument("--timezone", default="+08:00", help="Fixed offset used to interpret DateTime.Now.Ticks.")
parser.add_argument("--stride", type=int, default=1, help="Export every Nth lidar frame.")
parser.add_argument("--max-frames", type=int, default=0, help="0 exports all selected frames.")
parser.add_argument("--resume", action="store_true", help="Keep already exported frame files.")
parser.add_argument("--compress", action="store_true", help="Use ZIP deflate level 1 for NPZ files.")
parser.add_argument(
"--include-xyz",
choices=("none", "sensor", "cart", "both"),
default="none",
help="Optionally generate XYZ arrays. Raw N x 5 data is always exported.",
)
parser.add_argument("--x", type=float, default=736.0, help="Lidar X in vehicle frame, mm.")
parser.add_argument("--y", type=float, default=0.0, help="Lidar Y in vehicle frame, mm.")
parser.add_argument("--z", type=float, default=0.0, help="Lidar Z in vehicle frame, mm.")
parser.add_argument("--yaw", type=float, default=0.0, help="Yaw in degrees.")
parser.add_argument("--pitch", type=float, default=0.0, help="Pitch in degrees.")
parser.add_argument("--roll", type=float, default=0.0, help="Roll in degrees.")
parser.add_argument(
"--rtk-max-dt-ms",
type=float,
default=1000.0,
help=(
"Fail rtk_time_alignment when an interior frame's nearest |dt| exceeds this many "
"milliseconds. Edge frames before the first / after the last RTK are reported "
"separately and do not fail the check."
),
)
parser.add_argument(
"--skip-rtk",
action="store_true",
help="Export lidar only (no per-frame RTK matching).",
)
parser.add_argument(
"--rtk-sidecars",
action="store_true",
help=(
"Also write rtk/ sidecars (gps_post_z / rtk_binary / gps_post / text). "
"Default is off; per-frame NPZ still embeds matched RTK fields."
),
)
parser.add_argument(
"--write-reports",
action="store_true",
help=(
"Write audit files under reports/ (metadata.json, manifest.csv, "
"rtk_match.csv, validation_report.json). Default package only has "
"frames/ and README.md."
),
)
args = parser.parse_args()
if args.stride < 1:
parser.error("--stride must be >= 1")
if args.max_frames < 0:
parser.error("--max-frames must be >= 0")
if args.rtk_max_dt_ms < 0:
parser.error("--rtk-max-dt-ms must be >= 0")
if args.skip_rtk and args.rtk_sidecars:
parser.error("--rtk-sidecars cannot be used with --skip-rtk")
parse_timezone(args.timezone)
return args
def resolve_dlog_root(value: str) -> Path:
root = Path(value).expanduser().resolve()
if (root / "dobject").is_dir() and (root / "dobject_recording").is_dir():
return root
child = root / "dlog"
if (child / "dobject").is_dir() and (child / "dobject_recording").is_dir():
return child
raise FileNotFoundError(f"{root} does not contain dobject and dobject_recording")
def parse_timezone(text: str) -> timezone:
match = re.fullmatch(r"([+-])(\d{2}):(\d{2})", text.strip())
if not match:
raise ValueError(f"invalid timezone offset: {text!r}")
minutes = int(match.group(2)) * 60 + int(match.group(3))
if match.group(1) == "-":
minutes = -minutes
return timezone(timedelta(minutes=minutes))
def timezone_minutes(tz: timezone) -> int:
return int(tz.utcoffset(None).total_seconds() // 60)
def dotnet_ticks_to_values(ticks: int, tz: timezone) -> tuple[str, int]:
days, remainder = divmod(ticks, TICKS_PER_DAY)
seconds, subsecond_ticks = divmod(remainder, TICKS_PER_SECOND)
local_dt = datetime(1, 1, 1) + timedelta(days=days, seconds=seconds)
fraction = f"{subsecond_ticks:07d}"
offset = tz.utcoffset(None)
sign = "+" if offset >= timedelta(0) else "-"
total_minutes = abs(int(offset.total_seconds() // 60))
iso = f"{local_dt:%Y-%m-%dT%H:%M:%S}.{fraction}{sign}{total_minutes // 60:02d}:{total_minutes % 60:02d}"
offset_ticks = int(offset.total_seconds()) * TICKS_PER_SECOND
unix_ns = (ticks - DOTNET_UNIX_EPOCH_TICKS - offset_ticks) * NS_PER_TICK
return iso, unix_ns
def discover_records(dlog_root: Path, object_name: str) -> list[RecordRef]:
pending: list[tuple[str, str, str, int, int, str, int, str]] = []
for log_path in sorted((dlog_root / "dobject").rglob("*.log")):
relative_log = log_path.relative_to(dlog_root).as_posix()
with log_path.open("r", encoding="utf-8", errors="replace") as stream:
for line in stream:
match = RECORD_RE.search(line)
if not match or match.group("name").casefold() != object_name.casefold():
continue
pending.append(
(
match.group("name"),
match.group("log_time"),
relative_log,
int(match.group("offset")),
int(match.group("len")),
match.group("id").upper(),
int(match.group("tic")),
match.group("file"),
)
)
pending.sort(key=lambda item: (item[6], item[7].casefold(), item[3]))
seen: set[tuple[str, int, int]] = set()
records: list[RecordRef] = []
for item in pending:
key = (item[7].casefold(), item[3], item[6])
if key in seen:
continue
seen.add(key)
records.append(
RecordRef(
sequence=len(records),
object_name=item[0],
log_time=item[1],
source_log=item[2],
source_dorec=item[7],
source_offset=item[3],
payload_length=item[4],
log_record_id=item[5],
dotnet_ticks=item[6],
)
)
return records
def index_dorec_files(dlog_root: Path) -> dict[str, list[Path]]:
result: dict[str, list[Path]] = {}
for path in (dlog_root / "dobject_recording").rglob("*.dorec"):
result.setdefault(path.name.casefold(), []).append(path)
return result
def choose_dorec(index: dict[str, list[Path]], name: str) -> Path:
matches = index.get(Path(name).name.casefold(), [])
if not matches:
raise FileNotFoundError(f"missing recording file: {name}")
if len(matches) > 1:
raise RuntimeError(f"ambiguous recording file {name}: {matches}")
return matches[0]
def read_exact(stream: BinaryIO, size: int) -> bytes:
data = stream.read(size)
if len(data) != size:
raise EOFError(f"expected {size} bytes, got {len(data)}")
return data
def read_record(path: Path, record: RecordRef) -> tuple[dict[str, object], bytes]:
with path.open("rb") as stream:
stream.seek(record.source_offset)
name_length = read_exact(stream, 1)[0]
name = read_exact(stream, name_length).decode("ascii")
ticks = struct.unpack("<q", read_exact(stream, 8))[0]
id_length = read_exact(stream, 1)[0]
id_bytes = read_exact(stream, id_length)
payload_length = struct.unpack("<i", read_exact(stream, 4))[0]
payload = read_exact(stream, payload_length)
try:
record_id = id_bytes.decode("ascii")
except UnicodeDecodeError:
record_id = id_bytes.hex().upper()
if name != record.object_name:
raise ValueError(f"name mismatch: log={record.object_name}, dorec={name}")
if ticks != record.dotnet_ticks:
raise ValueError(f"tick mismatch: log={record.dotnet_ticks}, dorec={ticks}")
if payload_length != record.payload_length:
raise ValueError(f"payload mismatch: log={record.payload_length}, dorec={payload_length}")
if record_id.upper() != record.log_record_id.upper():
raise ValueError(f"record id mismatch: log={record.log_record_id}, dorec={record_id}")
return {
"record_id": record_id,
"record_id_bytes_hex": id_bytes.hex().upper(),
"payload_length": payload_length,
}, payload
def parse_lidar_payload(payload: bytes) -> tuple[int, int, bytes]:
if len(payload) < 8:
raise ValueError(f"payload too short: {len(payload)}")
frame_counter, point_count = struct.unpack_from("<ii", payload, 0)
if point_count < 0:
raise ValueError(f"negative point count: {point_count}")
expected = 8 + point_count * POINT_STRUCT.size
if expected != len(payload):
raise ValueError(f"lidar payload length mismatch: expected={expected}, actual={len(payload)}")
return frame_counter, point_count, payload[8:]
def read_len_prefixed_ascii(data: bytes, pos: int) -> tuple[str, int]:
if pos >= len(data):
raise ValueError("truncated length-prefixed string")
length = data[pos]
pos += 1
end = pos + length
if end > len(data):
raise ValueError("truncated length-prefixed string body")
return data[pos:end].decode("ascii", errors="replace"), end
def parse_bool_text(value: str) -> bool | None:
lowered = value.strip().casefold()
if lowered in ("true", "1", "yes"):
return True
if lowered in ("false", "0", "no"):
return False
return None
def parse_kv_payload(text: str, separators: Sequence[str] = ("=", ":")) -> dict[str, str]:
result: dict[str, str] = {}
for part in text.replace("\r", "").replace("\n", ",").split(","):
part = part.strip()
if not part:
continue
key = value = None
for sep in separators:
if sep in part:
key, value = part.split(sep, 1)
break
if key is None or value is None:
continue
result[key.strip()] = value.strip()
return result
def coerce_int(value: str | None) -> int | None:
if value is None or value == "":
return None
try:
return int(float(value))
except ValueError:
return None
def coerce_float(value: str | None) -> float | None:
if value is None or value == "":
return None
try:
return float(value)
except ValueError:
return None
def parse_gps_post_z_text(text: str) -> dict[str, object]:
fields = parse_kv_payload(text, separators=("=", ":"))
alt = fields.get("alt", fields.get("h"))
vehicle_heading = fields.get("vehicleHeading", fields.get("th"))
return {
"counter": coerce_int(fields.get("counter")),
"lat": coerce_float(fields.get("lat")),
"lon": coerce_float(fields.get("lon")),
"alt_m": coerce_float(alt),
"raw_heading_deg": coerce_float(fields.get("rawHeading")),
"vehicle_heading_deg": coerce_float(vehicle_heading),
"fix": coerce_int(fields.get("fix")),
"sat": coerce_int(fields.get("sat")),
"position_valid": parse_bool_text(fields.get("positionValid", "")),
"heading_valid": parse_bool_text(fields.get("headingValid", "")),
"heading_solution": fields.get("headingSolution"),
"position_time": fields.get("positionTime"),
"heading_time": fields.get("headingTime"),
"last_line": fields.get("lastLine"),
"time_field": fields.get("time"),
"raw_fields": fields,
}
def parse_rtk_binary_payload(payload: bytes) -> dict[str, object]:
pos = 0
device_name, pos = read_len_prefixed_ascii(payload, pos)
version, counter, unknown0 = struct.unpack_from("<iii", payload, pos)
pos += 12
device_stamp = payload[pos : pos + 8]
pos += 8
position_time, pos = read_len_prefixed_ascii(payload, pos)
heading_time, pos = read_len_prefixed_ascii(payload, pos)
lat, lon, alt, reserved0, reserved1 = struct.unpack_from("<ddddd", payload, pos)
pos += 40
fix, sat = struct.unpack_from("<ii", payload, pos)
pos += 8
if pos >= len(payload):
raise ValueError("rtk payload truncated before validity flags")
position_valid = bool(payload[pos])
pos += 1
raw_heading, vehicle_heading = struct.unpack_from("<dd", payload, pos)
pos += 16
if pos >= len(payload):
raise ValueError("rtk payload truncated before heading validity")
heading_valid = bool(payload[pos])
pos += 1
heading_solution, pos = read_len_prefixed_ascii(payload, pos)
has_nmea = bool(payload[pos]) if pos < len(payload) else False
if pos < len(payload):
pos += 1
last_line = None
if has_nmea and pos < len(payload):
last_line, pos = read_len_prefixed_ascii(payload, pos)
return {
"device_name": device_name,
"version": version,
"counter": counter,
"unknown0": unknown0,
"device_stamp_hex": device_stamp.hex().upper(),
"position_time": position_time,
"heading_time": heading_time,
"lat": lat,
"lon": lon,
"alt_m": alt,
"reserved0": reserved0,
"reserved1": reserved1,
"fix": fix,
"sat": sat,
"position_valid": position_valid,
"raw_heading_deg": raw_heading,
"vehicle_heading_deg": vehicle_heading,
"heading_valid": heading_valid,
"heading_solution": heading_solution,
"last_line": last_line,
"bytes_consumed": pos,
"payload_length": len(payload),
}
def parse_gps_post_payload(payload: bytes) -> dict[str, object]:
pos = 0
name, pos = read_len_prefixed_ascii(payload, pos)
if pos + 20 > len(payload):
raise ValueError("GPS-POST payload too short")
counter, unknown0 = struct.unpack_from("<ii", payload, pos)
pos += 8
flags = payload[pos : pos + 4]
pos += 4
stamp = payload[pos : pos + 8]
pos += 8
floats = []
while pos + 4 <= len(payload):
floats.append(struct.unpack_from("<f", payload, pos)[0])
pos += 4
return {
"device_name": name,
"counter": counter,
"unknown0": unknown0,
"flags_hex": flags.hex().upper(),
"device_stamp_hex": stamp.hex().upper(),
"floats_f32": floats,
"payload_hex": payload.hex().upper(),
}
def rotation_matrix(yaw_deg: float, pitch_deg: float, roll_deg: float) -> tuple[float, ...]:
yaw, pitch, roll = map(math.radians, (yaw_deg, pitch_deg, roll_deg))
cy, sy = math.cos(yaw), math.sin(yaw)
cp, sp = math.cos(pitch), math.sin(pitch)
cr, sr = math.cos(roll), math.sin(roll)
return (
cy * cp,
-sy * cr + cy * sp * sr,
sy * sr + cy * sp * cr,
sy * cp,
cy * cr + sy * sp * sr,
-cy * sr + sy * sp * cr,
-sp,
cp * sr,
cp * cr,
)
def generate_xyz(
raw_points: bytes,
mode: str,
translation: tuple[float, float, float],
rotation: tuple[float, ...],
) -> tuple[bytes | None, bytes | None]:
need_sensor = mode in ("sensor", "both")
need_cart = mode in ("cart", "both")
sensor = array.array("f") if need_sensor else None
cart = array.array("f") if need_cart else None
tx, ty, tz = translation
r = rotation
for distance, azimuth, altitude, _intensity, _progression in POINT_STRUCT.iter_unpack(raw_points):
alt = math.radians(altitude)
azi = math.radians(azimuth)
cos_alt = math.cos(alt)
sx = distance * cos_alt * math.cos(azi)
sy = distance * cos_alt * math.sin(azi)
sz = distance * math.sin(alt)
if sensor is not None:
sensor.extend((sx, sy, sz))
if cart is not None:
cart.extend(
(
r[0] * sx + r[1] * sy + r[2] * sz + tx,
r[3] * sx + r[4] * sy + r[5] * sz + ty,
r[6] * sx + r[7] * sy + r[8] * sz + tz,
)
)
if sys.byteorder != "little":
if sensor is not None:
sensor.byteswap()
if cart is not None:
cart.byteswap()
return (sensor.tobytes() if sensor is not None else None, cart.tobytes() if cart is not None else None)
def numpy_header(descr: str, shape: Sequence[int]) -> bytes:
shape_text = "(" + ", ".join(str(value) for value in shape)
if len(shape) == 1:
shape_text += ","
shape_text += ")"
header = f"{{'descr': '{descr}', 'fortran_order': False, 'shape': {shape_text}, }}"
encoded = header.encode("latin-1")
padding = (-((10 + len(encoded) + 1) % 16)) % 16
encoded += b" " * padding + b"\n"
if len(encoded) > 65535:
raise ValueError("NPY v1 header is too long")
return b"\x93NUMPY\x01\x00" + struct.pack("<H", len(encoded)) + encoded
def write_npy_entry(
archive: zipfile.ZipFile,
name: str,
descr: str,
shape: Sequence[int],
data: bytes,
) -> None:
with archive.open(name + ".npy", "w", force_zip64=True) as entry:
entry.write(numpy_header(descr, shape))
entry.write(data)
def scalar_bytes(fmt: str, value: int | float) -> bytes:
return struct.pack(fmt, value)
def open_npz_writer(path: Path, compress: bool) -> zipfile.ZipFile:
compression = zipfile.ZIP_DEFLATED if compress else zipfile.ZIP_STORED
kwargs: dict[str, object] = {"compression": compression, "allowZip64": True}
if compress:
kwargs["compresslevel"] = 1
return zipfile.ZipFile(path, "w", **kwargs)
def pack_f64_array(values: Sequence[float]) -> bytes:
data = array.array("d", values)
if sys.byteorder != "little":
data.byteswap()
return data.tobytes()
def pack_i8_array(values: Sequence[int]) -> bytes:
data = array.array("q", values)
if sys.byteorder != "little":
data.byteswap()
return data.tobytes()
def pack_i4_array(values: Sequence[int]) -> bytes:
data = array.array("i", values)
if sys.byteorder != "little":
data.byteswap()
return data.tobytes()
def pack_u1_bools(values: Sequence[bool | None]) -> bytes:
return bytes(1 if value else 0 for value in values)
def export_npz(
path: Path,
metadata: dict[str, object],
raw_points: bytes,
point_count: int,
xyz_sensor: bytes | None,
xyz_cart: bytes | None,
compress: bool,
rtk_arrays: dict[str, tuple[str, Sequence[int], bytes]] | None = None,
) -> None:
temp = path.with_suffix(path.suffix + ".tmp")
with open_npz_writer(temp, compress) as archive:
write_npy_entry(archive, "points_raw", "<f4", (point_count, 5), raw_points)
write_npy_entry(archive, "frame_counter", "<i4", (1,), scalar_bytes("<i", metadata["frame_counter"]))
write_npy_entry(archive, "point_count", "<i4", (1,), scalar_bytes("<i", point_count))
write_npy_entry(archive, "dotnet_ticks", "<i8", (1,), scalar_bytes("<q", metadata["dotnet_ticks"]))
write_npy_entry(archive, "unix_time_ns", "<i8", (1,), scalar_bytes("<q", metadata["unix_time_ns"]))
write_npy_entry(archive, "source_offset", "<i8", (1,), scalar_bytes("<q", metadata["source_offset"]))
write_npy_entry(archive, "payload_length", "<i4", (1,), scalar_bytes("<i", metadata["payload_length"]))
metadata_bytes = json.dumps(metadata, ensure_ascii=False, separators=(",", ":")).encode("utf-8")
write_npy_entry(archive, "metadata_json_utf8", "|u1", (len(metadata_bytes),), metadata_bytes)
if xyz_sensor is not None:
write_npy_entry(archive, "xyz_sensor_mm", "<f4", (point_count, 3), xyz_sensor)
if xyz_cart is not None:
write_npy_entry(archive, "xyz_cart_mm", "<f4", (point_count, 3), xyz_cart)
if rtk_arrays:
for name, (descr, shape, data) in rtk_arrays.items():
write_npy_entry(archive, name, descr, shape, data)
os.replace(temp, path)
def export_pickle(
path: Path,
metadata: dict[str, object],
raw_points: bytes,
point_count: int,
xyz_sensor: bytes | None,
xyz_cart: bytes | None,
) -> None:
points = array.array("f")
points.frombytes(raw_points)
if sys.byteorder != "little":
points.byteswap()
frame: dict[str, object] = {
"metadata": metadata,
"point_columns": ("d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"),
"points_raw_flat_f32": points,
"points_raw_shape": (point_count, 5),
}
for key, raw in (("xyz_sensor_mm_flat_f32", xyz_sensor), ("xyz_cart_mm_flat_f32", xyz_cart)):
if raw is not None:
values = array.array("f")
values.frombytes(raw)
if sys.byteorder != "little":
values.byteswap()
frame[key] = values
frame[key.replace("_flat_f32", "_shape")] = (point_count, 3)
temp = path.with_suffix(path.suffix + ".tmp")
with temp.open("wb") as stream:
pickle.dump(frame, stream, protocol=5)
os.replace(temp, path)
def frame_filename(record: RecordRef, frame_counter: int, extension: str) -> str:
return f"{record.object_name}_{record.sequence:06d}_{record.dotnet_ticks}_frame{frame_counter:010d}.{extension}"
def manifest_fields(include_rtk: bool) -> list[str]:
fields = [
"sequence",
"status",
"object_name",
"dotnet_ticks",
"timestamp_iso_local",
"unix_time_ns",
"log_time",
"record_id",
"record_id_bytes_hex",
"frame_counter",
"point_count",
"payload_length",
"source_log",
"source_dorec",
"source_offset",
"output_file",
"error",
]
if include_rtk:
fields.extend(
[
"rtk_matched",
"rtk_nearest_index",
"rtk_prev_index",
"rtk_next_index",
"rtk_dt_ns",
"rtk_prev_dt_ns",
"rtk_next_dt_ns",
"rtk_lat",
"rtk_lon",
"rtk_alt_m",
"rtk_vehicle_heading_deg",
"rtk_raw_heading_deg",
"rtk_fix",
"rtk_sat",
"rtk_position_valid",
"rtk_heading_valid",
"rtk_heading_solution",
]
)
return fields
def nan_if_none(value: float | None) -> float:
return float("nan") if value is None else float(value)
def int_or_sentinel(value: int | None, sentinel: int = -1) -> int:
return sentinel if value is None else int(value)
def bool_or_false(value: bool | None) -> bool:
return bool(value)
def sample_to_public_dict(sample: RtkSample) -> dict[str, object]:
data = asdict(sample)
return data
def write_jsonl(path: Path, rows: Iterable[dict[str, object]]) -> int:
count = 0
with path.open("w", encoding="utf-8") as stream:
for row in rows:
stream.write(json.dumps(row, ensure_ascii=False, separators=(",", ":")))
stream.write("\n")
count += 1
return count
def write_csv_rows(path: Path, fieldnames: Sequence[str], rows: Iterable[dict[str, object]]) -> int:
count = 0
with path.open("w", newline="", encoding="utf-8-sig") as stream:
writer = csv.DictWriter(stream, fieldnames=list(fieldnames), extrasaction="ignore")
writer.writeheader()
for row in rows:
writer.writerow({key: row.get(key, "") for key in fieldnames})
count += 1
return count
def export_rtk_table_npz(path: Path, samples: Sequence[RtkSample], compress: bool) -> None:
n = len(samples)
temp = path.with_suffix(path.suffix + ".tmp")
with open_npz_writer(temp, compress) as archive:
write_npy_entry(archive, "index", "<i4", (n,), pack_i4_array([s.index for s in samples]))
write_npy_entry(archive, "dotnet_ticks", "<i8", (n,), pack_i8_array([s.dotnet_ticks for s in samples]))
write_npy_entry(archive, "unix_time_ns", "<i8", (n,), pack_i8_array([s.unix_time_ns for s in samples]))
write_npy_entry(archive, "counter", "<i4", (n,), pack_i4_array([int_or_sentinel(s.counter) for s in samples]))
write_npy_entry(archive, "lat", "<f8", (n,), pack_f64_array([nan_if_none(s.lat) for s in samples]))
write_npy_entry(archive, "lon", "<f8", (n,), pack_f64_array([nan_if_none(s.lon) for s in samples]))
write_npy_entry(archive, "alt_m", "<f8", (n,), pack_f64_array([nan_if_none(s.alt_m) for s in samples]))
write_npy_entry(
archive,
"raw_heading_deg",
"<f8",
(n,),
pack_f64_array([nan_if_none(s.raw_heading_deg) for s in samples]),
)
write_npy_entry(
archive,
"vehicle_heading_deg",
"<f8",
(n,),
pack_f64_array([nan_if_none(s.vehicle_heading_deg) for s in samples]),
)
write_npy_entry(archive, "fix", "<i4", (n,), pack_i4_array([int_or_sentinel(s.fix) for s in samples]))
write_npy_entry(archive, "sat", "<i4", (n,), pack_i4_array([int_or_sentinel(s.sat) for s in samples]))
write_npy_entry(
archive,
"position_valid",
"|u1",
(n,),
pack_u1_bools([s.position_valid for s in samples]),
)
write_npy_entry(
archive,
"heading_valid",
"|u1",
(n,),
pack_u1_bools([s.heading_valid for s in samples]),
)
write_npy_entry(
archive,
"source_offset",
"<i8",
(n,),
pack_i8_array([s.source_offset for s in samples]),
)
write_npy_entry(
archive,
"payload_length",
"<i4",
(n,),
pack_i4_array([s.payload_length for s in samples]),
)
meta_bytes = json.dumps(
[sample_to_public_dict(sample) for sample in samples],
ensure_ascii=False,
separators=(",", ":"),
).encode("utf-8")
write_npy_entry(archive, "records_json_utf8", "|u1", (len(meta_bytes),), meta_bytes)
os.replace(temp, path)
def load_gps_post_z_samples(
root: Path,
dorec_index: dict[str, list[Path]],
tz: timezone,
timezone_text: str,
) -> tuple[list[RtkSample], dict[str, int]]:
records = discover_records(root, "GPS-POST-Z")
samples: list[RtkSample] = []
stats = {
"discovered": len(records),
"parsed_ok": 0,
"parse_errors": 0,
"read_errors": 0,
}
for record in records:
timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz)
sample = RtkSample(
index=len(samples),
source="dobject",
object_name=record.object_name,
dotnet_ticks=record.dotnet_ticks,
timestamp_iso_local=timestamp_iso,
unix_time_ns=unix_ns,
log_time=record.log_time,
record_id=record.log_record_id,
source_log=record.source_log,
source_dorec=record.source_dorec,
source_offset=record.source_offset,
payload_length=record.payload_length,
)
try:
dorec_path = choose_dorec(dorec_index, record.source_dorec)
record_meta, payload = read_record(dorec_path, record)
sample.record_id = str(record_meta["record_id"])
text = payload.decode("utf-8", errors="replace")
sample.raw_text = text
parsed = parse_gps_post_z_text(text)
sample.counter = parsed["counter"] # type: ignore[assignment]
sample.lat = parsed["lat"] # type: ignore[assignment]
sample.lon = parsed["lon"] # type: ignore[assignment]
sample.alt_m = parsed["alt_m"] # type: ignore[assignment]
sample.raw_heading_deg = parsed["raw_heading_deg"] # type: ignore[assignment]
sample.vehicle_heading_deg = parsed["vehicle_heading_deg"] # type: ignore[assignment]
sample.fix = parsed["fix"] # type: ignore[assignment]
sample.sat = parsed["sat"] # type: ignore[assignment]
sample.position_valid = parsed["position_valid"] # type: ignore[assignment]
sample.heading_valid = parsed["heading_valid"] # type: ignore[assignment]
sample.heading_solution = parsed["heading_solution"] # type: ignore[assignment]
sample.position_time = parsed["position_time"] # type: ignore[assignment]
sample.heading_time = parsed["heading_time"] # type: ignore[assignment]
sample.last_line = parsed["last_line"] # type: ignore[assignment]
stats["parsed_ok"] += 1
except Exception as exc:
sample.parse_error = f"{type(exc).__name__}: {exc}"
if "read" in type(exc).__name__.casefold() or "mismatch" in str(exc).casefold():
stats["read_errors"] += 1
else:
stats["parse_errors"] += 1
samples.append(sample)
_ = timezone_text
return samples, stats
def load_rtk_binary_samples(
root: Path,
dorec_index: dict[str, list[Path]],
tz: timezone,
) -> tuple[list[RtkSample], dict[str, int]]:
records = discover_records(root, "rtk")
samples: list[RtkSample] = []
stats = {"discovered": len(records), "parsed_ok": 0, "parse_errors": 0, "read_errors": 0}
for record in records:
timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz)
sample = RtkSample(
index=len(samples),
source="dobject",
object_name=record.object_name,
dotnet_ticks=record.dotnet_ticks,
timestamp_iso_local=timestamp_iso,
unix_time_ns=unix_ns,
log_time=record.log_time,
record_id=record.log_record_id,
source_log=record.source_log,
source_dorec=record.source_dorec,
source_offset=record.source_offset,
payload_length=record.payload_length,
)
try:
dorec_path = choose_dorec(dorec_index, record.source_dorec)
record_meta, payload = read_record(dorec_path, record)
sample.record_id = str(record_meta["record_id"])
parsed = parse_rtk_binary_payload(payload)
sample.device_name = str(parsed["device_name"])
sample.device_stamp_hex = str(parsed["device_stamp_hex"])
sample.counter = int(parsed["counter"]) # type: ignore[arg-type]
sample.lat = float(parsed["lat"]) # type: ignore[arg-type]
sample.lon = float(parsed["lon"]) # type: ignore[arg-type]
sample.alt_m = float(parsed["alt_m"]) # type: ignore[arg-type]
sample.raw_heading_deg = float(parsed["raw_heading_deg"]) # type: ignore[arg-type]
sample.vehicle_heading_deg = float(parsed["vehicle_heading_deg"]) # type: ignore[arg-type]
sample.fix = int(parsed["fix"]) # type: ignore[arg-type]
sample.sat = int(parsed["sat"]) # type: ignore[arg-type]
sample.position_valid = bool(parsed["position_valid"])
sample.heading_valid = bool(parsed["heading_valid"])
sample.heading_solution = str(parsed["heading_solution"])
sample.position_time = str(parsed["position_time"])
sample.heading_time = str(parsed["heading_time"])
sample.last_line = parsed["last_line"] # type: ignore[assignment]
sample.raw_text = json.dumps(parsed, ensure_ascii=False, separators=(",", ":"))
stats["parsed_ok"] += 1
except Exception as exc:
sample.parse_error = f"{type(exc).__name__}: {exc}"
stats["parse_errors"] += 1
samples.append(sample)
return samples, stats
def load_gps_post_samples(
root: Path,
dorec_index: dict[str, list[Path]],
tz: timezone,
) -> tuple[list[dict[str, object]], dict[str, int]]:
records = discover_records(root, "GPS-POST")
rows: list[dict[str, object]] = []
stats = {"discovered": len(records), "parsed_ok": 0, "parse_errors": 0, "read_errors": 0}
for index, record in enumerate(records):
timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz)
row: dict[str, object] = {
"index": index,
"object_name": record.object_name,
"dotnet_ticks": record.dotnet_ticks,
"timestamp_iso_local": timestamp_iso,
"unix_time_ns": unix_ns,
"log_time": record.log_time,
"record_id": record.log_record_id,
"source_log": record.source_log,
"source_dorec": record.source_dorec,
"source_offset": record.source_offset,
"payload_length": record.payload_length,
"parse_error": "",
}
try:
dorec_path = choose_dorec(dorec_index, record.source_dorec)
record_meta, payload = read_record(dorec_path, record)
row["record_id"] = record_meta["record_id"]
parsed = parse_gps_post_payload(payload)
row.update(parsed)
stats["parsed_ok"] += 1
except Exception as exc:
row["parse_error"] = f"{type(exc).__name__}: {exc}"
stats["parse_errors"] += 1
rows.append(row)
return rows, stats
def load_gps_post_z_text_logs(root: Path) -> tuple[list[dict[str, object]], dict[str, int]]:
rows: list[dict[str, object]] = []
stats = {"files": 0, "lines": 0, "parsed_ok": 0, "parse_errors": 0}
text_root = root / "GPS-POST-Z"
if not text_root.is_dir():
return rows, stats
for log_path in sorted(text_root.rglob("*.log")):
stats["files"] += 1
relative = log_path.relative_to(root).as_posix()
with log_path.open("r", encoding="utf-8", errors="replace") as stream:
for line_no, line in enumerate(stream, start=1):
match = GPS_TEXT_RE.search(line)
if not match:
continue
stats["lines"] += 1
body = match.group("body").strip()
row: dict[str, object] = {
"index": len(rows),
"source": "GPS-POST-Z-text-log",
"source_log": relative,
"line_no": line_no,
"log_time": match.group("log_time"),
"raw_text": body,
"parse_error": "",
}
try:
parsed = parse_gps_post_z_text(body)
row.update(
{
"lat": parsed["lat"],
"lon": parsed["lon"],
"alt_m": parsed["alt_m"],
"raw_heading_deg": parsed["raw_heading_deg"],
"vehicle_heading_deg": parsed["vehicle_heading_deg"],
"fix": parsed["fix"],
"sat": parsed["sat"],
"position_valid": parsed["position_valid"],
"heading_valid": parsed["heading_valid"],
"heading_solution": parsed["heading_solution"],
"position_time": parsed["position_time"],
"heading_time": parsed["heading_time"],
}
)
stats["parsed_ok"] += 1
except Exception as exc:
row["parse_error"] = f"{type(exc).__name__}: {exc}"
stats["parse_errors"] += 1
rows.append(row)
return rows, stats
def match_rtk(unix_time_ns: int, samples: Sequence[RtkSample]) -> dict[str, object]:
if not samples:
return {
"matched": False,
"nearest_index": -1,
"prev_index": -1,
"next_index": -1,
"dt_ns": None,
"prev_dt_ns": None,
"next_dt_ns": None,
"sample": None,
}
ticks = [sample.unix_time_ns for sample in samples]
pos = bisect.bisect_left(ticks, unix_time_ns)
if pos < len(samples) and ticks[pos] == unix_time_ns:
nearest = prev_index = next_index = pos
else:
candidates: list[int] = []
if pos < len(samples):
candidates.append(pos)
if pos > 0:
candidates.append(pos - 1)
nearest = min(candidates, key=lambda idx: (abs(ticks[idx] - unix_time_ns), idx))
prev_index = pos - 1 if pos > 0 else -1
next_index = pos if pos < len(samples) else -1
sample = samples[nearest]
prev_dt = None if prev_index < 0 else unix_time_ns - samples[prev_index].unix_time_ns
next_dt = None if next_index < 0 else samples[next_index].unix_time_ns - unix_time_ns
return {
"matched": True,
"nearest_index": nearest,
"prev_index": prev_index,
"next_index": next_index,
"dt_ns": unix_time_ns - sample.unix_time_ns,
"prev_dt_ns": prev_dt,
"next_dt_ns": next_dt,
"sample": sample,
}
def rtk_frame_arrays(match: dict[str, object]) -> dict[str, tuple[str, Sequence[int], bytes]]:
sample: RtkSample | None = match["sample"] # type: ignore[assignment]
return {
"rtk_nearest_index": ("<i4", (1,), scalar_bytes("<i", int(match["nearest_index"]))),
"rtk_prev_index": ("<i4", (1,), scalar_bytes("<i", int(match["prev_index"]))),
"rtk_next_index": ("<i4", (1,), scalar_bytes("<i", int(match["next_index"]))),
"rtk_dt_ns": (
"<i8",
(1,),
scalar_bytes("<q", 0 if match["dt_ns"] is None else int(match["dt_ns"])),
),
"rtk_prev_dt_ns": (
"<i8",
(1,),
scalar_bytes("<q", 0 if match["prev_dt_ns"] is None else int(match["prev_dt_ns"])),
),
"rtk_next_dt_ns": (
"<i8",
(1,),
scalar_bytes("<q", 0 if match["next_dt_ns"] is None else int(match["next_dt_ns"])),
),
"rtk_lat": ("<f8", (1,), scalar_bytes("<d", nan_if_none(None if sample is None else sample.lat))),
"rtk_lon": ("<f8", (1,), scalar_bytes("<d", nan_if_none(None if sample is None else sample.lon))),
"rtk_alt_m": ("<f8", (1,), scalar_bytes("<d", nan_if_none(None if sample is None else sample.alt_m))),
"rtk_raw_heading_deg": (
"<f8",
(1,),
scalar_bytes("<d", nan_if_none(None if sample is None else sample.raw_heading_deg)),
),
"rtk_vehicle_heading_deg": (
"<f8",
(1,),
scalar_bytes("<d", nan_if_none(None if sample is None else sample.vehicle_heading_deg)),
),
"rtk_fix": ("<i4", (1,), scalar_bytes("<i", int_or_sentinel(None if sample is None else sample.fix))),
"rtk_sat": ("<i4", (1,), scalar_bytes("<i", int_or_sentinel(None if sample is None else sample.sat))),
"rtk_position_valid": (
"|u1",
(1,),
bytes((1 if sample and sample.position_valid else 0,)),
),
"rtk_heading_valid": (
"|u1",
(1,),
bytes((1 if sample and sample.heading_valid else 0,)),
),
}
def export_rtk_sidecars(
output: Path,
compress: bool,
gps_post_z: Sequence[RtkSample],
rtk_binary: Sequence[RtkSample],
gps_post_rows: Sequence[dict[str, object]],
text_rows: Sequence[dict[str, object]],
) -> dict[str, object]:
rtk_dir = output / "rtk"
rtk_dir.mkdir(exist_ok=True)
written: dict[str, object] = {"directory": "rtk"}
export_rtk_table_npz(rtk_dir / "gps_post_z.npz", gps_post_z, compress)
gps_dicts = [sample_to_public_dict(sample) for sample in gps_post_z]
write_jsonl(rtk_dir / "gps_post_z.jsonl", gps_dicts)
write_csv_rows(
rtk_dir / "gps_post_z.csv",
[
"index",
"dotnet_ticks",
"timestamp_iso_local",
"unix_time_ns",
"log_time",
"record_id",
"counter",
"lat",
"lon",
"alt_m",
"raw_heading_deg",
"vehicle_heading_deg",
"fix",
"sat",
"position_valid",
"heading_valid",
"heading_solution",
"position_time",
"heading_time",
"last_line",
"source_log",
"source_dorec",
"source_offset",
"payload_length",
"parse_error",
"raw_text",
],
gps_dicts,
)
written["gps_post_z"] = {
"npz": "rtk/gps_post_z.npz",
"csv": "rtk/gps_post_z.csv",
"jsonl": "rtk/gps_post_z.jsonl",
"count": len(gps_post_z),
}
export_rtk_table_npz(rtk_dir / "rtk_binary.npz", rtk_binary, compress)
rtk_dicts = [sample_to_public_dict(sample) for sample in rtk_binary]
write_jsonl(rtk_dir / "rtk_binary.jsonl", rtk_dicts)
write_csv_rows(
rtk_dir / "rtk_binary.csv",
[
"index",
"dotnet_ticks",
"timestamp_iso_local",
"unix_time_ns",
"log_time",
"record_id",
"device_name",
"device_stamp_hex",
"counter",
"lat",
"lon",
"alt_m",
"raw_heading_deg",
"vehicle_heading_deg",
"fix",
"sat",
"position_valid",
"heading_valid",
"heading_solution",
"position_time",
"heading_time",
"last_line",
"source_log",
"source_dorec",
"source_offset",
"payload_length",
"parse_error",
],
rtk_dicts,
)
written["rtk_binary"] = {
"npz": "rtk/rtk_binary.npz",
"csv": "rtk/rtk_binary.csv",
"jsonl": "rtk/rtk_binary.jsonl",
"count": len(rtk_binary),
}
write_jsonl(rtk_dir / "gps_post.jsonl", gps_post_rows)
write_csv_rows(
rtk_dir / "gps_post.csv",
[
"index",
"dotnet_ticks",
"timestamp_iso_local",
"unix_time_ns",
"log_time",
"record_id",
"device_name",
"counter",
"unknown0",
"flags_hex",
"device_stamp_hex",
"source_log",
"source_dorec",
"source_offset",
"payload_length",
"payload_hex",
"parse_error",
],
gps_post_rows,
)
# Compact NPZ for GPS-POST counters / stamps only.
temp = (rtk_dir / "gps_post.npz").with_suffix(".npz.tmp")
with open_npz_writer(temp, compress) as archive:
n = len(gps_post_rows)
write_npy_entry(
archive,
"index",
"<i4",
(n,),
pack_i4_array([int(row["index"]) for row in gps_post_rows]),
)
write_npy_entry(
archive,
"dotnet_ticks",
"<i8",
(n,),
pack_i8_array([int(row["dotnet_ticks"]) for row in gps_post_rows]),
)
write_npy_entry(
archive,
"unix_time_ns",
"<i8",
(n,),
pack_i8_array([int(row["unix_time_ns"]) for row in gps_post_rows]),
)
write_npy_entry(
archive,
"counter",
"<i4",
(n,),
pack_i4_array([int(row.get("counter", -1) or -1) for row in gps_post_rows]),
)
meta_bytes = json.dumps(list(gps_post_rows), ensure_ascii=False, separators=(",", ":")).encode("utf-8")
write_npy_entry(archive, "records_json_utf8", "|u1", (len(meta_bytes),), meta_bytes)
os.replace(temp, rtk_dir / "gps_post.npz")
written["gps_post"] = {
"npz": "rtk/gps_post.npz",
"csv": "rtk/gps_post.csv",
"jsonl": "rtk/gps_post.jsonl",
"count": len(gps_post_rows),
}
write_jsonl(rtk_dir / "gps_post_z_text.jsonl", text_rows)
write_csv_rows(
rtk_dir / "gps_post_z_text.csv",
[
"index",
"source_log",
"line_no",
"log_time",
"lat",
"lon",
"alt_m",
"raw_heading_deg",
"vehicle_heading_deg",
"fix",
"sat",
"position_valid",
"heading_valid",
"heading_solution",
"position_time",
"heading_time",
"raw_text",
"parse_error",
],
text_rows,
)
written["gps_post_z_text"] = {
"csv": "rtk/gps_post_z_text.csv",
"jsonl": "rtk/gps_post_z_text.jsonl",
"count": len(text_rows),
}
return written
def build_validation_report(
*,
lidar_stats: dict[str, object],
gps_post_z_stats: dict[str, int],
rtk_binary_stats: dict[str, int],
gps_post_stats: dict[str, int],
text_stats: dict[str, int],
match_dts_ns: Sequence[int],
interior_dts_ns: Sequence[int],
edge_frames: int,
matched_frames: int,
unmatched_frames: int,
rtk_max_dt_ms: float,
rtk_sidecars: bool,
) -> dict[str, object]:
abs_dts = [abs(value) for value in match_dts_ns]
abs_interior = [abs(value) for value in interior_dts_ns]
threshold_ns = rtk_max_dt_ms * 1_000_000.0
over_threshold = sum(1 for value in abs_interior if value > threshold_ns)
checks: list[dict[str, object]] = [
{
"name": "lidar_payload_length",
"ok": lidar_stats.get("frames_with_errors", 0) == 0,
"detail": "Each lidar payload must equal 8 + point_count * 20.",
},
{
"name": "gps_post_z_parse",
"ok": gps_post_z_stats.get("parse_errors", 0) == 0
and gps_post_z_stats.get("read_errors", 0) == 0,
"detail": "All GPS-POST-Z dobject payloads should parse.",
},
{
"name": "lidar_rtk_coverage",
"ok": unmatched_frames == 0,
"detail": "Every exported lidar frame should have a nearest GPS-POST-Z sample.",
},
{
"name": "rtk_time_alignment",
"ok": over_threshold == 0,
"detail": (
f"Interior frames (with both prev/next RTK) should have nearest |dt| "
f"<= {rtk_max_dt_ms} ms."
),
},
]
if rtk_sidecars:
checks.insert(
2,
{
"name": "rtk_binary_parse",
"ok": rtk_binary_stats.get("parse_errors", 0) == 0,
"detail": "All rtk binary payloads should parse as UNICORE_N4_RTK_V1.",
},
)
report = {
"format_version": FORMAT_VERSION,
"generated_at": datetime.now().astimezone().isoformat(),
"lidar": lidar_stats,
"rtk_sidecars": rtk_sidecars,
"rtk_sources": {
"GPS-POST-Z_dobject": gps_post_z_stats,
"rtk_dobject": rtk_binary_stats if rtk_sidecars else {"skipped": True},
"GPS-POST_dobject": gps_post_stats if rtk_sidecars else {"skipped": True},
"GPS-POST-Z_text_log": text_stats if rtk_sidecars else {"skipped": True},
},
"matching": {
"primary_source": "GPS-POST-Z",
"matched_frames": matched_frames,
"unmatched_frames": unmatched_frames,
"edge_frames_outside_rtk_span": edge_frames,
"rtk_max_dt_ms_threshold": rtk_max_dt_ms,
"interior_frames_over_threshold": over_threshold,
"dt_ns": {
"count": len(abs_dts),
"min": min(match_dts_ns) if match_dts_ns else None,
"max": max(match_dts_ns) if match_dts_ns else None,
"abs_min": min(abs_dts) if abs_dts else None,
"abs_max": max(abs_dts) if abs_dts else None,
"abs_mean": (sum(abs_dts) / len(abs_dts)) if abs_dts else None,
},
"interior_dt_ns": {
"count": len(abs_interior),
"abs_min": min(abs_interior) if abs_interior else None,
"abs_max": max(abs_interior) if abs_interior else None,
"abs_mean": (sum(abs_interior) / len(abs_interior)) if abs_interior else None,
},
},
"checks": checks,
}
report["ok"] = all(bool(check["ok"]) for check in checks)
return report
def write_package_readme(
path: Path,
*,
format_version: str,
frame_count: int,
include_rtk: bool,
timezone_text: str,
) -> None:
rtk_note = (
"每帧 NPZ 已嵌入最近 RTK：`rtk_lat` / `rtk_lon` / `rtk_alt_m` / "
"`rtk_vehicle_heading_deg` / `rtk_fix` / `rtk_sat` 等。\n"
if include_rtk
else "本包未嵌入 RTK（导出时使用了 `--skip-rtk`）。\n"
)
text = """# FrontLidar 数据集
格式版本：`@@FORMAT_VERSION@@`
帧数：`@@FRAME_COUNT@@`
时区：`@@TIMEZONE@@`
## 目录
```text
.
README.md # 本说明
frames/ # 逐帧 NPZ
frontlidar_<seq>_<ticks>_frame<counter>.npz
```
直接打包本目录即可分发。
## 依赖
```powershell
pip install numpy
```
## 快速读取
```python
import json
from pathlib import Path
import numpy as np
root = Path(__file__).resolve().parent # 或改成数据集路径
frame_path = next(sorted((root / "frames").glob("*.npz")))
with np.load(frame_path, allow_pickle=False) as f:
points = f["points_raw"] # float32 (N, 5)
unix_ns = int(f["unix_time_ns"][0])
meta = json.loads(f["metadata_json_utf8"].tobytes().decode("utf-8"))
# 列: d_mm, azimuth_deg, altitude_deg, intensity, progression
d_mm, az, alt, intensity, prog = (points[:, i] for i in range(5))
# RTK（若导出时未 --skip-rtk）
if "rtk_lat" in f.files:
lat = float(f["rtk_lat"][0])
lon = float(f["rtk_lon"][0])
heading = float(f["rtk_vehicle_heading_deg"][0])
dt_ms = int(f["rtk_dt_ns"][0]) / 1e6
print(lat, lon, heading, dt_ms, meta.get("rtk", {}).get("heading_solution"))
```
@@RTK_NOTE@@
## 点云列
| 列 | 字段 | 单位 |
|---:|---|---|
| 0 | d_mm | mm |
| 1 | azimuth_deg | ° |
| 2 | altitude_deg | ° |
| 3 | intensity | 设备定义 |
| 4 | progression | 0–1 |
极坐标转传感器 XYZ（mm）：
```python
az = np.deg2rad(points[:, 1])
alt = np.deg2rad(points[:, 2])
d = points[:, 0]
xyz = np.column_stack((
d * np.cos(alt) * np.cos(az),
d * np.cos(alt) * np.sin(az),
d * np.sin(alt),
)).astype(np.float32)
```
## 时间戳
- `dotnet_ticks`：原始权威时间
- `unix_time_ns`：按导出时区转换的 Unix 纳秒
- 标量字段均为 shape `(1,)`，用 `[0]` 取出
## 批量遍历
```python
for path in sorted((root / "frames").glob("*.npz")):
with np.load(path, allow_pickle=False) as f:
pts = f["points_raw"]
# ...
```
更多字段说明见导出仓库中的 `FRONTLIDAR_NPZ_READ.md`（若一并提供）。
"""
text = (
text.replace("@@FORMAT_VERSION@@", format_version)
.replace("@@FRAME_COUNT@@", str(frame_count))
.replace("@@TIMEZONE@@", timezone_text)
.replace("@@RTK_NOTE@@", rtk_note)
)
path.write_text(text, encoding="utf-8")
def export_dataset(args: argparse.Namespace) -> int:
started = time.time()
root = resolve_dlog_root(args.dlog)
output = Path(args.out).expanduser().resolve()
output.mkdir(parents=True, exist_ok=True)
frames_dir = output / "frames"
frames_dir.mkdir(exist_ok=True)
tz = parse_timezone(args.timezone)
records = discover_records(root, args.object)
selected = records[:: args.stride]
if args.max_frames:
selected = selected[: args.max_frames]
if not selected:
raise RuntimeError(f"no DObject records named {args.object!r} found under {root}")
dorec_index = index_dorec_files(root)
rotation = rotation_matrix(args.yaw, args.pitch, args.roll)
translation = (args.x, args.y, args.z)
extension = "npz" if args.format == "npz" else "pkl"
include_rtk = not args.skip_rtk
rtk_sidecars = bool(args.rtk_sidecars)
gps_post_z: list[RtkSample] = []
rtk_binary: list[RtkSample] = []
gps_post_rows: list[dict[str, object]] = []
text_rows: list[dict[str, object]] = []
gps_post_z_stats = {"discovered": 0, "parsed_ok": 0, "parse_errors": 0, "read_errors": 0}
rtk_binary_stats = {"discovered": 0, "parsed_ok": 0, "parse_errors": 0, "read_errors": 0}
gps_post_stats = {"discovered": 0, "parsed_ok": 0, "parse_errors": 0, "read_errors": 0}
text_stats = {"files": 0, "lines": 0, "parsed_ok": 0, "parse_errors": 0}
rtk_outputs: dict[str, object] = {}
if include_rtk:
print("Loading GPS-POST-Z for per-frame RTK matching...", flush=True)
gps_post_z, gps_post_z_stats = load_gps_post_z_samples(root, dorec_index, tz, args.timezone)
if rtk_sidecars:
print("Loading RTK sidecars...", flush=True)
rtk_binary, rtk_binary_stats = load_rtk_binary_samples(root, dorec_index, tz)
gps_post_rows, gps_post_stats = load_gps_post_samples(root, dorec_index, tz)
text_rows, text_stats = load_gps_post_z_text_logs(root)
rtk_outputs = export_rtk_sidecars(
output,
bool(args.compress and args.format == "npz"),
gps_post_z,
rtk_binary,
gps_post_rows,
text_rows,
)
print(
f"RTK loaded: GPS-POST-Z={len(gps_post_z)} rtk={len(rtk_binary)} "
f"GPS-POST={len(gps_post_rows)} text={len(text_rows)} (sidecars on)",
flush=True,
)
else:
print(
f"RTK loaded: GPS-POST-Z={len(gps_post_z)} (sidecars off; use --rtk-sidecars to write rtk/)",
flush=True,
)
write_reports = bool(args.write_reports)
reports_dir = output / "reports"
if write_reports:
reports_dir.mkdir(exist_ok=True)
manifest_tmp = reports_dir / "manifest.partial.csv"
manifest_final = reports_dir / "manifest.csv"
match_tmp = reports_dir / "rtk_match.partial.csv"
match_final = reports_dir / "rtk_match.csv"
manifest_stream_cm: object = manifest_tmp.open("w", newline="", encoding="utf-8-sig")
match_stream = (
match_tmp.open("w", newline="", encoding="utf-8-sig") if include_rtk else None
)
else:
manifest_tmp = manifest_final = match_tmp = match_final = None
manifest_stream_cm = nullcontext(io.StringIO())
match_stream = io.StringIO() if include_rtk else None
exported = skipped = resumed = total_points = 0
matched_frames = unmatched_frames = edge_frames = 0
match_dts_ns: list[int] = []
interior_dts_ns: list[int] = []
match_fieldnames = [
"sequence",
"lidar_unix_time_ns",
"lidar_dotnet_ticks",
"output_file",
"rtk_matched",
"rtk_nearest_index",
"rtk_prev_index",
"rtk_next_index",
"rtk_dt_ns",
"rtk_prev_dt_ns",
"rtk_next_dt_ns",
"rtk_dotnet_ticks",
"rtk_unix_time_ns",
"rtk_lat",
"rtk_lon",
"rtk_alt_m",
"rtk_raw_heading_deg",
"rtk_vehicle_heading_deg",
"rtk_fix",
"rtk_sat",
"rtk_position_valid",
"rtk_heading_valid",
"rtk_heading_solution",
"rtk_position_time",
"rtk_heading_time",
"rtk_last_line",
"rtk_source_dorec",
"rtk_source_offset",
]
match_writer = csv.DictWriter(match_stream, fieldnames=match_fieldnames) if match_stream else None
if match_writer:
match_writer.writeheader()
with manifest_stream_cm as manifest_stream:
writer = csv.DictWriter(manifest_stream, fieldnames=manifest_fields(include_rtk))
writer.writeheader()
for selected_index, record in enumerate(selected):
row: dict[str, object] = {
"sequence": record.sequence,
"status": "error",
"object_name": record.object_name,
"dotnet_ticks": record.dotnet_ticks,
"timestamp_iso_local": "",
"unix_time_ns": "",
"log_time": record.log_time,
"record_id": record.log_record_id,
"record_id_bytes_hex": "",
"frame_counter": "",
"point_count": "",
"payload_length": record.payload_length,
"source_log": record.source_log,
"source_dorec": record.source_dorec,
"source_offset": record.source_offset,
"output_file": "",
"error": "",
}
if include_rtk:
row.update(
{
"rtk_matched": False,
"rtk_nearest_index": -1,
"rtk_prev_index": -1,
"rtk_next_index": -1,
"rtk_dt_ns": "",
"rtk_prev_dt_ns": "",
"rtk_next_dt_ns": "",
"rtk_lat": "",
"rtk_lon": "",
"rtk_alt_m": "",
"rtk_vehicle_heading_deg": "",
"rtk_raw_heading_deg": "",
"rtk_fix": "",
"rtk_sat": "",
"rtk_position_valid": "",
"rtk_heading_valid": "",
"rtk_heading_solution": "",
}
)
try:
dorec_path = choose_dorec(dorec_index, record.source_dorec)
record_meta, payload = read_record(dorec_path, record)
frame_counter, point_count, raw_points = parse_lidar_payload(payload)
timestamp_iso, unix_ns = dotnet_ticks_to_values(record.dotnet_ticks, tz)
filename = frame_filename(record, frame_counter, extension)
relative_output = (Path("frames") / filename).as_posix()
frame_path = output / relative_output
metadata: dict[str, object] = {
"format_version": FORMAT_VERSION,
"sequence": record.sequence,
"object_name": record.object_name,
"dotnet_ticks": record.dotnet_ticks,
"timestamp_iso_local": timestamp_iso,
"unix_time_ns": unix_ns,
"timezone": args.timezone,
"log_time": record.log_time,
"record_id": record_meta["record_id"],
"record_id_bytes_hex": record_meta["record_id_bytes_hex"],
"frame_counter": frame_counter,
"point_count": point_count,
"payload_length": record_meta["payload_length"],
"source_log": record.source_log,
"source_dorec": record.source_dorec,
"source_offset": record.source_offset,
"point_columns": ["d_mm", "azimuth_deg", "altitude_deg", "intensity", "progression"],
"extrinsic": {
"translation_mm": list(translation),
"yaw_pitch_roll_deg": [args.yaw, args.pitch, args.roll],
"rotation_row_major": list(rotation),
},
}
rtk_arrays = None
match_info: dict[str, object] | None = None
if include_rtk:
match_info = match_rtk(unix_ns, gps_post_z)
sample: RtkSample | None = match_info["sample"] # type: ignore[assignment]
if match_info["matched"]:
matched_frames += 1
dt_ns = int(match_info["dt_ns"])
match_dts_ns.append(dt_ns)
prev_i = int(match_info["prev_index"])
next_i = int(match_info["next_index"])
if prev_i < 0 or next_i < 0:
edge_frames += 1
else:
interior_dts_ns.append(dt_ns)
else:
unmatched_frames += 1
rtk_meta = {
"matched": bool(match_info["matched"]),
"nearest_index": match_info["nearest_index"],
"prev_index": match_info["prev_index"],
"next_index": match_info["next_index"],
"dt_ns": match_info["dt_ns"],
"prev_dt_ns": match_info["prev_dt_ns"],
"next_dt_ns": match_info["next_dt_ns"],
"primary_source": "GPS-POST-Z",
}
if sample is not None:
rtk_meta.update(
{
"dotnet_ticks": sample.dotnet_ticks,
"unix_time_ns": sample.unix_time_ns,
"timestamp_iso_local": sample.timestamp_iso_local,
"lat": sample.lat,
"lon": sample.lon,
"alt_m": sample.alt_m,
"raw_heading_deg": sample.raw_heading_deg,
"vehicle_heading_deg": sample.vehicle_heading_deg,
"fix": sample.fix,
"sat": sample.sat,
"position_valid": sample.position_valid,
"heading_valid": sample.heading_valid,
"heading_solution": sample.heading_solution,
"position_time": sample.position_time,
"heading_time": sample.heading_time,
"last_line": sample.last_line,
"counter": sample.counter,
"record_id": sample.record_id,
"source_dorec": sample.source_dorec,
"source_offset": sample.source_offset,
}
)
metadata["rtk"] = rtk_meta
rtk_arrays = rtk_frame_arrays(match_info)
row.update(
{
"rtk_matched": bool(match_info["matched"]),
"rtk_nearest_index": match_info["nearest_index"],
"rtk_prev_index": match_info["prev_index"],
"rtk_next_index": match_info["next_index"],
"rtk_dt_ns": "" if match_info["dt_ns"] is None else match_info["dt_ns"],
"rtk_prev_dt_ns": "" if match_info["prev_dt_ns"] is None else match_info["prev_dt_ns"],
"rtk_next_dt_ns": "" if match_info["next_dt_ns"] is None else match_info["next_dt_ns"],
"rtk_lat": "" if sample is None or sample.lat is None else sample.lat,
"rtk_lon": "" if sample is None or sample.lon is None else sample.lon,
"rtk_alt_m": "" if sample is None or sample.alt_m is None else sample.alt_m,
"rtk_vehicle_heading_deg": (
"" if sample is None or sample.vehicle_heading_deg is None else sample.vehicle_heading_deg
),
"rtk_raw_heading_deg": (
"" if sample is None or sample.raw_heading_deg is None else sample.raw_heading_deg
),
"rtk_fix": "" if sample is None or sample.fix is None else sample.fix,
"rtk_sat": "" if sample is None or sample.sat is None else sample.sat,
"rtk_position_valid": "" if sample is None else sample.position_valid,
"rtk_heading_valid": "" if sample is None else sample.heading_valid,
"rtk_heading_solution": "" if sample is None else sample.heading_solution,
}
)
if match_writer is not None:
match_writer.writerow(
{
"sequence": record.sequence,
"lidar_unix_time_ns": unix_ns,
"lidar_dotnet_ticks": record.dotnet_ticks,
"output_file": relative_output,
"rtk_matched": bool(match_info["matched"]),
"rtk_nearest_index": match_info["nearest_index"],
"rtk_prev_index": match_info["prev_index"],
"rtk_next_index": match_info["next_index"],
"rtk_dt_ns": "" if match_info["dt_ns"] is None else match_info["dt_ns"],
"rtk_prev_dt_ns": "" if match_info["prev_dt_ns"] is None else match_info["prev_dt_ns"],
"rtk_next_dt_ns": "" if match_info["next_dt_ns"] is None else match_info["next_dt_ns"],
"rtk_dotnet_ticks": "" if sample is None else sample.dotnet_ticks,
"rtk_unix_time_ns": "" if sample is None else sample.unix_time_ns,
"rtk_lat": "" if sample is None else sample.lat,
"rtk_lon": "" if sample is None else sample.lon,
"rtk_alt_m": "" if sample is None else sample.alt_m,
"rtk_raw_heading_deg": "" if sample is None else sample.raw_heading_deg,
"rtk_vehicle_heading_deg": "" if sample is None else sample.vehicle_heading_deg,
"rtk_fix": "" if sample is None else sample.fix,
"rtk_sat": "" if sample is None else sample.sat,
"rtk_position_valid": "" if sample is None else sample.position_valid,
"rtk_heading_valid": "" if sample is None else sample.heading_valid,
"rtk_heading_solution": "" if sample is None else sample.heading_solution,
"rtk_position_time": "" if sample is None else sample.position_time,
"rtk_heading_time": "" if sample is None else sample.heading_time,
"rtk_last_line": "" if sample is None else sample.last_line,
"rtk_source_dorec": "" if sample is None else sample.source_dorec,
"rtk_source_offset": "" if sample is None else sample.source_offset,
}
)
row.update(
{
"timestamp_iso_local": timestamp_iso,
"unix_time_ns": unix_ns,
"record_id": record_meta["record_id"],
"record_id_bytes_hex": record_meta["record_id_bytes_hex"],
"frame_counter": frame_counter,
"point_count": point_count,
"output_file": relative_output,
}
)
if args.resume and frame_path.exists():
row["status"] = "resumed"
resumed += 1
else:
xyz_sensor, xyz_cart = generate_xyz(raw_points, args.include_xyz, translation, rotation)
if args.format == "npz":
export_npz(
frame_path,
metadata,
raw_points,
point_count,
xyz_sensor,
xyz_cart,
args.compress,
rtk_arrays=rtk_arrays,
)
else:
export_pickle(frame_path, metadata, raw_points, point_count, xyz_sensor, xyz_cart)
row["status"] = "exported"
exported += 1
total_points += point_count
except Exception as exc:
skipped += 1
row["error"] = f"{type(exc).__name__}: {exc}"
writer.writerow(row)
if (selected_index + 1) % 25 == 0 or selected_index + 1 == len(selected):
manifest_stream.flush()
if match_stream is not None:
match_stream.flush()
print(
f"[{selected_index + 1}/{len(selected)}] exported={exported} resumed={resumed} "
f"errors={skipped} points={total_points}",
flush=True,
)
if match_stream is not None:
match_stream.close()
if write_reports and match_tmp is not None and match_final is not None:
os.replace(match_tmp, match_final)
if write_reports and manifest_tmp is not None and manifest_final is not None:
os.replace(manifest_tmp, manifest_final)
lidar_stats = {
"records_discovered": len(records),
"records_selected": len(selected),
"frames_exported": exported,
"frames_resumed": resumed,
"frames_with_errors": skipped,
"total_points_in_manifest": total_points,
}
validation = build_validation_report(
lidar_stats=lidar_stats,
gps_post_z_stats=gps_post_z_stats,
rtk_binary_stats=rtk_binary_stats,
gps_post_stats=gps_post_stats,
text_stats=text_stats,
match_dts_ns=match_dts_ns,
interior_dts_ns=interior_dts_ns,
edge_frames=edge_frames,
matched_frames=matched_frames,
unmatched_frames=unmatched_frames,
rtk_max_dt_ms=args.rtk_max_dt_ms,
rtk_sidecars=rtk_sidecars,
)
write_package_readme(
output / "README.md",
format_version=FORMAT_VERSION,
frame_count=exported + resumed,
include_rtk=include_rtk,
timezone_text=args.timezone,
)
metadata = {
"format_version": FORMAT_VERSION,
"generated_at": datetime.now().astimezone().isoformat(),
"source_dlog": str(root),
"object_name": args.object,
"output_format": args.format,
"npz_compressed": bool(args.compress and args.format == "npz"),
"include_xyz": args.include_xyz,
"include_rtk": include_rtk,
"rtk_sidecars": rtk_sidecars,
"write_reports": write_reports,
"timezone": args.timezone,
"timezone_offset_minutes": timezone_minutes(tz),
"rtk_max_dt_ms": args.rtk_max_dt_ms,
"rtk_objects": list(RTK_OBJECT_NAMES),
"rtk_outputs": rtk_outputs if rtk_sidecars else None,
"package_contents": ["README.md", "frames/"]
+ (["rtk/"] if rtk_sidecars else [])
+ (["reports/"] if write_reports else []),
**lidar_stats,
"point_columns": [
{"name": "d_mm", "dtype": "float32", "unit": "mm"},
{"name": "azimuth_deg", "dtype": "float32", "unit": "degree"},
{"name": "altitude_deg", "dtype": "float32", "unit": "degree"},
{"name": "intensity", "dtype": "float32", "unit": "device-specific"},
{"name": "progression", "dtype": "float32", "unit": "scan fraction"},
],
"extrinsic": {
"translation_mm": list(translation),
"yaw_pitch_roll_deg": [args.yaw, args.pitch, args.roll],
"rotation_row_major": list(rotation),
},
"validation_ok": validation["ok"],
"duration_seconds": round(time.time() - started, 3),
}
if write_reports:
with (reports_dir / "metadata.json").open("w", encoding="utf-8") as stream:
json.dump(metadata, stream, ensure_ascii=False, indent=2)
stream.write("\n")
with (reports_dir / "validation_report.json").open("w", encoding="utf-8") as stream:
json.dump(validation, stream, ensure_ascii=False, indent=2)
stream.write("\n")
print(json.dumps(metadata, ensure_ascii=False, indent=2), flush=True)
print(
json.dumps(
{"validation_ok": validation["ok"], "checks": validation["checks"]},
ensure_ascii=False,
indent=2,
),
flush=True,
)
if skipped != 0:
return 1
if include_rtk and not validation["ok"]:
return 1
return 0
def main() -> int:
try:
return export_dataset(parse_args())
except Exception as exc:
print(f"ERROR: {type(exc).__name__}: {exc}", file=sys.stderr)
return 2
if __name__ == "__main__":
raise SystemExit(main())
Reference in New Issue Copy Permalink