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

修正基线系标定默认：机械初值、地面ROI与航向偏移可配，并补充G90窗导出与契约测试

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-10 22:28:25 +08:00
co-authored by Cursor
parent 69bb44bccd
commit 46d2fa1d69
14 changed files with 932 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
tools/prepare_multisensor_station_dataset.py
+18 -3
View File
@@ -58,6 +58,17 @@ def yaw_rotation(yaw: float) -> np.ndarray:
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:
return float(np.asarray(data[name]).reshape(-1)[0])
@@ -112,13 +123,15 @@ def main() -> int:
continue
frame = good[len(good) // 2]
source = args.combined_root / Path(frame["output"])
reported_std = values[:, 5]
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": float(np.nanmean(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:
@@ -138,8 +151,7 @@ def main() -> int:
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)
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)])))
@@ -156,6 +168,9 @@ def main() -> int:
"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"}