195 lines
12 KiB
Python
195 lines
12 KiB
Python
#!/usr/bin/env python3
|
|
"""Prepare one static LiDAR frame and one yaw-only RTK body pose per station."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import csv
|
|
import json
|
|
import math
|
|
import re
|
|
import shutil
|
|
import sys
|
|
from pathlib import Path
|
|
from typing import Any
|
|
|
|
import numpy as np
|
|
|
|
POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"]
|
|
|
|
|
|
def parse_args() -> argparse.Namespace:
|
|
parser = argparse.ArgumentParser(
|
|
description="Select one static LiDAR frame per exported station and rebuild yaw-only RTK body poses."
|
|
)
|
|
parser.add_argument("--export-root", type=Path, required=True, help="Directory containing exported station directories.")
|
|
parser.add_argument("--output", type=Path, required=True, help="Output prepared directory.")
|
|
parser.add_argument("--pose-name", default="rear_gga_raw_rear_to_front", help="Suffix of body_poses_<name>.csv.")
|
|
parser.add_argument("--heading-offset-deg", type=float, required=True, help="Added to raw_heading_deg before ENU yaw conversion.")
|
|
parser.add_argument("--antenna-lever", type=float, nargs=3, metavar=("X", "Y", "Z"), required=True, help="Antenna position in body coordinates [m].")
|
|
parser.add_argument("--expected-stations", type=int, default=0, help="Require exactly this many usable stations; 0 disables.")
|
|
parser.add_argument("--min-stations", type=int, default=30, help="Fail below this many usable stations.")
|
|
parser.add_argument("--groups", nargs="*", default=None, help="Optional explicit station directory names.")
|
|
parser.add_argument("--accepted-fixes", type=int, nargs="+", default=[4, 5], help="Accepted GGA fix values.")
|
|
parser.add_argument("--heading-std-limit-deg", type=float, default=float("inf"), help="Reject a station above this heading circular stddev.")
|
|
parser.add_argument("--overwrite", action="store_true", help="Allow replacing generated files in a non-empty output directory.")
|
|
return parser.parse_args()
|
|
|
|
|
|
def truth(value: Any) -> bool:
|
|
return str(value).strip().lower() in {"1", "true", "yes", "y"}
|
|
|
|
|
|
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 read_csv(path: Path) -> list[dict[str, str]]:
|
|
with path.open("r", encoding="utf-8-sig", newline="") as stream:
|
|
return list(csv.DictReader(stream))
|
|
|
|
|
|
def require_columns(rows: list[dict[str, str]], path: Path, columns: list[str]) -> None:
|
|
if not rows:
|
|
raise ValueError(f"{path}: no rows")
|
|
missing = [column for column in columns if column not in rows[0]]
|
|
if missing:
|
|
raise ValueError(f"{path}: missing required columns: {', '.join(missing)}")
|
|
|
|
|
|
def circular_mean_deg(values: np.ndarray) -> float:
|
|
radians = np.deg2rad(values)
|
|
return float(np.rad2deg(math.atan2(float(np.mean(np.sin(radians))), float(np.mean(np.cos(radians))))) % 360.0)
|
|
|
|
|
|
def circular_std_deg(values: np.ndarray) -> float:
|
|
radians = np.deg2rad(values)
|
|
resultant = max(math.hypot(float(np.mean(np.cos(radians))), float(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_ecef: np.ndarray, origin_lat_deg: float, origin_lon_deg: float) -> np.ndarray:
|
|
lat, lon = math.radians(origin_lat_deg), math.radians(origin_lon_deg)
|
|
sin_lat, cos_lat, sin_lon, cos_lon = math.sin(lat), math.cos(lat), math.sin(lon), math.cos(lon)
|
|
rotation = np.array([[-sin_lon, cos_lon, 0.0], [-sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat], [cos_lat * cos_lon, cos_lat * sin_lon, sin_lat]], dtype=float)
|
|
return rotation @ (ecef - origin_ecef)
|
|
|
|
|
|
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]], dtype=float)
|
|
|
|
|
|
def select_frame(station: Path) -> tuple[dict[str, str], int, int]:
|
|
path = station / "reports" / "manifest.csv"
|
|
rows = read_csv(path)
|
|
require_columns(rows, path, ["status", "unix_time_ns", "output_file"])
|
|
exported = [row for row in rows if row["status"] in {"exported", "resumed"}]
|
|
if not exported:
|
|
raise ValueError(f"{station.name}: no exported/resumed LiDAR frames")
|
|
valid = [row for row in exported if truth(row.get("rtk_position_valid")) and truth(row.get("rtk_heading_valid"))]
|
|
candidates = valid or exported
|
|
candidates.sort(key=lambda row: int(row["unix_time_ns"]))
|
|
return candidates[len(candidates) // 2], len(exported), len(valid)
|
|
|
|
|
|
def validation_checks(station: Path) -> dict[str, bool]:
|
|
path = station / "reports" / "validation_report.json"
|
|
if not path.exists():
|
|
return {}
|
|
document = json.loads(path.read_text(encoding="utf-8"))
|
|
return {str(item.get("name")): bool(item.get("ok")) for item in document.get("checks", [])}
|
|
|
|
|
|
def process_station(station: Path, args: argparse.Namespace) -> tuple[dict[str, Any], dict[str, Any]]:
|
|
rtk_path = station / "rtk" / "gps_post_z.csv"
|
|
rows = read_csv(rtk_path)
|
|
columns = ["lat", "lon", "alt_m", "raw_heading_deg", "fix", "position_valid", "heading_valid", "unix_time_ns"]
|
|
require_columns(rows, rtk_path, columns)
|
|
fixes = set(args.accepted_fixes)
|
|
good = [row for row in rows if truth(row["position_valid"]) and truth(row["heading_valid"]) and int(row["fix"]) in fixes]
|
|
if not good:
|
|
raise ValueError(f"{station.name}: no RTK samples pass position/heading/fix filters")
|
|
lat = np.asarray([float(row["lat"]) for row in good])
|
|
lon = np.asarray([float(row["lon"]) for row in good])
|
|
alt = np.asarray([float(row["alt_m"]) for row in good])
|
|
heading = np.asarray([float(row["raw_heading_deg"]) for row in good])
|
|
times = np.asarray([int(row["unix_time_ns"]) for row in good], dtype=np.int64)
|
|
heading_std = circular_std_deg(heading)
|
|
if heading_std > args.heading_std_limit_deg:
|
|
raise ValueError(f"{station.name}: heading circular stddev {heading_std:.3f} deg exceeds {args.heading_std_limit_deg:.3f} deg")
|
|
frame, frame_count, valid_frame_count = select_frame(station)
|
|
source = station / frame["output_file"]
|
|
if not source.is_file():
|
|
raise FileNotFoundError(f"{station.name}: selected frame is absent: {source}")
|
|
checks = validation_checks(station)
|
|
selected = {"station": station.name, "source_frame": source, "frame_time": int(frame["unix_time_ns"]) / 1e9, "lat_deg": float(np.mean(lat)), "lon_deg": float(np.mean(lon)), "alt_m": float(np.mean(alt)), "raw_heading_deg": circular_mean_deg(heading), "raw_heading_std_deg": heading_std}
|
|
summary = {"station": station.name, "selected_frame": frame["output_file"], "exported_frames": frame_count, "frames_with_valid_rtk": valid_frame_count, "valid_rtk_samples": len(good), "raw_heading_mean_deg": selected["raw_heading_deg"], "raw_heading_std_deg": heading_std, "alt_std_m": float(np.std(alt)), "rtk_span_sec": float((times.max() - times.min()) / 1e9) if len(times) > 1 else 0.0, "payload_ok": checks.get("lidar_payload_length", ""), "rtk_parse_ok": checks.get("gps_post_z_parse", ""), "time_check_3s_ok": checks.get("rtk_time_alignment", "")}
|
|
return selected, summary
|
|
|
|
|
|
def main() -> int:
|
|
args = parse_args()
|
|
if args.min_stations < 2 or args.expected_stations < 0:
|
|
raise ValueError("--min-stations must be >=2 and --expected-stations must be >=0")
|
|
if not args.export_root.is_dir():
|
|
raise NotADirectoryError(args.export_root)
|
|
names = args.groups or [path.name for path in args.export_root.iterdir() if path.is_dir()]
|
|
stations = [args.export_root / name for name in sorted(names, key=natural_key)]
|
|
missing = [str(path) for path in stations if not path.is_dir()]
|
|
if missing:
|
|
raise FileNotFoundError("station directories do not exist: " + ", ".join(missing))
|
|
selected, summaries, rejected = [], [], []
|
|
for station in stations:
|
|
try:
|
|
item, summary = process_station(station, args)
|
|
selected.append(item); summaries.append(summary)
|
|
except (ValueError, FileNotFoundError) as error:
|
|
rejected.append({"station": station.name, "reason": str(error)})
|
|
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}")
|
|
output, frames_dir = args.output, args.output / "frames_all"
|
|
if output.exists() and any(output.iterdir()) and not args.overwrite:
|
|
raise FileExistsError(f"{output} is non-empty; pass --overwrite to replace generated files")
|
|
frames_dir.mkdir(parents=True, exist_ok=True)
|
|
origin = selected[0]
|
|
origin_ecef = geodetic_to_ecef(origin["lat_deg"], origin["lon_deg"], origin["alt_m"])
|
|
lever = np.asarray(args.antenna_lever, dtype=float)
|
|
pose_rows = []
|
|
for index, item in enumerate(selected, 1):
|
|
destination = frames_dir / f"station_{index:02d}.npz"
|
|
shutil.copy2(item["source_frame"], destination)
|
|
antenna_enu = ecef_to_enu(geodetic_to_ecef(item["lat_deg"], item["lon_deg"], item["alt_m"]), origin_ecef, origin["lat_deg"], origin["lon_deg"])
|
|
corrected_heading = (item["raw_heading_deg"] + args.heading_offset_deg) % 360.0
|
|
yaw = math.radians(90.0 - corrected_heading)
|
|
body_position = antenna_enu - yaw_rotation(yaw) @ lever
|
|
pose_rows.append(dict(zip(POSE_FIELDS, [item["frame_time"], *body_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 = 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)
|
|
fields = sorted({key for row in summaries for key in row})
|
|
with (output / "station_summary.csv").open("w", encoding="utf-8", newline="") as stream:
|
|
writer = csv.DictWriter(stream, fieldnames=fields); writer.writeheader(); writer.writerows(summaries)
|
|
manifest = {"source_export_root": str(args.export_root.resolve()), "station_count": len(selected), "pose_csv": pose_path.name, "frame_directory": "frames_all", "selection_policy": "middle frame among exported frames with valid RTK; falls back to middle exported frame", "rtk_filter": {"position_valid": True, "heading_valid": True, "accepted_fixes": args.accepted_fixes, "heading_std_limit_deg": args.heading_std_limit_deg}, "body_pose_configuration": {"raw_heading_offset_deg": args.heading_offset_deg, "antenna_lever_body_m": args.antenna_lever, "body_axes": "x forward, y left, z up; origin must match the supplied lever", "orientation_model": "yaw-only from raw_heading_deg; this tool does not reconstruct RTK pitch or roll"}, "enu_origin": {"lat_deg": origin["lat_deg"], "lon_deg": origin["lon_deg"], "alt_m": origin["alt_m"]}, "stations": [{"sequence": index + 1, "source_station": item["station"], "source_frame": str(item["source_frame"]), "prepared_frame": f"station_{index + 1:02d}.npz"} for index, item in enumerate(selected)]}
|
|
(output / "manifest.json").write_text(json.dumps(manifest, ensure_ascii=False, indent=2), encoding="utf-8")
|
|
print(json.dumps({"prepared": str(output.resolve()), "stations": len(selected), "rejected": rejected, "pose_csv": pose_path.name}, ensure_ascii=False, indent=2))
|
|
return 0
|
|
|
|
|
|
if __name__ == "__main__":
|
|
try:
|
|
raise SystemExit(main())
|
|
except Exception as error:
|
|
print(f"ERROR: {error}", file=sys.stderr)
|
|
raise
|