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

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

Browse Source
This commit is contained in:
lichun.qu
2026-07-23 18:55:50 +08:00
parent 6b7844a977
commit f72fcb71cc
91 changed files with 39561 additions and 503 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/README.md
+26
View File
@@ -0,0 +1,26 @@
# tools
本目录负责从原始 dlog/rscap 导出 NPZ、关联 RTK/IMU 并生成 prepared；完整流程和参数统一见仓库根目录 `README.md`。
| 文件 | 作用 |
|---|---|
| `frontlidar_dlog_export.py` | 从单个 Medulla dlog 导出原始 LiDAR NPZ，可匹配站内 RTK。 |
| `prepare_station_dataset.py` | 旧式逐站导出生成 prepared。 |
| `build_multisensor_npz.py` | 以 LiDAR 帧为索引关联独立 RTK/IMU。 |
| `prepare_multisensor_station_dataset.py` | combined NPZ 生成 prepared。 |
| `rscap_v2/capture_format_v2.py` | 读取 rscap v2。 |
| `rscap_v2/audit_capture_v2.py` | 审计 capture 完整性。 |
| `rscap_v2/parse_rtk_imu_v2.py` | 解析 RTK/IMU 为 JSONL。 |
| `rscap_v2/pipeline_common*.py` | 时间和协议解析共用逻辑。 |
统一输出接口：
```text
prepared/
frames_all/station_*.npz
body_poses_rear_gga_raw_rear_to_front.csv
station_summary.csv
manifest.json
```
后续配准必须使用 `points_raw`，不能使用已经由某套外参变换后的点云。
tools/build_multisensor_npz.py
+312
View File
@@ -0,0 +1,312 @@
#!/usr/bin/env python3
"""Build one LiDAR-centric NPZ per frame with matched RTK and an IMU window.
Inputs are LiDAR frame NPZ files from frontlidar_dlog_export.py and parsed
RTK/IMU JSONL files from parse_rtk_imu_v2.py. Raw .rscap files remain the
traceability source; this script never modifies them.
"""
from __future__ import annotations
import argparse
import csv
import json
from pathlib import Path
from typing import Any
import numpy as np
GPS_EPOCH_UNIX_NS = 315964800 * 1_000_000_000
def parse_named_path(text: str) -> tuple[str, Path]:
if "=" not in text:
raise argparse.ArgumentTypeError("expected NAME=PATH")
name, raw_path = text.split("=", 1)
if not name.strip():
raise argparse.ArgumentTypeError("segment name is empty")
return name.strip(), Path(raw_path)
def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser()
parser.add_argument(
"--lidar",
type=parse_named_path,
action="append",
required=True,
metavar="NAME=FRAMES_DIR",
help="Repeat for each LiDAR segment; directory contains exported *.npz frames.",
)
parser.add_argument("--rtk", type=Path, action="append", required=True, help="Parsed rtk.jsonl; repeat per session.")
parser.add_argument("--imu", type=Path, action="append", required=True, help="Parsed imu.jsonl; repeat per session.")
parser.add_argument("--out", type=Path, required=True)
parser.add_argument("--rtk-max-dt-ms", type=float, default=150.0)
parser.add_argument("--imu-before-ms", type=float, default=100.0)
parser.add_argument("--imu-after-ms", type=float, default=100.0)
parser.add_argument("--gps-utc-leap-seconds", type=int, default=18)
parser.add_argument("--overwrite", action="store_true")
return parser.parse_args()
def load_jsonl(paths: list[Path]) -> list[dict[str, Any]]:
rows: list[dict[str, Any]] = []
for source_index, path in enumerate(paths):
source_file = str(path.resolve())
with path.open("r", encoding="utf-8") as stream:
for line_number, line in enumerate(stream, start=1):
if not line.strip():
continue
row = json.loads(line)
row["_source_file"] = source_file
row["_source_index"] = source_index
row["_source_line"] = line_number
rows.append(row)
return rows
def utf8_array(value: Any) -> np.ndarray:
return np.frombuffer(str(value if value is not None else "").encode("utf-8"), dtype=np.uint8)
def scalar(array: np.ndarray) -> Any:
return array.reshape(-1)[0].item()
def nearest_index(times: np.ndarray, target: int) -> int:
if not len(times):
return -1
right = int(np.searchsorted(times, target, side="left"))
candidates = [index for index in (right - 1, right) if 0 <= index < len(times)]
return min(candidates, key=lambda index: abs(int(times[index]) - target))
def estimate_imu_times(rows: list[dict[str, Any]]) -> list[dict[str, Any]]:
"""Recover 100 Hz timing inside each serial chunk from device timestamps.
A capture chunk has one host receive timestamp but may contain several IMU
frames. The last frame is anchored to the chunk receive time and earlier
frames are moved backwards by their device timestamp difference.
"""
groups: dict[tuple[int, int], list[dict[str, Any]]] = {}
for row in rows:
if not row.get("crc_valid") or row.get("device_timestamp_ms") is None:
continue
key = (int(row["_source_index"]), int(row.get("source_chunk_sequence_last", -1)))
groups.setdefault(key, []).append(row)
result: list[dict[str, Any]] = []
for group in groups.values():
group.sort(key=lambda row: (int(row["device_timestamp_ms"]), int(row["_source_line"])))
last_device = int(group[-1]["device_timestamp_ms"])
host_ns = int(group[-1]["host_receive_utc_ns"])
for row in group:
delta_ms = (last_device - int(row["device_timestamp_ms"])) & 0xFFFFFFFF
if delta_ms > 60_000:
delta_ms = 0
copied = dict(row)
copied["estimated_time_ns"] = host_ns - delta_ms * 1_000_000
result.append(copied)
result.sort(key=lambda row: int(row["estimated_time_ns"]))
return result
def gnss_utc_ns(row: dict[str, Any], leap_seconds: int) -> int | None:
week, tow_ms = row.get("gnss_week"), row.get("gnss_tow_ms")
if week is None or tow_ms is None:
return None
seconds = int(week) * 604800 + float(tow_ms) / 1000.0 - leap_seconds
return GPS_EPOCH_UNIX_NS + int(round(seconds * 1_000_000_000))
def numeric_array(rows: list[dict[str, Any]], key: str, dtype: Any, default: Any) -> np.ndarray:
return np.asarray([row.get(key, default) if row.get(key) is not None else default for row in rows], dtype=dtype)
def raw_frame_matrix(rows: list[dict[str, Any]]) -> tuple[np.ndarray, np.ndarray]:
frames = [bytes.fromhex(str(row.get("raw_frame_hex", ""))) for row in rows]
lengths = np.asarray([len(frame) for frame in frames], dtype=np.int32)
width = max(lengths, default=0)
matrix = np.zeros((len(frames), width), dtype=np.uint8)
for index, frame in enumerate(frames):
matrix[index, : len(frame)] = np.frombuffer(frame, dtype=np.uint8)
return matrix, lengths
def add_rtk(values: dict[str, np.ndarray], prefix: str, row: dict[str, Any] | None, dt_ns: int | None) -> None:
values[f"{prefix}_valid"] = np.asarray([row is not None], dtype=np.uint8)
values[f"{prefix}_dt_ns"] = np.asarray([dt_ns or 0], dtype=np.int64)
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([0], dtype=np.int64)
values[f"{prefix}_raw_utf8"] = utf8_array("")
values[f"{prefix}_source_file_utf8"] = utf8_array("")
values[f"{prefix}_source_raw_file_offset"] = np.asarray([-1], dtype=np.int64)
values[f"{prefix}_source_raw_byte_length"] = np.asarray([0], dtype=np.int32)
if row is None:
return
values[f"{prefix}_host_receive_utc_ns"] = np.asarray([row.get("host_receive_utc_ns", 0)], dtype=np.int64)
values[f"{prefix}_raw_utf8"] = utf8_array(row.get("raw_line", ""))
values[f"{prefix}_source_file_utf8"] = utf8_array(row.get("_source_file", ""))
values[f"{prefix}_source_raw_file_offset"] = np.asarray([row.get("source_raw_file_offset", -1)], dtype=np.int64)
values[f"{prefix}_source_raw_byte_length"] = np.asarray([row.get("source_raw_byte_length", 0)], dtype=np.int32)
def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
for key, dtype, default in (
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
("differential_age_s", np.float64, np.nan),
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
("pitch_stddev_deg", np.float64, np.nan), ("heading_satellites", np.int32, -1),
("solution_satellites", np.int32, -1),
):
values[f"rtk_{key}"] = np.asarray([default], dtype=dtype)
values["rtk_fixed"] = np.asarray([0], dtype=np.uint8)
values["rtk_heading_solution_utf8"] = utf8_array("")
values["rtk_heading_gnss_utc_ns"] = np.asarray([0], dtype=np.int64)
values["rtk_heading_host_minus_gnss_ns"] = np.asarray([0], dtype=np.int64)
def main() -> int:
args = parse_args()
if args.out.exists() and any(args.out.iterdir()) and not args.overwrite:
raise FileExistsError(f"{args.out} is non-empty; pass --overwrite")
frames_out = args.out / "frames"
frames_out.mkdir(parents=True, exist_ok=True)
rtk_rows = load_jsonl(args.rtk)
gga = sorted(
[row for row in rtk_rows if row.get("type") == "GGA" and row.get("checksum_valid") and row.get("lat_deg") is not None],
key=lambda row: int(row["host_receive_utc_ns"]),
)
heading = sorted(
[row for row in rtk_rows if row.get("type") == "UNIHEADINGA" and row.get("checksum_valid") and row.get("heading_valid")],
key=lambda row: int(row["host_receive_utc_ns"]),
)
imu = estimate_imu_times(load_jsonl(args.imu))
gga_times = np.asarray([int(row["host_receive_utc_ns"]) for row in gga], dtype=np.int64)
heading_times = np.asarray([int(row["host_receive_utc_ns"]) for row in heading], dtype=np.int64)
imu_times = np.asarray([int(row["estimated_time_ns"]) for row in imu], dtype=np.int64)
manifest: list[dict[str, Any]] = []
global_index = 0
max_rtk_ns = int(args.rtk_max_dt_ms * 1_000_000)
before_ns = int(args.imu_before_ms * 1_000_000)
after_ns = int(args.imu_after_ms * 1_000_000)
for segment_name, frame_dir in args.lidar:
frame_paths = sorted(frame_dir.glob("*.npz"))
if not frame_paths:
raise FileNotFoundError(f"no NPZ frames under {frame_dir}")
for segment_index, source in enumerate(frame_paths):
with np.load(source, allow_pickle=False) as frame:
values = {key: np.asarray(frame[key]) for key in frame.files}
lidar_time_ns = int(scalar(values["unix_time_ns"]))
gga_index = nearest_index(gga_times, lidar_time_ns)
heading_index = nearest_index(heading_times, lidar_time_ns)
gga_row = gga[gga_index] if gga_index >= 0 else None
heading_row = heading[heading_index] if heading_index >= 0 else None
gga_dt = int(gga_times[gga_index]) - lidar_time_ns if gga_index >= 0 else None
heading_dt = int(heading_times[heading_index]) - lidar_time_ns if heading_index >= 0 else None
gga_ok = gga_row is not None and abs(gga_dt or 0) <= max_rtk_ns
heading_ok = heading_row is not None and abs(heading_dt or 0) <= max_rtk_ns
add_rtk(values, "rtk_gga", gga_row if gga_ok else None, gga_dt)
add_rtk(values, "rtk_heading", heading_row if heading_ok else None, heading_dt)
initialize_rtk_measurements(values)
if gga_ok and gga_row:
for key, dtype, default in (
("lat_deg", np.float64, np.nan), ("lon_deg", np.float64, np.nan),
("altitude_m", np.float64, np.nan), ("hdop", np.float64, np.nan),
("fix_quality", np.int32, -1), ("gga_satellites", np.int32, -1),
("differential_age_s", np.float64, np.nan),
):
values[f"rtk_{key}"] = np.asarray([gga_row.get(key, default)], dtype=dtype)
values["rtk_gga_satellites"] = np.asarray([gga_row.get("satellites", -1)], dtype=np.int32)
values["rtk_fixed"] = np.asarray([int(gga_row.get("fix_quality", -1)) in {4, 5}], dtype=np.uint8)
if heading_ok and heading_row:
for key, dtype, default in (
("gnss_week", np.int32, -1), ("gnss_tow_ms", np.int64, -1),
("baseline_length_m", np.float64, np.nan), ("raw_heading_deg", np.float64, np.nan),
("pitch_deg", np.float64, np.nan), ("heading_stddev_deg", np.float64, np.nan),
("pitch_stddev_deg", np.float64, np.nan),
("solution_satellites", np.int32, -1),
):
values[f"rtk_{key}"] = np.asarray([heading_row.get(key, default)], dtype=dtype)
values["rtk_heading_satellites"] = np.asarray([heading_row.get("satellites", -1)], dtype=np.int32)
values["rtk_heading_solution_utf8"] = utf8_array(heading_row.get("heading_solution", ""))
device_ns = gnss_utc_ns(heading_row, args.gps_utc_leap_seconds)
values["rtk_heading_gnss_utc_ns"] = np.asarray([device_ns or 0], dtype=np.int64)
values["rtk_heading_host_minus_gnss_ns"] = np.asarray(
[int(heading_row["host_receive_utc_ns"]) - device_ns if device_ns is not None else 0], dtype=np.int64
)
left = int(np.searchsorted(imu_times, lidar_time_ns - before_ns, side="left"))
right = int(np.searchsorted(imu_times, lidar_time_ns + after_ns, side="right"))
window = imu[left:right]
values["imu_window_count"] = np.asarray([len(window)], dtype=np.int32)
values["imu_valid"] = np.asarray([bool(window)], dtype=np.uint8)
values["imu_time_ns"] = numeric_array(window, "estimated_time_ns", np.int64, 0)
values["imu_host_receive_utc_ns"] = numeric_array(window, "host_receive_utc_ns", np.int64, 0)
for key in ("device_timestamp_ms", "pps_sync_stamp_ms", "tag"):
values[f"imu_{key}"] = numeric_array(window, key, np.int64, -1)
for key in (
"temperature_c", "air_pressure_pa", "accel_x_mps2", "accel_y_mps2", "accel_z_mps2",
"gyro_x_radps", "gyro_y_radps", "gyro_z_radps", "mag_x_ut", "mag_y_ut", "mag_z_ut",
"roll_deg", "pitch_deg", "yaw_deg", "quaternion_w", "quaternion_x", "quaternion_y", "quaternion_z",
):
values[f"imu_{key}"] = numeric_array(window, key, np.float64, np.nan)
values["imu_source_index"] = numeric_array(window, "_source_index", np.int32, -1)
values["imu_source_raw_file_offset"] = numeric_array(window, "source_raw_file_offset", np.int64, -1)
raw_matrix, raw_lengths = raw_frame_matrix(window)
values["imu_raw_frame_bytes"] = raw_matrix
values["imu_raw_frame_length"] = raw_lengths
values["imu_source_files_json_utf8"] = utf8_array(json.dumps([str(path.resolve()) for path in args.imu], ensure_ascii=False))
values["source_lidar_file_utf8"] = utf8_array(source.resolve())
values["segment_name_utf8"] = utf8_array(segment_name)
output = frames_out / f"{segment_name}_{segment_index:06d}.npz"
np.savez_compressed(output, **values)
manifest.append({
"global_index": global_index,
"segment": segment_name,
"segment_index": segment_index,
"output": str(output.relative_to(args.out)),
"source_lidar": str(source.resolve()),
"lidar_time_ns": lidar_time_ns,
"rtk_gga_dt_ns": gga_dt,
"rtk_heading_dt_ns": heading_dt,
"rtk_valid": gga_ok,
"heading_valid": heading_ok,
"rtk_fix_quality": gga_row.get("fix_quality") if gga_ok and gga_row else None,
"rtk_fixed": bool(gga_ok and gga_row and int(gga_row.get("fix_quality", -1)) in {4, 5}),
"imu_window_count": len(window),
})
global_index += 1
fields = sorted({key for row in manifest for key in row})
with (args.out / "manifest.csv").open("w", encoding="utf-8", newline="") as stream:
writer = csv.DictWriter(stream, fieldnames=fields)
writer.writeheader()
writer.writerows(manifest)
summary = {
"frames": len(manifest),
"segments": {name: sum(row["segment"] == name for row in manifest) for name, _ in args.lidar},
"rtk_valid": sum(bool(row["rtk_valid"]) for row in manifest),
"heading_valid": sum(bool(row["heading_valid"]) for row in manifest),
"rtk_fixed": sum(bool(row["rtk_fixed"]) for row in manifest),
"imu_window_nonempty": sum(int(row["imu_window_count"]) > 0 for row in manifest),
"rtk_max_dt_ms": args.rtk_max_dt_ms,
"imu_window_ms": [-args.imu_before_ms, args.imu_after_ms],
"time_basis": "LiDAR and serial host UTC; RTK GNSS time and IMU device time are retained for clock-model refinement",
"imu_orientation_warning": "IMU values are in the raw IMU sensor frame; no LiDAR/body extrinsic is applied",
}
(args.out / "dataset_summary.json").write_text(json.dumps(summary, ensure_ascii=False, indent=2), encoding="utf-8")
print(json.dumps(summary, ensure_ascii=False, indent=2))
return 0
if __name__ == "__main__":
raise SystemExit(main())
tools/prepare_multisensor_station_dataset.py
+170
View File
@@ -0,0 +1,170 @@
#!/usr/bin/env python3
"""Prepare one static LiDAR frame and one yaw-only RTK body 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="rear_gga_raw_rear_to_front")
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)
body = antenna - yaw_rotation(yaw) @ lever
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *body, 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"body_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",
"body_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())
tools/rscap_v2/audit_capture_v2.py
+22
View File
@@ -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()
tools/rscap_v2/capture_format_v2.py
+254
View File
@@ -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
tools/rscap_v2/parse_rtk_imu_v2.py
+35
View File
@@ -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()
tools/rscap_v2/pipeline_common.py
+300
View File
@@ -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()]
tools/rscap_v2/pipeline_common_corrected.py
+106
View File
@@ -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