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/build_slam_delivery.py
+119 -63
View File
@@ -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",