改为车头向前整链：主从装反机械初值、双天线 pitch/roll 姿态与默认 HeadingOffsetDeg=-90
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
Cursor
@@ -183,7 +183,8 @@ def initialize_rtk_measurements(values: dict[str, np.ndarray]) -> None:
|
||||
("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_deg", np.float64, np.nan), ("roll_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),
|
||||
):
|
||||
@@ -297,11 +298,15 @@ def build_combined(
|
||||
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_deg", np.float64, np.nan), ("roll_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)
|
||||
value = heading_row.get(key, default)
|
||||
if key == "roll_deg" and value is None:
|
||||
value = 0.0
|
||||
values[f"rtk_{key}"] = np.asarray([value], 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, gps_utc_leap_seconds)
|
||||
|
||||
+119
-63
@@ -13,6 +13,8 @@ from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from rtk_attitude import heading_to_enu_yaw, rotation_to_quat_xyzw, rtk_body_rotation
|
||||
|
||||
|
||||
def args() -> argparse.Namespace:
|
||||
p = argparse.ArgumentParser(description=__doc__)
|
||||
@@ -23,9 +25,48 @@ def args() -> argparse.Namespace:
|
||||
p.add_argument("--out", type=Path, required=True)
|
||||
p.add_argument("--max-bracket-ms", type=float, default=150.0)
|
||||
p.add_argument("--heading-std-limit-deg", type=float, default=0.5)
|
||||
p.add_argument(
|
||||
"--heading-offset-deg",
|
||||
type=float,
|
||||
default=None,
|
||||
help="Added to rawHeading before ENU yaw. Default: body_heading_offset_deg from extrinsic JSON, else 0.",
|
||||
)
|
||||
p.add_argument(
|
||||
"--orientation-model",
|
||||
choices=("heading_pitch_roll", "yaw_only"),
|
||||
default="heading_pitch_roll",
|
||||
help="heading_pitch_roll uses GNHPR/UNIHEADINGA pitch+roll in T_W_RTK; yaw_only forces pitch=roll=0",
|
||||
)
|
||||
return p.parse_args()
|
||||
|
||||
|
||||
POSITION_TYPES = {"GGA", "PVTSLNA"}
|
||||
HEADING_TYPES = {"UNIHEADINGA", "GNHPR"}
|
||||
|
||||
|
||||
def heading_row_valid(row: dict[str, Any]) -> bool:
|
||||
if row.get("type") == "UNIHEADINGA":
|
||||
return bool(row.get("checksum_valid") and row.get("heading_valid") and row.get("raw_heading_deg") is not None)
|
||||
if row.get("type") == "GNHPR":
|
||||
return bool(row.get("checksum_valid") and row.get("heading_valid") and row.get("raw_heading_deg") is not None)
|
||||
return False
|
||||
|
||||
|
||||
def heading_quality_ok(row: dict[str, Any], std_limit_deg: float) -> list[str]:
|
||||
reasons: list[str] = []
|
||||
if row.get("type") == "UNIHEADINGA":
|
||||
if str(row.get("heading_solution", "")) != "NARROW_INT":
|
||||
reasons.append("HEADING_NOT_NARROW_INT")
|
||||
std = float(row.get("heading_stddev_deg") or math.inf)
|
||||
if std > std_limit_deg:
|
||||
reasons.append("HEADING_STD_EXCEEDED")
|
||||
elif row.get("type") == "GNHPR":
|
||||
quality = int(row.get("heading_quality", -1) or -1)
|
||||
if quality not in {4, 5} and not row.get("heading_valid"):
|
||||
reasons.append("HEADING_QUALITY_NOT_FIXED")
|
||||
return reasons
|
||||
|
||||
|
||||
def read_jsonl(path: Path) -> list[dict[str, Any]]:
|
||||
with path.open(encoding="utf-8") as f:
|
||||
return [json.loads(line) for line in f if line.strip()]
|
||||
@@ -50,39 +91,6 @@ def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: f
|
||||
return r @ (ecef - origin)
|
||||
|
||||
|
||||
def yaw_matrix(yaw_rad: float) -> np.ndarray:
|
||||
c, s = math.cos(yaw_rad), math.sin(yaw_rad)
|
||||
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]], dtype=float)
|
||||
|
||||
|
||||
def matrix_to_quat_xyzw(r: np.ndarray) -> np.ndarray:
|
||||
# Stable branch-based conversion; output convention is x,y,z,w.
|
||||
tr = float(np.trace(r))
|
||||
if tr > 0.0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
q = np.array([(r[2, 1] - r[1, 2]) / s,
|
||||
(r[0, 2] - r[2, 0]) / s,
|
||||
(r[1, 0] - r[0, 1]) / s, 0.25 * s])
|
||||
else:
|
||||
i = int(np.argmax(np.diag(r)))
|
||||
if i == 0:
|
||||
s = math.sqrt(1.0 + r[0, 0] - r[1, 1] - r[2, 2]) * 2.0
|
||||
q = np.array([0.25 * s, (r[0, 1] + r[1, 0]) / s,
|
||||
(r[0, 2] + r[2, 0]) / s, (r[2, 1] - r[1, 2]) / s])
|
||||
elif i == 1:
|
||||
s = math.sqrt(1.0 + r[1, 1] - r[0, 0] - r[2, 2]) * 2.0
|
||||
q = np.array([(r[0, 1] + r[1, 0]) / s, 0.25 * s,
|
||||
(r[1, 2] + r[2, 1]) / s, (r[0, 2] - r[2, 0]) / s])
|
||||
else:
|
||||
s = math.sqrt(1.0 + r[2, 2] - r[0, 0] - r[1, 1]) * 2.0
|
||||
q = np.array([(r[0, 2] + r[2, 0]) / s,
|
||||
(r[1, 2] + r[2, 1]) / s, 0.25 * s,
|
||||
(r[1, 0] - r[0, 1]) / s])
|
||||
if q[3] < 0.0:
|
||||
q = -q
|
||||
return q / np.linalg.norm(q)
|
||||
|
||||
|
||||
def bracket(rows: list[dict[str, Any]], times: np.ndarray, t: int,
|
||||
max_ns: int) -> tuple[dict[str, Any], dict[str, Any], float] | None:
|
||||
right = int(np.searchsorted(times, t, side="left"))
|
||||
@@ -100,6 +108,10 @@ def circular_lerp_deg(a: float, b: float, u: float) -> float:
|
||||
return (a + u * delta) % 360.0
|
||||
|
||||
|
||||
def linear_lerp(a: float, b: float, u: float) -> float:
|
||||
return (1.0 - u) * a + u * b
|
||||
|
||||
|
||||
def iso_utc(ns: int) -> str:
|
||||
return dt.datetime.fromtimestamp(ns / 1e9, dt.timezone.utc).isoformat(timespec="microseconds")
|
||||
|
||||
@@ -128,22 +140,38 @@ def main() -> int:
|
||||
lidar = [row for row in csv.DictReader(f) if not row.get("error")]
|
||||
rtk = read_jsonl(a.rtk_jsonl)
|
||||
imu = [row for row in read_jsonl(a.imu_jsonl) if row.get("crc_valid")]
|
||||
gga = sorted([r for r in rtk if r.get("type") == "GGA" and r.get("checksum_valid")
|
||||
and r.get("lat_deg") is not None], key=lambda r: int(r["host_receive_utc_ns"]))
|
||||
heading = sorted([r for r in rtk if r.get("type") == "UNIHEADINGA" and r.get("checksum_valid")
|
||||
and r.get("heading_valid") and r.get("raw_heading_deg") is not None],
|
||||
key=lambda r: int(r["host_receive_utc_ns"]))
|
||||
if not lidar or len(gga) < 2 or len(heading) < 2:
|
||||
raise RuntimeError("insufficient LiDAR/GGA/heading data")
|
||||
positions = sorted(
|
||||
[
|
||||
r for r in rtk
|
||||
if r.get("type") in POSITION_TYPES
|
||||
and r.get("checksum_valid")
|
||||
and r.get("lat_deg") is not None
|
||||
],
|
||||
key=lambda r: int(r["host_receive_utc_ns"]),
|
||||
)
|
||||
heading = sorted(
|
||||
[r for r in rtk if r.get("type") in HEADING_TYPES and heading_row_valid(r)],
|
||||
key=lambda r: int(r["host_receive_utc_ns"]),
|
||||
)
|
||||
if not lidar or len(positions) < 2 or len(heading) < 2:
|
||||
raise RuntimeError("insufficient LiDAR/GGA|PVTSLNA/heading(GNHPR|UNIHEADINGA) data")
|
||||
|
||||
ext = json.loads(a.extrinsic.read_text(encoding="utf-8"))
|
||||
t_r_l = np.asarray(ext["matrix_4x4"], dtype=float)
|
||||
if t_r_l.shape != (4, 4):
|
||||
raise ValueError("extrinsic matrix_4x4 must be 4x4")
|
||||
heading_offset_deg = (
|
||||
float(a.heading_offset_deg)
|
||||
if a.heading_offset_deg is not None
|
||||
else float(ext.get("body_heading_offset_deg", 0.0) or 0.0)
|
||||
)
|
||||
|
||||
gga_times = np.asarray([int(r["host_receive_utc_ns"]) for r in gga], dtype=np.int64)
|
||||
position_times = np.asarray([int(r["host_receive_utc_ns"]) for r in positions], dtype=np.int64)
|
||||
heading_times = np.asarray([int(r["host_receive_utc_ns"]) for r in heading], dtype=np.int64)
|
||||
origin_row = next(r for r in gga if int(r.get("fix_quality", -1)) == 4)
|
||||
origin_row = next(
|
||||
(r for r in positions if int(r.get("fix_quality", -1)) in {4, 5}),
|
||||
positions[0],
|
||||
)
|
||||
origin_lat, origin_lon, origin_alt = (float(origin_row[k]) for k in ("lat_deg", "lon_deg", "altitude_m"))
|
||||
origin_ecef = geodetic_to_ecef(origin_lat, origin_lon, origin_alt)
|
||||
max_ns = int(a.max_bracket_ms * 1_000_000)
|
||||
@@ -151,7 +179,8 @@ def main() -> int:
|
||||
|
||||
for index, frame in enumerate(lidar):
|
||||
t = int(frame["unix_time_ns"])
|
||||
gb, hb = bracket(gga, gga_times, t, max_ns), bracket(heading, heading_times, t, max_ns)
|
||||
gb = bracket(positions, position_times, t, max_ns)
|
||||
hb = bracket(heading, heading_times, t, max_ns)
|
||||
reasons: list[str] = []
|
||||
available = gb is not None and hb is not None
|
||||
row: dict[str, Any] = {
|
||||
@@ -160,7 +189,7 @@ def main() -> int:
|
||||
"pose_available": int(available), "gt_valid": 0, "invalid_reason": "",
|
||||
}
|
||||
if not available:
|
||||
if gb is None: reasons.append("GGA_NOT_BRACKETED")
|
||||
if gb is None: reasons.append("POSITION_NOT_BRACKETED")
|
||||
if hb is None: reasons.append("HEADING_NOT_BRACKETED")
|
||||
row.update({k: "" for k in ("x_m", "y_m", "z_m", "qx", "qy", "qz", "qw",
|
||||
"rtk_x_m", "rtk_y_m", "rtk_z_m", "raw_heading_deg")})
|
||||
@@ -174,31 +203,45 @@ def main() -> int:
|
||||
p1 = geodetic_to_ecef(float(g1["lat_deg"]), float(g1["lon_deg"]), float(g1["altitude_m"]))
|
||||
p_rtk = ecef_to_enu((1.0 - gu) * p0 + gu * p1, origin_ecef, origin_lat, origin_lon)
|
||||
raw_heading = circular_lerp_deg(float(h0["raw_heading_deg"]), float(h1["raw_heading_deg"]), hu)
|
||||
yaw = math.radians(90.0 - raw_heading)
|
||||
corrected_heading, yaw = heading_to_enu_yaw(raw_heading, heading_offset_deg)
|
||||
if a.orientation_model == "heading_pitch_roll":
|
||||
pitch = linear_lerp(float(h0.get("pitch_deg") or 0.0), float(h1.get("pitch_deg") or 0.0), hu)
|
||||
roll = linear_lerp(float(h0.get("roll_deg") or 0.0), float(h1.get("roll_deg") or 0.0), hu)
|
||||
else:
|
||||
pitch = 0.0
|
||||
roll = 0.0
|
||||
t_w_r = np.eye(4)
|
||||
t_w_r[:3, :3] = yaw_matrix(yaw)
|
||||
t_w_r[:3, :3] = rtk_body_rotation(
|
||||
raw_heading, heading_offset_deg, pitch_deg=pitch, roll_deg=roll
|
||||
)
|
||||
t_w_r[:3, 3] = p_rtk
|
||||
t_w_l = t_w_r @ t_r_l
|
||||
q = matrix_to_quat_xyzw(t_w_l[:3, :3])
|
||||
q = rotation_to_quat_xyzw(t_w_l[:3, :3])
|
||||
|
||||
fix0, fix1 = int(g0.get("fix_quality", -1)), int(g1.get("fix_quality", -1))
|
||||
sol0, sol1 = str(h0.get("heading_solution", "")), str(h1.get("heading_solution", ""))
|
||||
std0 = float(h0.get("heading_stddev_deg") or math.inf)
|
||||
std1 = float(h1.get("heading_stddev_deg") or math.inf)
|
||||
if fix0 != 4 or fix1 != 4: reasons.append("RTK_POSITION_NOT_FIXED")
|
||||
if sol0 != "NARROW_INT" or sol1 != "NARROW_INT": reasons.append("HEADING_NOT_NARROW_INT")
|
||||
if max(std0, std1) > a.heading_std_limit_deg: reasons.append("HEADING_STD_EXCEEDED")
|
||||
if fix0 not in {4, 5} or fix1 not in {4, 5}:
|
||||
reasons.append("RTK_POSITION_NOT_FIXED")
|
||||
reasons.extend(heading_quality_ok(h0, a.heading_std_limit_deg))
|
||||
reasons.extend(heading_quality_ok(h1, a.heading_std_limit_deg))
|
||||
# Deduplicate while preserving order
|
||||
reasons = list(dict.fromkeys(reasons))
|
||||
row.update({
|
||||
"gt_valid": int(not reasons), "invalid_reason": ";".join(reasons),
|
||||
"x_m": t_w_l[0, 3], "y_m": t_w_l[1, 3], "z_m": t_w_l[2, 3],
|
||||
"qx": q[0], "qy": q[1], "qz": q[2], "qw": q[3],
|
||||
"rtk_x_m": p_rtk[0], "rtk_y_m": p_rtk[1], "rtk_z_m": p_rtk[2],
|
||||
"raw_heading_deg": raw_heading, "yaw_enu_deg": math.degrees(yaw),
|
||||
"gga_fix_before": fix0, "gga_fix_after": fix1,
|
||||
"heading_solution_before": sol0, "heading_solution_after": sol1,
|
||||
"heading_std_max_deg": max(std0, std1),
|
||||
"gga_before_dt_ms": (t - int(g0["host_receive_utc_ns"])) / 1e6,
|
||||
"gga_after_dt_ms": (int(g1["host_receive_utc_ns"]) - t) / 1e6,
|
||||
"raw_heading_deg": raw_heading,
|
||||
"corrected_heading_deg": corrected_heading,
|
||||
"heading_offset_deg": heading_offset_deg,
|
||||
"yaw_enu_deg": math.degrees(yaw),
|
||||
"pitch_deg": pitch,
|
||||
"roll_deg": roll,
|
||||
"position_fix_before": fix0, "position_fix_after": fix1,
|
||||
"heading_type_before": h0.get("type"), "heading_type_after": h1.get("type"),
|
||||
"heading_solution_before": h0.get("heading_solution"),
|
||||
"heading_solution_after": h1.get("heading_solution"),
|
||||
"position_before_dt_ms": (t - int(g0["host_receive_utc_ns"])) / 1e6,
|
||||
"position_after_dt_ms": (int(g1["host_receive_utc_ns"]) - t) / 1e6,
|
||||
"heading_before_dt_ms": (t - int(h0["host_receive_utc_ns"])) / 1e6,
|
||||
"heading_after_dt_ms": (int(h1["host_receive_utc_ns"]) - t) / 1e6,
|
||||
})
|
||||
@@ -213,9 +256,19 @@ def main() -> int:
|
||||
|
||||
summary = {
|
||||
"coordinate_convention": "T_W_L maps raw LiDAR points to local ENU; T_W_L = T_W_RTK @ T_RTK_lidar",
|
||||
"world_frame": "local ENU, origin is the first RTK FIX GGA sample",
|
||||
"rtk_frame": "x is rawHeading baseline direction projected horizontally, y left, z up",
|
||||
"orientation_model": "RTK pose is yaw-only; IMU orientation is not fused",
|
||||
"world_frame": "local ENU, origin is the first RTK FIX position sample",
|
||||
"rtk_frame": (
|
||||
"delivered body X follows rawHeading after heading_offset_deg; "
|
||||
"pitch/roll applied in baseline frame before the fixed offset"
|
||||
),
|
||||
"heading_offset_deg": heading_offset_deg,
|
||||
"heading_sources_accepted": sorted(HEADING_TYPES),
|
||||
"position_sources_accepted": sorted(POSITION_TYPES),
|
||||
"orientation_model": a.orientation_model,
|
||||
"orientation_composition": (
|
||||
"R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset)"
|
||||
),
|
||||
"orientation_note": "Uses dual-antenna GNHPR/UNIHEADINGA pitch/roll; IMU orientation is not fused",
|
||||
"time_basis": "LiDAR and serial host UTC; no jointly estimated clock offset/drift",
|
||||
"lidar_frames": len(pose_rows),
|
||||
"pose_available_frames": sum(int(r["pose_available"]) for r in pose_rows),
|
||||
@@ -223,7 +276,10 @@ def main() -> int:
|
||||
"gt_invalid_frames": sum(not int(r["gt_valid"]) for r in pose_rows),
|
||||
"imu_frames": len(imu),
|
||||
"enu_origin": {"lat_deg": origin_lat, "lon_deg": origin_lon, "altitude_m": origin_alt},
|
||||
"quality_rule": "GGA endpoints fix_quality=4, heading endpoints NARROW_INT, heading std <= limit, both streams bracket LiDAR time",
|
||||
"quality_rule": (
|
||||
"position endpoints fix_quality in {4,5}; UNIHEADINGA endpoints NARROW_INT with std gate; "
|
||||
"GNHPR endpoints heading_valid/quality 4|5; both streams bracket LiDAR time"
|
||||
),
|
||||
"heading_std_limit_deg": a.heading_std_limit_deg,
|
||||
"max_bracket_ms": a.max_bracket_ms,
|
||||
"warning": "gt_valid is a quality gate, not independent proof of +/-3 cm absolute accuracy",
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Prepare one static LiDAR frame and one yaw-only RTK reference pose per NPZ segment."""
|
||||
"""Prepare one static LiDAR frame and one RTK reference pose per NPZ segment."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -14,6 +14,13 @@ from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from rtk_attitude import (
|
||||
heading_to_enu_yaw,
|
||||
parse_pitch_roll_from_heading_raw,
|
||||
rotation_to_quat_xyzw,
|
||||
rtk_body_rotation,
|
||||
)
|
||||
|
||||
POSE_FIELDS = ["time", "x", "y", "z", "qx", "qy", "qz", "qw"]
|
||||
|
||||
|
||||
@@ -53,26 +60,30 @@ def ecef_to_enu(ecef: np.ndarray, origin: np.ndarray, lat_deg: float, lon_deg: f
|
||||
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 heading_to_enu_yaw(raw_heading_deg: float, heading_offset_deg: float) -> tuple[float, float]:
|
||||
"""Convert GNHPR navigation heading to mathematical ENU yaw.
|
||||
|
||||
``heading_offset_deg`` is added in the receiver's clockwise-from-north
|
||||
heading convention. It is therefore not interchangeable with a ROS yaw
|
||||
offset, whose sign and zero axis depend on the ROS frame definition.
|
||||
"""
|
||||
corrected_heading = (raw_heading_deg + heading_offset_deg) % 360.0
|
||||
return corrected_heading, math.radians(90.0 - corrected_heading)
|
||||
|
||||
|
||||
def scalar(data: np.lib.npyio.NpzFile, name: str) -> float:
|
||||
def scalar(data: np.lib.npyio.NpzFile, name: str, default: float | None = None) -> float:
|
||||
if name not in data.files:
|
||||
if default is None:
|
||||
raise KeyError(name)
|
||||
return float(default)
|
||||
return float(np.asarray(data[name]).reshape(-1)[0])
|
||||
|
||||
|
||||
def frame_pitch_roll(data: np.lib.npyio.NpzFile) -> tuple[float, float]:
|
||||
pitch = scalar(data, "rtk_pitch_deg", math.nan)
|
||||
roll = scalar(data, "rtk_roll_deg", math.nan)
|
||||
if math.isfinite(pitch) and math.isfinite(roll):
|
||||
return pitch, roll
|
||||
raw = None
|
||||
if "rtk_heading_raw_utf8" in data.files:
|
||||
raw = bytes(np.asarray(data["rtk_heading_raw_utf8"]).reshape(-1))
|
||||
parsed_pitch, parsed_roll = parse_pitch_roll_from_heading_raw(raw)
|
||||
if not math.isfinite(pitch):
|
||||
pitch = float(parsed_pitch) if parsed_pitch is not None else 0.0
|
||||
if not math.isfinite(roll):
|
||||
roll = float(parsed_roll) if parsed_roll is not None else 0.0
|
||||
return pitch, roll
|
||||
|
||||
|
||||
def parse_args() -> argparse.Namespace:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--combined-root", type=Path, required=True)
|
||||
@@ -84,6 +95,12 @@ def parse_args() -> argparse.Namespace:
|
||||
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(
|
||||
"--orientation-model",
|
||||
choices=("heading_pitch_roll", "yaw_only"),
|
||||
default="heading_pitch_roll",
|
||||
help="heading_pitch_roll uses GNHPR/UNIHEADINGA pitch+roll; yaw_only forces roll=pitch=0",
|
||||
)
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
return parser.parse_args()
|
||||
|
||||
@@ -113,9 +130,10 @@ def main() -> int:
|
||||
for row in good:
|
||||
path = args.combined_root / Path(row["output"])
|
||||
with np.load(path, allow_pickle=False) as data:
|
||||
pitch, roll = frame_pitch_roll(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")))
|
||||
pitch, roll, scalar(data, "rtk_heading_stddev_deg", math.nan)))
|
||||
values = np.asarray(samples, dtype=float)
|
||||
heading_std = circular_std_deg(values[:, 3])
|
||||
if heading_std > args.heading_std_limit_deg:
|
||||
@@ -123,16 +141,23 @@ def main() -> int:
|
||||
continue
|
||||
frame = good[len(good) // 2]
|
||||
source = args.combined_root / Path(frame["output"])
|
||||
reported_std = values[:, 5]
|
||||
reported_std = values[:, 6]
|
||||
reported_std_mean = float(np.nanmean(reported_std)) if np.isfinite(reported_std).any() else None
|
||||
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": reported_std_mean,
|
||||
"altitude_std_m": float(np.std(values[:, 2])), "selected_source": str(source)})
|
||||
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]),
|
||||
"pitch": float(np.mean(values[:, 4])), "roll": float(np.mean(values[:, 5])),
|
||||
})
|
||||
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])),
|
||||
"rtk_roll_mean_deg": float(np.mean(values[:, 5])),
|
||||
"reported_heading_std_mean_deg": reported_std_mean,
|
||||
"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}")
|
||||
@@ -145,6 +170,7 @@ def main() -> int:
|
||||
origin = selected[0]
|
||||
origin_ecef = geodetic_to_ecef(origin["lat"], origin["lon"], origin["alt"])
|
||||
lever = np.asarray(args.antenna_lever, dtype=float)
|
||||
use_attitude = args.orientation_model == "heading_pitch_roll"
|
||||
pose_rows = []
|
||||
for index, item in enumerate(selected, 1):
|
||||
destination = frames / f"station_{index:02d}.npz"
|
||||
@@ -152,32 +178,54 @@ def main() -> int:
|
||||
antenna = ecef_to_enu(geodetic_to_ecef(item["lat"], item["lon"], item["alt"]), origin_ecef,
|
||||
origin["lat"], origin["lon"])
|
||||
corrected_heading, yaw = heading_to_enu_yaw(item["heading"], args.heading_offset_deg)
|
||||
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})
|
||||
pitch = float(item["pitch"]) if use_attitude else 0.0
|
||||
roll = float(item["roll"]) if use_attitude else 0.0
|
||||
rotation = rtk_body_rotation(
|
||||
item["heading"], args.heading_offset_deg, pitch_deg=pitch, roll_deg=roll
|
||||
)
|
||||
reference_position = antenna - rotation @ lever
|
||||
quat = rotation_to_quat_xyzw(rotation)
|
||||
pose_rows.append(dict(zip(POSE_FIELDS, [item["time"], *reference_position, *quat])))
|
||||
summaries[index - 1].update({
|
||||
"sequence": index, "prepared_frame": destination.name,
|
||||
"corrected_heading_deg": corrected_heading,
|
||||
"pose_yaw_enu_deg": math.degrees(yaw),
|
||||
"pose_pitch_deg": pitch, "pose_roll_deg": roll,
|
||||
})
|
||||
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,
|
||||
"heading_offset_semantics": (
|
||||
"added to clockwise-from-north GNHPR heading before ENU yaw conversion"
|
||||
),
|
||||
"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)]}
|
||||
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,
|
||||
"heading_offset_semantics": (
|
||||
"added to clockwise-from-north GNHPR heading before ENU yaw conversion"
|
||||
),
|
||||
"orientation_model": args.orientation_model,
|
||||
"orientation_composition": (
|
||||
"R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset); "
|
||||
"yaw_raw from rawHeading, pitch/roll stay in baseline frame"
|
||||
),
|
||||
"pitch_roll_note": (
|
||||
"pitch/roll come from dual-antenna GNHPR/UNIHEADINGA (baseline elevation / reported roll). "
|
||||
"This is not a fused IMU vehicle attitude; G90 roll is often ~0."
|
||||
),
|
||||
},
|
||||
"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))
|
||||
"rejected": rejected, "pose_csv": pose_path.name,
|
||||
"orientation_model": args.orientation_model}, ensure_ascii=False, indent=2))
|
||||
return 0
|
||||
|
||||
|
||||
|
||||
@@ -120,6 +120,7 @@ def parse_heading(line: str) -> dict:
|
||||
"baseline_length_m": safe_float(fields[2]),
|
||||
"raw_heading_deg": raw_heading,
|
||||
"pitch_deg": safe_float(fields[4]),
|
||||
"roll_deg": 0.0,
|
||||
"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 "",
|
||||
|
||||
@@ -0,0 +1,127 @@
|
||||
#!/usr/bin/env python3
|
||||
"""RTK dual-antenna attitude helpers shared by prepare and SLAM delivery."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import re
|
||||
|
||||
import numpy as np
|
||||
|
||||
# GNHPR / UNIHEADINGA pitch is baseline elevation (far antenna higher ⇒ +pitch).
|
||||
# Build the baseline-frame attitude first, then apply the fixed body yaw offset:
|
||||
# R_W_body = Rz(yaw_raw) Ry(-pitch) Rx(roll) Rz(-heading_offset)
|
||||
# so pitch/roll stay about the physical baseline, even when delivering vehicle-forward.
|
||||
|
||||
|
||||
def heading_to_enu_yaw(raw_heading_deg: float, heading_offset_deg: float = 0.0) -> tuple[float, float]:
|
||||
"""Convert clockwise-from-north heading to mathematical ENU yaw (rad)."""
|
||||
corrected_heading = (raw_heading_deg + heading_offset_deg) % 360.0
|
||||
return corrected_heading, math.radians(90.0 - corrected_heading)
|
||||
|
||||
|
||||
def _rz(yaw_rad: float) -> np.ndarray:
|
||||
c, s = math.cos(yaw_rad), math.sin(yaw_rad)
|
||||
return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]], dtype=float)
|
||||
|
||||
|
||||
def _ry(pitch_rad: float) -> np.ndarray:
|
||||
c, s = math.cos(pitch_rad), math.sin(pitch_rad)
|
||||
return np.array([[c, 0.0, s], [0.0, 1.0, 0.0], [-s, 0.0, c]], dtype=float)
|
||||
|
||||
|
||||
def _rx(roll_rad: float) -> np.ndarray:
|
||||
c, s = math.cos(roll_rad), math.sin(roll_rad)
|
||||
return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]], dtype=float)
|
||||
|
||||
|
||||
def attitude_rotation(
|
||||
yaw_rad: float,
|
||||
pitch_deg: float = 0.0,
|
||||
roll_deg: float = 0.0,
|
||||
) -> np.ndarray:
|
||||
"""ENU←baseline rotation: Rz(yaw) Ry(-pitch) Rx(roll).
|
||||
|
||||
Positive ``pitch_deg`` elevates baseline X (slave higher than master).
|
||||
"""
|
||||
return _rz(float(yaw_rad)) @ _ry(-math.radians(float(pitch_deg))) @ _rx(math.radians(float(roll_deg)))
|
||||
|
||||
|
||||
def rtk_body_rotation(
|
||||
raw_heading_deg: float,
|
||||
heading_offset_deg: float = 0.0,
|
||||
pitch_deg: float = 0.0,
|
||||
roll_deg: float = 0.0,
|
||||
) -> np.ndarray:
|
||||
"""ENU←delivered RTK body frame.
|
||||
|
||||
Pitch/roll are applied in the raw baseline frame; ``heading_offset_deg`` then
|
||||
rotates that frame into the delivered body (0 = baseline X, -90 = vehicle
|
||||
forward when baseline points vehicle-right on this vehicle).
|
||||
"""
|
||||
_, yaw_baseline = heading_to_enu_yaw(raw_heading_deg, 0.0)
|
||||
return attitude_rotation(yaw_baseline, pitch_deg, roll_deg) @ _rz(-math.radians(float(heading_offset_deg)))
|
||||
|
||||
|
||||
def rotation_to_quat_xyzw(rotation: np.ndarray) -> np.ndarray:
|
||||
r = np.asarray(rotation, dtype=float)
|
||||
tr = float(np.trace(r))
|
||||
if tr > 0.0:
|
||||
s = math.sqrt(tr + 1.0) * 2.0
|
||||
q = np.array(
|
||||
[(r[2, 1] - r[1, 2]) / s, (r[0, 2] - r[2, 0]) / s, (r[1, 0] - r[0, 1]) / s, 0.25 * s],
|
||||
dtype=float,
|
||||
)
|
||||
else:
|
||||
i = int(np.argmax(np.diag(r)))
|
||||
if i == 0:
|
||||
s = math.sqrt(1.0 + r[0, 0] - r[1, 1] - r[2, 2]) * 2.0
|
||||
q = np.array(
|
||||
[0.25 * s, (r[0, 1] + r[1, 0]) / s, (r[0, 2] + r[2, 0]) / s, (r[2, 1] - r[1, 2]) / s],
|
||||
dtype=float,
|
||||
)
|
||||
elif i == 1:
|
||||
s = math.sqrt(1.0 + r[1, 1] - r[0, 0] - r[2, 2]) * 2.0
|
||||
q = np.array(
|
||||
[(r[0, 1] + r[1, 0]) / s, 0.25 * s, (r[1, 2] + r[2, 1]) / s, (r[0, 2] - r[2, 0]) / s],
|
||||
dtype=float,
|
||||
)
|
||||
else:
|
||||
s = math.sqrt(1.0 + r[2, 2] - r[0, 0] - r[1, 1]) * 2.0
|
||||
q = np.array(
|
||||
[(r[0, 2] + r[2, 0]) / s, (r[1, 2] + r[2, 1]) / s, 0.25 * s, (r[1, 0] - r[0, 1]) / s],
|
||||
dtype=float,
|
||||
)
|
||||
if q[3] < 0.0:
|
||||
q = -q
|
||||
return q / np.linalg.norm(q)
|
||||
|
||||
|
||||
def parse_pitch_roll_from_heading_raw(raw_utf8: bytes | str | None) -> tuple[float | None, float | None]:
|
||||
"""Best-effort pitch/roll from a stored GNHPR/UNIHEADINGA raw line."""
|
||||
if raw_utf8 is None:
|
||||
return None, None
|
||||
text = raw_utf8.decode("ascii", "ignore") if isinstance(raw_utf8, (bytes, bytearray)) else str(raw_utf8)
|
||||
text = text.strip()
|
||||
if "GNHPR" in text:
|
||||
parts = text.split(",")
|
||||
if len(parts) >= 5:
|
||||
try:
|
||||
return float(parts[3]), float(parts[4])
|
||||
except ValueError:
|
||||
return None, None
|
||||
if "UNIHEADINGA" in text.upper() or "HEADINGA" in text.upper():
|
||||
payload = text.split(";", 1)[-1]
|
||||
fields = payload.split(",")
|
||||
if len(fields) >= 5:
|
||||
try:
|
||||
return float(fields[4]), 0.0
|
||||
except ValueError:
|
||||
return None, None
|
||||
match = re.search(r",(-?\d+(?:\.\d+)?),(-?\d+(?:\.\d+)?),\d,", text)
|
||||
if match:
|
||||
try:
|
||||
return float(match.group(1)), float(match.group(2))
|
||||
except ValueError:
|
||||
return None, None
|
||||
return None, None
|
||||
Reference in New Issue
Block a user