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