新增雷达到RTK直接手眼标定流程
This commit is contained in:
lichun.qu
@@ -0,0 +1,11 @@
|
||||
# tools目录
|
||||
|
||||
| 文件 | 输入→输出 |
|
||||
|---|---|
|
||||
| `frontlidar_dlog_export.py` | LiDAR dlog → 逐帧原始点云NPZ；时间来自DObject post tick |
|
||||
| `rscap_v2/parse_rtk_imu_v2.py` | RTK/IMU rscap → JSONL，保存校验状态、主机时间、GNSS/IMU设备字段和原始报文 |
|
||||
| `rscap_v2/audit_capture_v2.py` | 检查rscap结构、时间范围和记录统计 |
|
||||
| `build_multisensor_npz.py` | 按LiDAR帧最近邻关联GGA/heading，并附加IMU时间窗 → combined NPZ |
|
||||
| `prepare_multisensor_station_dataset.py` | combined NPZ → 每站一帧`frames_all`和`reference_poses_*.csv` |
|
||||
|
||||
当前标定只使用LiDAR和RTK；IMU保持原始传感器坐标，不参与点云去畸变或外参求解。prepared阶段对站内有效RTK取平均、对heading取圆均值，并选择有效帧序列的中间LiDAR帧。
|
||||
@@ -0,0 +1,312 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Build one LiDAR-centric NPZ per frame with matched RTK and an IMU window.
|
||||
|
||||
Inputs are LiDAR frame NPZ files from frontlidar_dlog_export.py and parsed
|
||||
RTK/IMU JSONL files from parse_rtk_imu_v2.py. Raw .rscap files remain the
|
||||
traceability source; this script never modifies them.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
|
||||
|
||||
|
||||
def parse_named_path(text: str) -> tuple[str, Path]:
|
||||
if "=" not in text:
|
||||
raise argparse.ArgumentTypeError("expected NAME=PATH")
|
||||
name, raw_path = text.split("=", 1)
|
||||
if not name.strip():
|
||||
raise argparse.ArgumentTypeError("segment name is empty")
|
||||
return name.strip(), Path(raw_path)
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument(
|
||||
"--lidar",
|
||||
type=parse_named_path,
|
||||
action="append",
|
||||
required=True,
|
||||
metavar="NAME=FRAMES_DIR",
|
||||
help="Repeat for each LiDAR segment; directory contains exported *.npz frames.",
|
||||
)
|
||||
parser.add_argument("--rtk", type=Path, action="append", required=True, help="Parsed rtk.jsonl; repeat per session.")
|
||||
parser.add_argument("--imu", type=Path, action="append", required=True, help="Parsed imu.jsonl; repeat per session.")
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
|
||||
parser.add_argument("--imu-before-ms", type=float, default=100.0)
|
||||
parser.add_argument("--imu-after-ms", type=float, default=100.0)
|
||||
parser.add_argument("--gps-utc-leap-seconds", type=int, default=18)
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def load_jsonl(paths: list[Path]) -> list[dict[str, Any]]:
|
||||
rows: list[dict[str, Any]] = []
|
||||
for source_index, path in enumerate(paths):
|
||||
source_file = str(path.resolve())
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
for line_number, line in enumerate(stream, start=1):
|
||||
if not line.strip():
|
||||
continue
|
||||
row = json.loads(line)
|
||||
row["_source_file"] = source_file
|
||||
row["_source_index"] = source_index
|
||||
row["_source_line"] = line_number
|
||||
rows.append(row)
|
||||
return rows
|
||||
|
||||
|
||||
def utf8_array(value: Any) -> np.ndarray:
|
||||
return np.frombuffer(str(value if value is not None else "").encode("utf-8"), dtype=np.uint8)
|
||||
|
||||
|
||||
def scalar(array: np.ndarray) -> Any:
|
||||
return array.reshape(-1)[0].item()
|
||||
|
||||
|
||||
def nearest_index(times: np.ndarray, target: int) -> int:
|
||||
if not len(times):
|
||||
return -1
|
||||
right = int(np.searchsorted(times, target, side="left"))
|
||||
candidates = [index for index in (right - 1, right) if 0 <= index < len(times)]
|
||||
return min(candidates, key=lambda index: abs(int(times[index]) - target))
|
||||
|
||||
|
||||
def estimate_imu_times(rows: list[dict[str, Any]]) -> list[dict[str, Any]]:
|
||||
"""Recover 100 Hz timing inside each serial chunk from device timestamps.
|
||||
|
||||
A capture chunk has one host receive timestamp but may contain several IMU
|
||||
frames. The last frame is anchored to the chunk receive time and earlier
|
||||
frames are moved backwards by their device timestamp difference.
|
||||
"""
|
||||
groups: dict[tuple[int, int], list[dict[str, Any]]] = {}
|
||||
for row in rows:
|
||||
if not row.get("crc_valid") or row.get("device_timestamp_ms") is None:
|
||||
continue
|
||||
key = (int(row["_source_index"]), int(row.get("source_chunk_sequence_last", -1)))
|
||||
groups.setdefault(key, []).append(row)
|
||||
result: list[dict[str, Any]] = []
|
||||
for group in groups.values():
|
||||
group.sort(key=lambda row: (int(row["device_timestamp_ms"]), int(row["_source_line"])))
|
||||
last_device = int(group[-1]["device_timestamp_ms"])
|
||||
host_ns = int(group[-1]["host_receive_utc_ns"])
|
||||
for row in group:
|
||||
delta_ms = (last_device - int(row["device_timestamp_ms"])) & 0xFFFFFFFF
|
||||
if delta_ms > 60_000:
|
||||
delta_ms = 0
|
||||
copied = dict(row)
|
||||
copied["estimated_time_ns"] = host_ns - delta_ms * 1_000_000
|
||||
result.append(copied)
|
||||
result.sort(key=lambda row: int(row["estimated_time_ns"]))
|
||||
return result
|
||||
|
||||
|
||||
def gnss_utc_ns(row: dict[str, Any], leap_seconds: int) -> int | None:
|
||||
week, tow_ms = row.get("gnss_week"), row.get("gnss_tow_ms")
|
||||
if week is None or tow_ms is None:
|
||||
return None
|
||||
seconds = int(week) * 604800 + float(tow_ms) / 1000.0 - leap_seconds
|
||||
return GPS_EPOCH_UNIX_NS + int(round(seconds * 1_000_000_000))
|
||||
|
||||
|
||||
def numeric_array(rows: list[dict[str, Any]], key: str, dtype: Any, default: Any) -> np.ndarray:
|
||||
return np.asarray([row.get(key, default) if row.get(key) is not None else default for row in rows], dtype=dtype)
|
||||
|
||||
|
||||
def raw_frame_matrix(rows: list[dict[str, Any]]) -> tuple[np.ndarray, np.ndarray]:
|
||||
frames = [bytes.fromhex(str(row.get("raw_frame_hex", ""))) for row in rows]
|
||||
lengths = np.asarray([len(frame) for frame in frames], dtype=np.int32)
|
||||
width = max(lengths, default=0)
|
||||
matrix = np.zeros((len(frames), width), dtype=np.uint8)
|
||||
for index, frame in enumerate(frames):
|
||||
matrix[index, : len(frame)] = np.frombuffer(frame, dtype=np.uint8)
|
||||
return matrix, lengths
|
||||
|
||||
|
||||
def add_rtk(values: dict[str, np.ndarray], prefix: str, row: dict[str, Any] | None, dt_ns: int | None) -> None:
|
||||
values[f"{prefix}_valid"] = np.asarray([row is not None], dtype=np.uint8)
|
||||
values[f"{prefix}_dt_ns"] = np.asarray([dt_ns or 0], dtype=np.int64)
|
||||
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([0], dtype=np.int64)
|
||||
values[f"{prefix}_raw_utf8"] = utf8_array("")
|
||||
values[f"{prefix}_source_file_utf8"] = utf8_array("")
|
||||
values[f"{prefix}_source_raw_file_offset"] = np.asarray([-1], dtype=np.int64)
|
||||
values[f"{prefix}_source_raw_byte_length"] = np.asarray([0], dtype=np.int32)
|
||||
if row is None:
|
||||
return
|
||||
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([row.get("host_receive_utc_ns", 0)], dtype=np.int64)
|
||||
values[f"{prefix}_raw_utf8"] = utf8_array(row.get("raw_line", ""))
|
||||
values[f"{prefix}_source_file_utf8"] = utf8_array(row.get("_source_file", ""))
|
||||
values[f"{prefix}_source_raw_file_offset"] = np.asarray([row.get("source_raw_file_offset", -1)], dtype=np.int64)
|
||||
values[f"{prefix}_source_raw_byte_length"] = np.asarray([row.get("source_raw_byte_length", 0)], dtype=np.int32)
|
||||
|
||||
|
||||
def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
|
||||
for key, dtype, default in (
|
||||
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
|
||||
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
|
||||
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
|
||||
("differential_age_s", np.float64, np.nan),
|
||||
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
|
||||
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
|
||||
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
|
||||
("pitch_stddev_deg", np.float64, np.nan), ("heading_satellites", np.int32, -1),
|
||||
("solution_satellites", np.int32, -1),
|
||||
):
|
||||
values[f"rtk_{key}"] = np.asarray([default], dtype=dtype)
|
||||
values["rtk_fixed"] = np.asarray([0], dtype=np.uint8)
|
||||
values["rtk_heading_solution_utf8"] = utf8_array("")
|
||||
values["rtk_heading_gnss_utc_ns"] = np.asarray([0], dtype=np.int64)
|
||||
values["rtk_heading_host_minus_gnss_ns"] = np.asarray([0], dtype=np.int64)
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
if args.out.exists() and any(args.out.iterdir()) and not args.overwrite:
|
||||
raise FileExistsError(f"{args.out} is non-empty; pass --overwrite")
|
||||
frames_out = args.out / "frames"
|
||||
frames_out.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
rtk_rows = load_jsonl(args.rtk)
|
||||
gga = sorted(
|
||||
[row for row in rtk_rows if row.get("type") == "GGA" and row.get("checksum_valid") and row.get("lat_deg") is not None],
|
||||
key=lambda row: int(row["host_receive_utc_ns"]),
|
||||
)
|
||||
heading = sorted(
|
||||
[row for row in rtk_rows if row.get("type") == "UNIHEADINGA" and row.get("checksum_valid") and row.get("heading_valid")],
|
||||
key=lambda row: int(row["host_receive_utc_ns"]),
|
||||
)
|
||||
imu = estimate_imu_times(load_jsonl(args.imu))
|
||||
gga_times = np.asarray([int(row["host_receive_utc_ns"]) for row in gga], dtype=np.int64)
|
||||
heading_times = np.asarray([int(row["host_receive_utc_ns"]) for row in heading], dtype=np.int64)
|
||||
imu_times = np.asarray([int(row["estimated_time_ns"]) for row in imu], dtype=np.int64)
|
||||
|
||||
manifest: list[dict[str, Any]] = []
|
||||
global_index = 0
|
||||
max_rtk_ns = int(args.rtk_max_dt_ms * 1_000_000)
|
||||
before_ns = int(args.imu_before_ms * 1_000_000)
|
||||
after_ns = int(args.imu_after_ms * 1_000_000)
|
||||
|
||||
for segment_name, frame_dir in args.lidar:
|
||||
frame_paths = sorted(frame_dir.glob("*.npz"))
|
||||
if not frame_paths:
|
||||
raise FileNotFoundError(f"no NPZ frames under {frame_dir}")
|
||||
for segment_index, source in enumerate(frame_paths):
|
||||
with np.load(source, allow_pickle=False) as frame:
|
||||
values = {key: np.asarray(frame[key]) for key in frame.files}
|
||||
lidar_time_ns = int(scalar(values["unix_time_ns"]))
|
||||
|
||||
gga_index = nearest_index(gga_times, lidar_time_ns)
|
||||
heading_index = nearest_index(heading_times, lidar_time_ns)
|
||||
gga_row = gga[gga_index] if gga_index >= 0 else None
|
||||
heading_row = heading[heading_index] if heading_index >= 0 else None
|
||||
gga_dt = int(gga_times[gga_index]) - lidar_time_ns if gga_index >= 0 else None
|
||||
heading_dt = int(heading_times[heading_index]) - lidar_time_ns if heading_index >= 0 else None
|
||||
gga_ok = gga_row is not None and abs(gga_dt or 0) <= max_rtk_ns
|
||||
heading_ok = heading_row is not None and abs(heading_dt or 0) <= max_rtk_ns
|
||||
add_rtk(values, "rtk_gga", gga_row if gga_ok else None, gga_dt)
|
||||
add_rtk(values, "rtk_heading", heading_row if heading_ok else None, heading_dt)
|
||||
initialize_rtk_measurements(values)
|
||||
|
||||
if gga_ok and gga_row:
|
||||
for key, dtype, default in (
|
||||
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
|
||||
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
|
||||
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
|
||||
("differential_age_s", np.float64, np.nan),
|
||||
):
|
||||
values[f"rtk_{key}"] = np.asarray([gga_row.get(key, default)], dtype=dtype)
|
||||
values["rtk_gga_satellites"] = np.asarray([gga_row.get("satellites", -1)], dtype=np.int32)
|
||||
values["rtk_fixed"] = np.asarray([int(gga_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8)
|
||||
if heading_ok and heading_row:
|
||||
for key, dtype, default in (
|
||||
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
|
||||
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
|
||||
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
|
||||
("pitch_stddev_deg", np.float64, np.nan),
|
||||
("solution_satellites", np.int32, -1),
|
||||
):
|
||||
values[f"rtk_{key}"] = np.asarray([heading_row.get(key, default)], dtype=dtype)
|
||||
values["rtk_heading_satellites"] = np.asarray([heading_row.get("satellites", -1)], dtype=np.int32)
|
||||
values["rtk_heading_solution_utf8"] = utf8_array(heading_row.get("heading_solution", ""))
|
||||
device_ns = gnss_utc_ns(heading_row, args.gps_utc_leap_seconds)
|
||||
values["rtk_heading_gnss_utc_ns"] = np.asarray([device_ns or 0], dtype=np.int64)
|
||||
values["rtk_heading_host_minus_gnss_ns"] = np.asarray(
|
||||
[int(heading_row["host_receive_utc_ns"]) - device_ns if device_ns is not None else 0], dtype=np.int64
|
||||
)
|
||||
|
||||
left = int(np.searchsorted(imu_times, lidar_time_ns - before_ns, side="left"))
|
||||
right = int(np.searchsorted(imu_times, lidar_time_ns + after_ns, side="right"))
|
||||
window = imu[left:right]
|
||||
values["imu_window_count"] = np.asarray([len(window)], dtype=np.int32)
|
||||
values["imu_valid"] = np.asarray([bool(window)], dtype=np.uint8)
|
||||
values["imu_time_ns"] = numeric_array(window, "estimated_time_ns", np.int64, 0)
|
||||
values["imu_host_receive_utc_ns"] = numeric_array(window, "host_receive_utc_ns", np.int64, 0)
|
||||
for key in ("device_timestamp_ms", "pps_sync_stamp_ms", "tag"):
|
||||
values[f"imu_{key}"] = numeric_array(window, key, np.int64, -1)
|
||||
for key in (
|
||||
"temperature_c", "air_pressure_pa", "accel_x_mps2", "accel_y_mps2", "accel_z_mps2",
|
||||
"gyro_x_radps", "gyro_y_radps", "gyro_z_radps", "mag_x_ut", "mag_y_ut", "mag_z_ut",
|
||||
"roll_deg", "pitch_deg", "yaw_deg", "quaternion_w", "quaternion_x", "quaternion_y", "quaternion_z",
|
||||
):
|
||||
values[f"imu_{key}"] = numeric_array(window, key, np.float64, np.nan)
|
||||
values["imu_source_index"] = numeric_array(window, "_source_index", np.int32, -1)
|
||||
values["imu_source_raw_file_offset"] = numeric_array(window, "source_raw_file_offset", np.int64, -1)
|
||||
raw_matrix, raw_lengths = raw_frame_matrix(window)
|
||||
values["imu_raw_frame_bytes"] = raw_matrix
|
||||
values["imu_raw_frame_length"] = raw_lengths
|
||||
values["imu_source_files_json_utf8"] = utf8_array(json.dumps([str(path.resolve()) for path in args.imu], ensure_ascii=False))
|
||||
values["source_lidar_file_utf8"] = utf8_array(source.resolve())
|
||||
values["segment_name_utf8"] = utf8_array(segment_name)
|
||||
|
||||
output = frames_out / f"{segment_name}_{segment_index:06d}.npz"
|
||||
np.savez_compressed(output, **values)
|
||||
manifest.append({
|
||||
"global_index": global_index,
|
||||
"segment": segment_name,
|
||||
"segment_index": segment_index,
|
||||
"output": str(output.relative_to(args.out)),
|
||||
"source_lidar": str(source.resolve()),
|
||||
"lidar_time_ns": lidar_time_ns,
|
||||
"rtk_gga_dt_ns": gga_dt,
|
||||
"rtk_heading_dt_ns": heading_dt,
|
||||
"rtk_valid": gga_ok,
|
||||
"heading_valid": heading_ok,
|
||||
"rtk_fix_quality": gga_row.get("fix_quality") if gga_ok and gga_row else None,
|
||||
"rtk_fixed": bool(gga_ok and gga_row and int(gga_row.get("fix_quality", -1)) in {4, 5}),
|
||||
"imu_window_count": len(window),
|
||||
})
|
||||
global_index += 1
|
||||
|
||||
fields = sorted({key for row in manifest for key in row})
|
||||
with (args.out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=fields)
|
||||
writer.writeheader()
|
||||
writer.writerows(manifest)
|
||||
summary = {
|
||||
"frames": len(manifest),
|
||||
"segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in args.lidar},
|
||||
"rtk_valid": sum(bool(row["rtk_valid"]) for row in manifest),
|
||||
"heading_valid": sum(bool(row["heading_valid"]) for row in manifest),
|
||||
"rtk_fixed": sum(bool(row["rtk_fixed"]) for row in manifest),
|
||||
"imu_window_nonempty": sum(int(row["imu_window_count"]) > 0 for row in manifest),
|
||||
"rtk_max_dt_ms": args.rtk_max_dt_ms,
|
||||
"imu_window_ms": [-args.imu_before_ms, args.imu_after_ms],
|
||||
"time_basis": "LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement",
|
||||
"imu_orientation_warning": "IMU values are in the raw IMU sensor frame; no LiDAR/body extrinsic is applied",
|
||||
}
|
||||
(args.out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps(summary, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,170 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Prepare one static LiDAR frame and one yaw-only RTK reference pose per NPZ segment."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import re
|
||||
import shutil
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"]
|
||||
|
||||
|
||||
def natural_key(value: str) -> list[Any]:
|
||||
return [int(part) if part.isdigit() else part.lower() for part in re.split(r"(\d+)", value)]
|
||||
|
||||
|
||||
def truth(value: Any) -> bool:
|
||||
return str(value).strip().lower() in {"1", "true", "yes", "y"}
|
||||
|
||||
|
||||
def circular_mean_deg(values: np.ndarray) -> float:
|
||||
radians = np.deg2rad(values)
|
||||
return float(np.rad2deg(math.atan2(np.mean(np.sin(radians)), np.mean(np.cos(radians)))) % 360.0)
|
||||
|
||||
|
||||
def circular_std_deg(values: np.ndarray) -> float:
|
||||
radians = np.deg2rad(values)
|
||||
resultant = max(math.hypot(np.mean(np.cos(radians)), np.mean(np.sin(radians))), 1e-12)
|
||||
return float(np.rad2deg(math.sqrt(-2.0 * math.log(resultant))))
|
||||
|
||||
|
||||
def geodetic_to_ecef(lat_deg: float, lon_deg: float, height_m: float) -> np.ndarray:
|
||||
a, e2 = 6378137.0, 6.69437999014e-3
|
||||
lat, lon = math.radians(lat_deg), math.radians(lon_deg)
|
||||
sin_lat, cos_lat, sin_lon, cos_lon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
|
||||
n = a / math.sqrt(1.0 - e2 * sin_lat * sin_lat)
|
||||
return np.array([(n + height_m) * cos_lat * cos_lon, (n + height_m) * cos_lat * sin_lon,
|
||||
(n * (1.0 - e2) + height_m) * sin_lat], dtype=float)
|
||||
|
||||
|
||||
def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: float) -> np.ndarray:
|
||||
lat, lon = math.radians(lat_deg), math.radians(lon_deg)
|
||||
slat, clat, slon, clon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
|
||||
rotation = np.array([[-slon, clon, 0.0], [-slat * clon, -slat * slon, clat],
|
||||
[clat * clon, clat * slon, slat]], dtype=float)
|
||||
return rotation @ (ecef - origin)
|
||||
|
||||
|
||||
def yaw_rotation(yaw: float) -> np.ndarray:
|
||||
c, s = math.cos(yaw), math.sin(yaw)
|
||||
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
|
||||
|
||||
|
||||
def scalar(data: np.lib.npyio.NpzFile, name: str) -> float:
|
||||
return float(np.asarray(data[name]).reshape(-1)[0])
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--combined-root", type=Path, required=True)
|
||||
parser.add_argument("--output", type=Path, required=True)
|
||||
parser.add_argument("--pose-name", default="rtk_gga_raw_heading")
|
||||
parser.add_argument("--heading-offset-deg", type=float, required=True)
|
||||
parser.add_argument("--antenna-lever", type=float, nargs=3, required=True, metavar=("X", "Y", "Z"))
|
||||
parser.add_argument("--accepted-fixes", type=int, nargs="+", default=[4, 5])
|
||||
parser.add_argument("--heading-std-limit-deg", type=float, default=0.5)
|
||||
parser.add_argument("--min-stations", type=int, default=30)
|
||||
parser.add_argument("--expected-stations", type=int, default=0)
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
def main() -> int:
|
||||
args = parse_args()
|
||||
manifest_path = args.combined_root / "manifest.csv"
|
||||
with manifest_path.open("r", encoding="utf-8-sig", newline="") as stream:
|
||||
rows = list(csv.DictReader(stream))
|
||||
required = {"segment", "output", "lidar_time_ns", "rtk_valid", "heading_valid", "rtk_fix_quality"}
|
||||
if not rows or not required.issubset(rows[0]):
|
||||
raise ValueError(f"{manifest_path} is empty or lacks {sorted(required)}")
|
||||
groups: dict[str, list[dict[str, str]]] = {}
|
||||
for row in rows:
|
||||
groups.setdefault(row["segment"], []).append(row)
|
||||
|
||||
selected, summaries, rejected = [], [], []
|
||||
accepted_fixes = set(args.accepted_fixes)
|
||||
for segment in sorted(groups, key=natural_key):
|
||||
group = sorted(groups[segment], key=lambda row: int(row["lidar_time_ns"]))
|
||||
good = [row for row in group if truth(row["rtk_valid"]) and truth(row["heading_valid"])
|
||||
and int(row["rtk_fix_quality"]) in accepted_fixes]
|
||||
if not good:
|
||||
rejected.append({"station": segment, "reason": "no associated fixed RTK position and valid heading"})
|
||||
continue
|
||||
samples = []
|
||||
for row in good:
|
||||
path = args.combined_root / Path(row["output"])
|
||||
with np.load(path, allow_pickle=False) as data:
|
||||
samples.append((scalar(data, "rtk_lat_deg"), scalar(data, "rtk_lon_deg"),
|
||||
scalar(data, "rtk_altitude_m"), scalar(data, "rtk_raw_heading_deg"),
|
||||
scalar(data, "rtk_pitch_deg"), scalar(data, "rtk_heading_stddev_deg")))
|
||||
values = np.asarray(samples, dtype=float)
|
||||
heading_std = circular_std_deg(values[:, 3])
|
||||
if heading_std > args.heading_std_limit_deg:
|
||||
rejected.append({"station": segment, "reason": f"heading std {heading_std:.4f} deg exceeds limit"})
|
||||
continue
|
||||
frame = good[len(good) // 2]
|
||||
source = args.combined_root / Path(frame["output"])
|
||||
selected.append({"station": segment, "source": source, "time": int(frame["lidar_time_ns"]) / 1e9,
|
||||
"lat": float(np.mean(values[:, 0])), "lon": float(np.mean(values[:, 1])),
|
||||
"alt": float(np.mean(values[:, 2])), "heading": circular_mean_deg(values[:, 3])})
|
||||
summaries.append({"station": segment, "frames": len(group), "valid_fixed_frames": len(good),
|
||||
"heading_mean_deg": circular_mean_deg(values[:, 3]),
|
||||
"heading_circular_std_deg": heading_std, "rtk_pitch_mean_deg": float(np.mean(values[:, 4])),
|
||||
"reported_heading_std_mean_deg": float(np.nanmean(values[:, 5])),
|
||||
"altitude_std_m": float(np.std(values[:, 2])), "selected_source": str(source)})
|
||||
|
||||
if args.expected_stations and len(selected) != args.expected_stations:
|
||||
raise RuntimeError(f"expected {args.expected_stations} usable stations, got {len(selected)}; rejected={rejected}")
|
||||
if len(selected) < args.min_stations:
|
||||
raise RuntimeError(f"need at least {args.min_stations} usable stations, got {len(selected)}; rejected={rejected}")
|
||||
if args.output.exists() and any(args.output.iterdir()) and not args.overwrite:
|
||||
raise FileExistsError(f"{args.output} is non-empty; pass --overwrite")
|
||||
frames = args.output / "frames_all"
|
||||
frames.mkdir(parents=True, exist_ok=True)
|
||||
origin = selected[0]
|
||||
origin_ecef = geodetic_to_ecef(origin["lat"], origin["lon"], origin["alt"])
|
||||
lever = np.asarray(args.antenna_lever, dtype=float)
|
||||
pose_rows = []
|
||||
for index, item in enumerate(selected, 1):
|
||||
destination = frames / f"station_{index:02d}.npz"
|
||||
shutil.copy2(item["source"], destination)
|
||||
antenna = ecef_to_enu(geodetic_to_ecef(item["lat"], item["lon"], item["alt"]), origin_ecef,
|
||||
origin["lat"], origin["lon"])
|
||||
corrected_heading = (item["heading"] + args.heading_offset_deg) % 360.0
|
||||
yaw = math.radians(90.0 - corrected_heading)
|
||||
reference_position = antenna - yaw_rotation(yaw) @ lever
|
||||
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *reference_position, 0.0, 0.0,
|
||||
math.sin(yaw / 2.0), math.cos(yaw / 2.0)])))
|
||||
summaries[index - 1].update({"sequence": index, "prepared_frame": destination.name,
|
||||
"corrected_heading_deg": corrected_heading})
|
||||
pose_path = args.output / f"reference_poses_{args.pose_name}.csv"
|
||||
with pose_path.open("w", encoding="utf-8", newline="") as stream:
|
||||
writer = csv.DictWriter(stream, fieldnames=POSE_FIELDS); writer.writeheader(); writer.writerows(pose_rows)
|
||||
with (args.output / "station_summary.csv").open("w", encoding="utf-8", newline="") as stream:
|
||||
fields = sorted({key for row in summaries for key in row})
|
||||
writer = csv.DictWriter(stream, fieldnames=fields); writer.writeheader(); writer.writerows(summaries)
|
||||
document = {"source_combined_root": str(args.combined_root.resolve()), "station_count": len(selected),
|
||||
"rejected": rejected, "pose_csv": pose_path.name,
|
||||
"selection_policy": "middle LiDAR frame among fixed-position and valid-heading associations",
|
||||
"reference_pose_configuration": {"raw_heading_offset_deg": args.heading_offset_deg,
|
||||
"antenna_lever_body_m": args.antenna_lever,
|
||||
"orientation_model": "yaw-only, identical to the previous calibration workflow"},
|
||||
"stations": [{"sequence": i + 1, "source_station": item["station"],
|
||||
"source_frame": str(item["source"]), "prepared_frame": f"station_{i + 1:02d}.npz"}
|
||||
for i, item in enumerate(selected)]}
|
||||
(args.output / "manifest.json").write_text(json.dumps(document, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
print(json.dumps({"prepared": str(args.output.resolve()), "stations": len(selected),
|
||||
"rejected": rejected, "pose_csv": pose_path.name}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,22 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from capture_format_v2 import file_summary, read_capture
|
||||
from pipeline_common import write_json
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("captures", nargs="+", type=Path)
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
summaries = [file_summary(read_capture(path)) for path in args.captures]
|
||||
write_json(args.out, {"captures": summaries})
|
||||
for summary in summaries:
|
||||
print(summary)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,254 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import binascii
|
||||
import io
|
||||
import struct
|
||||
from dataclasses import dataclass, asdict
|
||||
from pathlib import Path
|
||||
from typing import BinaryIO, Iterator
|
||||
|
||||
|
||||
FILE_MAGIC = "RAW_SERIAL_CAPTURE_FILE_V2"
|
||||
RECORD_MAGIC = "RAW_SERIAL_RECORD_V2"
|
||||
FOOTER_MAGIC = "RAW_SERIAL_CAPTURE_FOOTER_V2"
|
||||
|
||||
|
||||
def read_7bit_int(stream: BinaryIO) -> int:
|
||||
value = 0
|
||||
shift = 0
|
||||
while True:
|
||||
raw = stream.read(1)
|
||||
if not raw:
|
||||
raise EOFError("truncated .NET string length")
|
||||
value |= (raw[0] & 0x7F) << shift
|
||||
if not raw[0] & 0x80:
|
||||
return value
|
||||
shift += 7
|
||||
if shift > 35:
|
||||
raise ValueError("invalid .NET string length")
|
||||
|
||||
|
||||
def read_dotnet_string(stream: BinaryIO) -> str:
|
||||
length = read_7bit_int(stream)
|
||||
raw = stream.read(length)
|
||||
if len(raw) != length:
|
||||
raise EOFError("truncated .NET string")
|
||||
return raw.decode("utf-8")
|
||||
|
||||
|
||||
def read_i32(stream: BinaryIO) -> int:
|
||||
raw = stream.read(4)
|
||||
if len(raw) != 4:
|
||||
raise EOFError("truncated int32")
|
||||
return struct.unpack("<i", raw)[0]
|
||||
|
||||
|
||||
def read_i64(stream: BinaryIO) -> int:
|
||||
raw = stream.read(8)
|
||||
if len(raw) != 8:
|
||||
raise EOFError("truncated int64")
|
||||
return struct.unpack("<q", raw)[0]
|
||||
|
||||
|
||||
def read_u32(stream: BinaryIO) -> int:
|
||||
raw = stream.read(4)
|
||||
if len(raw) != 4:
|
||||
raise EOFError("truncated uint32")
|
||||
return struct.unpack("<I", raw)[0]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CaptureHeader:
|
||||
sensor_kind: str
|
||||
session_id: str
|
||||
session_start_utc_ticks: int
|
||||
session_start_monotonic_ticks: int
|
||||
monotonic_frequency: int
|
||||
port: str
|
||||
baud: int
|
||||
file_start_utc_ticks: int
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RawChunk:
|
||||
sequence: int
|
||||
receive_utc_ticks: int
|
||||
receive_monotonic_ticks: int
|
||||
raw: bytes
|
||||
record_file_offset: int
|
||||
raw_file_offset: int
|
||||
record_crc32: int
|
||||
crc_valid: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CaptureFooter:
|
||||
clean_close: bool
|
||||
records: int
|
||||
bytes: int
|
||||
first_sequence: int
|
||||
last_sequence: int
|
||||
dropped_chunks: int
|
||||
dropped_bytes: int
|
||||
crc_valid: bool
|
||||
|
||||
|
||||
@dataclass
|
||||
class CaptureFile:
|
||||
path: str
|
||||
header: CaptureHeader
|
||||
chunks: list[RawChunk]
|
||||
footer: CaptureFooter | None
|
||||
truncated_tail: bool = False
|
||||
|
||||
|
||||
def read_header(stream: BinaryIO) -> CaptureHeader:
|
||||
if read_dotnet_string(stream) != FILE_MAGIC:
|
||||
raise ValueError("not a V2 raw capture file")
|
||||
version = read_i32(stream)
|
||||
if version != 2:
|
||||
raise ValueError(f"unsupported capture version: {version}")
|
||||
return CaptureHeader(
|
||||
sensor_kind=read_dotnet_string(stream),
|
||||
session_id=read_dotnet_string(stream),
|
||||
session_start_utc_ticks=read_i64(stream),
|
||||
session_start_monotonic_ticks=read_i64(stream),
|
||||
monotonic_frequency=read_i64(stream),
|
||||
port=read_dotnet_string(stream),
|
||||
baud=read_i32(stream),
|
||||
file_start_utc_ticks=read_i64(stream),
|
||||
)
|
||||
|
||||
|
||||
def parse_record_body(body: bytes, record_file_offset: int, record_crc: int) -> RawChunk:
|
||||
stream = io.BytesIO(body)
|
||||
if read_dotnet_string(stream) != RECORD_MAGIC:
|
||||
raise ValueError("invalid record magic")
|
||||
sequence = read_i64(stream)
|
||||
receive_utc_ticks = read_i64(stream)
|
||||
receive_monotonic_ticks = read_i64(stream)
|
||||
raw_length = read_i32(stream)
|
||||
if raw_length < 0 or raw_length > 64 * 1024 * 1024:
|
||||
raise ValueError(f"invalid raw length: {raw_length}")
|
||||
raw_offset = record_file_offset + 4 + stream.tell()
|
||||
raw = stream.read(raw_length)
|
||||
if len(raw) != raw_length:
|
||||
raise EOFError("truncated raw bytes")
|
||||
crc_valid = (binascii.crc32(body) & 0xFFFFFFFF) == record_crc
|
||||
return RawChunk(
|
||||
sequence=sequence,
|
||||
receive_utc_ticks=receive_utc_ticks,
|
||||
receive_monotonic_ticks=receive_monotonic_ticks,
|
||||
raw=raw,
|
||||
record_file_offset=record_file_offset,
|
||||
raw_file_offset=raw_offset,
|
||||
record_crc32=record_crc,
|
||||
crc_valid=crc_valid,
|
||||
)
|
||||
|
||||
|
||||
def parse_footer(body: bytes, expected_crc: int) -> CaptureFooter:
|
||||
stream = io.BytesIO(body)
|
||||
if read_dotnet_string(stream) != FOOTER_MAGIC:
|
||||
raise ValueError("invalid footer magic")
|
||||
clean_close = stream.read(1) == b"\x01"
|
||||
records = read_i64(stream)
|
||||
raw_bytes = read_i64(stream)
|
||||
first_sequence = read_i64(stream)
|
||||
last_sequence = read_i64(stream)
|
||||
dropped_chunks = read_i64(stream)
|
||||
dropped_bytes = read_i64(stream)
|
||||
return CaptureFooter(
|
||||
clean_close=clean_close,
|
||||
records=records,
|
||||
bytes=raw_bytes,
|
||||
first_sequence=first_sequence,
|
||||
last_sequence=last_sequence,
|
||||
dropped_chunks=dropped_chunks,
|
||||
dropped_bytes=dropped_bytes,
|
||||
crc_valid=(binascii.crc32(body) & 0xFFFFFFFF) == expected_crc,
|
||||
)
|
||||
|
||||
|
||||
def read_capture(path: Path) -> CaptureFile:
|
||||
chunks: list[RawChunk] = []
|
||||
footer = None
|
||||
truncated = False
|
||||
with path.open("rb") as stream:
|
||||
header = read_header(stream)
|
||||
while True:
|
||||
record_offset = stream.tell()
|
||||
length_raw = stream.read(4)
|
||||
if not length_raw:
|
||||
break
|
||||
if len(length_raw) != 4:
|
||||
truncated = True
|
||||
break
|
||||
length = struct.unpack("<i", length_raw)[0]
|
||||
try:
|
||||
if length == -1:
|
||||
footer_length = read_i32(stream)
|
||||
if footer_length < 0 or footer_length > 1024 * 1024:
|
||||
raise ValueError("invalid footer length")
|
||||
footer_body = stream.read(footer_length)
|
||||
if len(footer_body) != footer_length:
|
||||
raise EOFError("truncated footer")
|
||||
footer = parse_footer(footer_body, read_u32(stream))
|
||||
break
|
||||
if length <= 0 or length > 64 * 1024 * 1024:
|
||||
raise ValueError("invalid record length")
|
||||
body = stream.read(length)
|
||||
if len(body) != length:
|
||||
raise EOFError("truncated record body")
|
||||
record_crc = read_u32(stream)
|
||||
chunks.append(parse_record_body(body, record_offset, record_crc))
|
||||
except (EOFError, ValueError):
|
||||
truncated = True
|
||||
break
|
||||
return CaptureFile(str(path), header, chunks, footer, truncated)
|
||||
|
||||
|
||||
def sequence_gaps(chunks: list[RawChunk]) -> list[tuple[int, int, int]]:
|
||||
result = []
|
||||
for previous, current in zip(chunks, chunks[1:]):
|
||||
if current.sequence > previous.sequence + 1:
|
||||
result.append((previous.sequence, current.sequence, current.sequence - previous.sequence - 1))
|
||||
return result
|
||||
|
||||
|
||||
def file_summary(capture: CaptureFile) -> dict:
|
||||
gaps = sequence_gaps(capture.chunks)
|
||||
sequences = [chunk.sequence for chunk in capture.chunks]
|
||||
return {
|
||||
"path": capture.path,
|
||||
"sensor": capture.header.sensor_kind,
|
||||
"session_id": capture.header.session_id,
|
||||
"port": capture.header.port,
|
||||
"baud": capture.header.baud,
|
||||
"chunks_read": len(capture.chunks),
|
||||
"bytes_read": sum(len(chunk.raw) for chunk in capture.chunks),
|
||||
"first_sequence": sequences[0] if sequences else None,
|
||||
"last_sequence": sequences[-1] if sequences else None,
|
||||
"missing_chunks": sum(gap[2] for gap in gaps),
|
||||
"gap_count": len(gaps),
|
||||
"bad_record_crc": sum(not chunk.crc_valid for chunk in capture.chunks),
|
||||
"truncated_tail": capture.truncated_tail,
|
||||
"footer": None if capture.footer is None else asdict(capture.footer),
|
||||
"gaps": gaps[:100],
|
||||
}
|
||||
|
||||
|
||||
def iter_contiguous_segments(chunks: list[RawChunk]) -> Iterator[tuple[int, list[RawChunk]]]:
|
||||
if not chunks:
|
||||
return
|
||||
segment_id = 0
|
||||
current = [chunks[0]]
|
||||
for previous, chunk in zip(chunks, chunks[1:]):
|
||||
if chunk.sequence != previous.sequence + 1:
|
||||
yield segment_id, current
|
||||
segment_id += 1
|
||||
current = [chunk]
|
||||
else:
|
||||
current.append(chunk)
|
||||
yield segment_id, current
|
||||
|
||||
@@ -0,0 +1,35 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
from capture_format_v2 import file_summary, read_capture
|
||||
from pipeline_common_corrected import parse_imu_capture, parse_rtk_capture, write_json, write_jsonl
|
||||
|
||||
|
||||
def main() -> None:
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--rtk", type=Path, required=True)
|
||||
parser.add_argument("--imu", type=Path, required=True)
|
||||
parser.add_argument("--out", type=Path, required=True)
|
||||
args = parser.parse_args()
|
||||
args.out.mkdir(parents=True, exist_ok=True)
|
||||
rtk_capture = read_capture(args.rtk)
|
||||
imu_capture = read_capture(args.imu)
|
||||
rtk_rows = parse_rtk_capture(rtk_capture)
|
||||
imu_rows = parse_imu_capture(imu_capture)
|
||||
write_jsonl(args.out / "rtk.jsonl", rtk_rows)
|
||||
write_jsonl(args.out / "imu.jsonl", imu_rows)
|
||||
write_json(args.out / "parse_summary.json", {
|
||||
"rtk_capture": file_summary(rtk_capture),
|
||||
"imu_capture": file_summary(imu_capture),
|
||||
"rtk_records": len(rtk_rows),
|
||||
"rtk_checksum_valid": sum(bool(row.get("checksum_valid")) for row in rtk_rows),
|
||||
"imu_frames": len(imu_rows),
|
||||
"imu_crc_valid": sum(bool(row.get("crc_valid")) for row in imu_rows),
|
||||
})
|
||||
print(f"RTK records={len(rtk_rows)}, IMU frames={len(imu_rows)}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,300 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import binascii
|
||||
import json
|
||||
import math
|
||||
import struct
|
||||
from pathlib import Path
|
||||
from typing import Iterable
|
||||
|
||||
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments, read_capture
|
||||
|
||||
|
||||
DOTNET_UNIX_EPOCH_TICKS = 621355968000000000
|
||||
|
||||
|
||||
def ticks_to_unix_ns(ticks: int) -> int:
|
||||
return (ticks - DOTNET_UNIX_EPOCH_TICKS) * 100
|
||||
|
||||
|
||||
def safe_float(value: str, default=None):
|
||||
try:
|
||||
return float(value)
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
|
||||
def safe_int(value: str, default=None):
|
||||
try:
|
||||
return int(value)
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
|
||||
def nmea_checksum_valid(line: str) -> bool:
|
||||
star = line.rfind("*")
|
||||
if star < 0:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1:star + 3], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
value = 0
|
||||
for char in line[1:star]:
|
||||
value ^= ord(char)
|
||||
return value == expected
|
||||
|
||||
|
||||
def unicore_crc32(text: str) -> int:
|
||||
crc = 0
|
||||
for value in text.encode("ascii", "replace"):
|
||||
crc ^= value
|
||||
for _ in range(8):
|
||||
crc = (crc >> 1) ^ (0xEDB88320 if crc & 1 else 0)
|
||||
return crc & 0xFFFFFFFF
|
||||
|
||||
|
||||
def unicore_checksum_valid(line: str) -> bool:
|
||||
star = line.rfind("*")
|
||||
if star < 0 or len(line) < star + 9:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1:star + 9], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
return unicore_crc32(line[1:star]) == expected
|
||||
|
||||
|
||||
def parse_checksum(line: str) -> bool:
|
||||
if line.startswith("$"):
|
||||
return nmea_checksum_valid(line)
|
||||
if line.startswith("#"):
|
||||
return unicore_checksum_valid(line)
|
||||
return False
|
||||
|
||||
|
||||
def parse_nmea_latlon(value: str, hemisphere: str):
|
||||
raw = safe_float(value)
|
||||
if raw is None:
|
||||
return None
|
||||
degrees = math.floor(raw / 100.0)
|
||||
result = degrees + (raw - degrees * 100.0) / 60.0
|
||||
if hemisphere.upper() in ("S", "W"):
|
||||
result = -result
|
||||
return result
|
||||
|
||||
|
||||
def parse_gga(line: str) -> dict:
|
||||
fields = line[:line.rfind("*")].split(",")
|
||||
if len(fields) < 10:
|
||||
raise ValueError("GGA has too few fields")
|
||||
return {
|
||||
"type": "GGA",
|
||||
"position_time_utc": fields[1],
|
||||
"lat_deg": parse_nmea_latlon(fields[2], fields[3]),
|
||||
"lon_deg": parse_nmea_latlon(fields[4], fields[5]),
|
||||
"fix_quality": safe_int(fields[6], -1),
|
||||
"satellites": safe_int(fields[7], -1),
|
||||
"hdop": safe_float(fields[8]),
|
||||
"altitude_m": safe_float(fields[9]),
|
||||
"geoid_separation_m": safe_float(fields[11]) if len(fields) > 11 else None,
|
||||
"differential_age_s": safe_float(fields[13]) if len(fields) > 13 else None,
|
||||
"station_id": fields[14].strip('"') if len(fields) > 14 else "",
|
||||
}
|
||||
|
||||
|
||||
def parse_heading(line: str) -> dict:
|
||||
before_crc = line[:line.rfind("*")]
|
||||
header, payload = before_crc.split(";", 1)
|
||||
header_fields = header.split(",")
|
||||
fields = payload.split(",")
|
||||
if len(fields) < 7:
|
||||
raise ValueError("UNIHEADINGA has too few fields")
|
||||
raw_heading = safe_float(fields[3])
|
||||
return {
|
||||
"type": "UNIHEADINGA",
|
||||
"gnss_week": safe_int(header_fields[4]) if len(header_fields) > 4 else None,
|
||||
"gnss_tow_ms": safe_int(header_fields[5]) if len(header_fields) > 5 else None,
|
||||
"heading_status": fields[0],
|
||||
"heading_solution": fields[1],
|
||||
"baseline_length_m": safe_float(fields[2]),
|
||||
"raw_heading_deg": raw_heading,
|
||||
"pitch_deg": safe_float(fields[4]),
|
||||
"heading_stddev_deg": safe_float(fields[6]),
|
||||
"pitch_stddev_deg": safe_float(fields[7]) if len(fields) > 7 else None,
|
||||
"station_id": fields[8].strip('"') if len(fields) > 8 else "",
|
||||
"satellites": safe_int(fields[9], -1) if len(fields) > 9 else -1,
|
||||
"solution_satellites": safe_int(fields[10], -1) if len(fields) > 10 else -1,
|
||||
"observations": safe_int(fields[11], -1) if len(fields) > 11 else -1,
|
||||
"multi_count": safe_int(fields[12], -1) if len(fields) > 12 else -1,
|
||||
"heading_valid": fields[0] == "SOL_COMPUTED" and fields[1] in {"NARROW_INT", "NARROW_FLOAT"},
|
||||
}
|
||||
|
||||
|
||||
def chunk_source(chunks: list[RawChunk], offset: int, end: int) -> dict:
|
||||
first = chunks[0]
|
||||
last = chunks[-1]
|
||||
cursor = 0
|
||||
start_chunk = first
|
||||
end_chunk = last
|
||||
for chunk in chunks:
|
||||
chunk_start = cursor
|
||||
chunk_end = cursor + len(chunk.raw)
|
||||
if chunk_start <= offset < chunk_end:
|
||||
start_chunk = chunk
|
||||
if chunk_start < end <= chunk_end:
|
||||
end_chunk = chunk
|
||||
break
|
||||
cursor = chunk_end
|
||||
return {
|
||||
"source_segment_id": None,
|
||||
"source_chunk_sequence_first": start_chunk.sequence,
|
||||
"source_chunk_sequence_last": end_chunk.sequence,
|
||||
"source_raw_file_offset": start_chunk.raw_file_offset + max(0, offset - sum(len(c.raw) for c in chunks if c.sequence < start_chunk.sequence)),
|
||||
"source_raw_byte_length": max(0, end - offset),
|
||||
}
|
||||
|
||||
|
||||
def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while cursor < len(stream):
|
||||
newline = stream.find(b"\n", cursor)
|
||||
if newline < 0:
|
||||
break
|
||||
end = newline + 1
|
||||
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
||||
cursor = end
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
valid = parse_checksum(line)
|
||||
row = {
|
||||
"type": "UNKNOWN",
|
||||
"raw_line": line,
|
||||
"checksum_valid": valid,
|
||||
"host_receive_utc_ns": ticks_to_unix_ns(chunks[-1].receive_utc_ticks),
|
||||
"host_receive_monotonic_ticks": chunks[-1].receive_monotonic_ticks,
|
||||
"source_segment_id": segment_id,
|
||||
"source_byte_offset_in_segment": cursor - len(raw_line) - 1,
|
||||
"source_byte_length": len(raw_line) + 1,
|
||||
}
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
row.update(parse_gga(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
except ValueError as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
return rows
|
||||
|
||||
|
||||
def crc16_hi13(data: bytes) -> int:
|
||||
crc = 0
|
||||
for value in data:
|
||||
crc ^= value << 8
|
||||
for _ in range(8):
|
||||
crc = ((crc << 1) ^ 0x1021) & 0xFFFF if crc & 0x8000 else (crc << 1) & 0xFFFF
|
||||
return crc
|
||||
|
||||
|
||||
def decode_hi91(frame: bytes) -> dict:
|
||||
f32 = lambda i: struct.unpack_from("<f", frame, i)[0]
|
||||
return {
|
||||
"tag": 0x91,
|
||||
"pps_sync_stamp_ms": int.from_bytes(frame[7:9], "little"),
|
||||
"temperature_c": struct.unpack_from("<b", frame, 9)[0],
|
||||
"air_pressure_pa": f32(10),
|
||||
"device_timestamp_ms": int.from_bytes(frame[14:18], "little"),
|
||||
"accel_x_mps2": f32(18) * 9.80665,
|
||||
"accel_y_mps2": f32(22) * 9.80665,
|
||||
"accel_z_mps2": f32(26) * 9.80665,
|
||||
"gyro_x_radps": f32(30) * math.pi / 180.0,
|
||||
"gyro_y_radps": f32(34) * math.pi / 180.0,
|
||||
"gyro_z_radps": f32(38) * math.pi / 180.0,
|
||||
"mag_x_ut": f32(42), "mag_y_ut": f32(46), "mag_z_ut": f32(50),
|
||||
"roll_deg": f32(54), "pitch_deg": f32(58), "yaw_deg": f32(62),
|
||||
"quaternion_w": f32(66), "quaternion_x": f32(70),
|
||||
"quaternion_y": f32(74), "quaternion_z": f32(78),
|
||||
}
|
||||
|
||||
|
||||
def decode_hi92(frame: bytes) -> dict:
|
||||
i16 = lambda i: struct.unpack_from("<h", frame, i)[0]
|
||||
i32 = lambda i: struct.unpack_from("<i", frame, i)[0]
|
||||
return {
|
||||
"tag": 0x92,
|
||||
"status": int.from_bytes(frame[7:9], "little"),
|
||||
"temperature_c": struct.unpack_from("<b", frame, 9)[0],
|
||||
"pps_sync_stamp_ms": int.from_bytes(frame[10:12], "little"),
|
||||
"air_pressure_pa": i16(12) + 100000.0,
|
||||
"heave_m": i16(14) * 0.001,
|
||||
"gyro_x_radps": i16(16) * 0.001, "gyro_y_radps": i16(18) * 0.001, "gyro_z_radps": i16(20) * 0.001,
|
||||
"accel_x_mps2": i16(22) * 0.0048828, "accel_y_mps2": i16(24) * 0.0048828, "accel_z_mps2": i16(26) * 0.0048828,
|
||||
"mag_x_ut": i16(28) * 0.030517, "mag_y_ut": i16(30) * 0.030517, "mag_z_ut": i16(32) * 0.030517,
|
||||
"roll_deg": i32(34) * 0.001, "pitch_deg": i32(38) * 0.001, "yaw_deg": i32(42) * 0.001,
|
||||
"quaternion_w": i16(46) * 0.0001, "quaternion_x": i16(48) * 0.0001,
|
||||
"quaternion_y": i16(50) * 0.0001, "quaternion_z": i16(52) * 0.0001,
|
||||
}
|
||||
|
||||
|
||||
def parse_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while True:
|
||||
start = stream.find(b"\x5a\xa5", cursor)
|
||||
if start < 0 or start + 6 > len(stream):
|
||||
break
|
||||
payload_length = int.from_bytes(stream[start + 2:start + 4], "little")
|
||||
frame_length = 6 + payload_length
|
||||
if payload_length <= 0 or payload_length > 512:
|
||||
cursor = start + 1
|
||||
continue
|
||||
if start + frame_length > len(stream):
|
||||
break
|
||||
frame = stream[start:start + frame_length]
|
||||
expected = int.from_bytes(frame[4:6], "little")
|
||||
actual = crc16_hi13(frame[:4] + frame[6:])
|
||||
end = start + frame_length
|
||||
source = chunk_source(chunks, start, end)
|
||||
source["source_segment_id"] = segment_id
|
||||
row = {
|
||||
"type": "HI13",
|
||||
"tag": frame[6],
|
||||
"frame_length": frame_length,
|
||||
"crc_valid": expected == actual,
|
||||
"host_receive_utc_ns": ticks_to_unix_ns(chunks[-1].receive_utc_ticks),
|
||||
"host_receive_monotonic_ticks": chunks[-1].receive_monotonic_ticks,
|
||||
"source_segment_id": segment_id,
|
||||
"source_byte_offset_in_segment": start,
|
||||
"source_byte_length": frame_length,
|
||||
"raw_frame_hex": frame.hex(),
|
||||
}
|
||||
if expected == actual:
|
||||
try:
|
||||
row.update(decode_hi91(frame) if frame[6] == 0x91 else decode_hi92(frame) if frame[6] == 0x92 else {})
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
cursor = end
|
||||
return rows
|
||||
|
||||
|
||||
def write_jsonl(path: Path, rows: Iterable[dict]) -> None:
|
||||
with path.open("w", encoding="utf-8", newline="\n") as stream:
|
||||
for row in rows:
|
||||
stream.write(json.dumps(row, ensure_ascii=False, separators=(",", ":")) + "\n")
|
||||
|
||||
|
||||
def write_json(path: Path, value: dict) -> None:
|
||||
path.write_text(json.dumps(value, ensure_ascii=False, indent=2), encoding="utf-8")
|
||||
|
||||
|
||||
def load_jsonl(path: Path) -> list[dict]:
|
||||
with path.open("r", encoding="utf-8") as stream:
|
||||
return [json.loads(line) for line in stream if line.strip()]
|
||||
@@ -0,0 +1,106 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import bisect
|
||||
|
||||
from pipeline_common import *
|
||||
from capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
||||
|
||||
|
||||
_SPAN_CACHE: dict[int, tuple[list[RawChunk], list[int]]] = {}
|
||||
|
||||
|
||||
def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
|
||||
key = id(chunks)
|
||||
cached = _SPAN_CACHE.get(key)
|
||||
if cached is not None and cached[0] is chunks:
|
||||
return cached[1]
|
||||
starts = []
|
||||
cursor = 0
|
||||
for chunk in chunks:
|
||||
starts.append(cursor)
|
||||
cursor += len(chunk.raw)
|
||||
_SPAN_CACHE[key] = (chunks, starts)
|
||||
return starts
|
||||
|
||||
|
||||
def source_for_span(chunks: list[RawChunk], start: int, end: int, segment_id: int) -> dict:
|
||||
starts = _chunk_starts(chunks)
|
||||
start_index = max(0, min(len(chunks) - 1, bisect.bisect_right(starts, start) - 1))
|
||||
end_index = max(start_index, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
|
||||
start_chunk = chunks[start_index]
|
||||
end_chunk = chunks[end_index]
|
||||
return {
|
||||
"source_segment_id": segment_id,
|
||||
"source_chunk_sequence_first": start_chunk.sequence,
|
||||
"source_chunk_sequence_last": end_chunk.sequence,
|
||||
"source_raw_file_offset": start_chunk.raw_file_offset + (start - starts[start_index]),
|
||||
"source_raw_byte_length": end - start,
|
||||
"host_receive_utc_ns": ticks_to_unix_ns(end_chunk.receive_utc_ticks),
|
||||
"host_receive_monotonic_ticks": end_chunk.receive_monotonic_ticks,
|
||||
}
|
||||
|
||||
def parse_rtk_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while cursor < len(stream):
|
||||
newline = stream.find(b"\n", cursor)
|
||||
if newline < 0:
|
||||
break
|
||||
end = newline + 1
|
||||
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
||||
start = cursor
|
||||
cursor = end
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
row = {"type": "UNKNOWN", "raw_line": line, "checksum_valid": parse_checksum(line)}
|
||||
row.update(source_for_span(chunks, start, end, segment_id))
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
row.update(parse_gga(line))
|
||||
elif line.startswith("#UNIHEADINGA"):
|
||||
row.update(parse_heading(line))
|
||||
except ValueError as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
return rows
|
||||
|
||||
|
||||
def parse_imu_capture(capture: CaptureFile) -> list[dict]:
|
||||
rows = []
|
||||
for segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
cursor = 0
|
||||
while True:
|
||||
start = stream.find(b"\x5a\xa5", cursor)
|
||||
if start < 0 or start + 6 > len(stream):
|
||||
break
|
||||
payload_length = int.from_bytes(stream[start + 2:start + 4], "little")
|
||||
frame_length = 6 + payload_length
|
||||
if payload_length <= 0 or payload_length > 512:
|
||||
cursor = start + 1
|
||||
continue
|
||||
if start + frame_length > len(stream):
|
||||
break
|
||||
frame = stream[start:start + frame_length]
|
||||
expected = int.from_bytes(frame[4:6], "little")
|
||||
actual = crc16_hi13(frame[:4] + frame[6:])
|
||||
end = start + frame_length
|
||||
row = {
|
||||
"type": "HI13",
|
||||
"tag": frame[6],
|
||||
"frame_length": frame_length,
|
||||
"crc_valid": expected == actual,
|
||||
"raw_frame_hex": frame.hex(),
|
||||
}
|
||||
row.update(source_for_span(chunks, start, end, segment_id))
|
||||
if row["crc_valid"]:
|
||||
try:
|
||||
row.update(decode_hi91(frame) if frame[6] == 0x91 else decode_hi92(frame) if frame[6] == 0x92 else {})
|
||||
except (IndexError, struct.error, ValueError) as ex:
|
||||
row["parse_error"] = str(ex)
|
||||
rows.append(row)
|
||||
cursor = end
|
||||
return rows
|
||||
Reference in New Issue
Block a user