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

改为车头向前整链：主从装反机械初值、双天线 pitch/roll 姿态与默认 HeadingOffsetDeg=-90

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-11 18:07:54 +08:00
co-authored by Cursor
parent 6242fd1081
commit 5f59bcd795
13 changed files with 698 additions and 199 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/prepare_multisensor_station_dataset.py
+94 -46
View File
@@ -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