新增独立RTK与IMU外参标定流程及质量验证

imu_lidar/rtk_attitude.py

@@ -0,0 +1,73 @@
+"""GNHPR attitude conventions and SO(3) interpolation."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+from scipy.spatial.transform import Rotation, Slerp
+
+@dataclass(frozen=True)
+class GnhprConvention:
+    """Interpretation of GNHPR angles as ``R_ENU_RTK``.
+
+    Heading is normally clockwise from north. With ENU and an x-forward RTK
+    frame this becomes yaw ``90 deg - heading``. Aircraft-positive pitch is
+    nose-up, which is the negative mathematical Y rotation in an FLU frame.
+    Alternative signs are retained for empirical protocol validation.
+    """
+
+    name: str
+    heading_sign: float = -1.0
+    pitch_sign: float = -1.0
+    roll_sign: float = 1.0
+
+
+EXPECTED_GNHPR = GnhprConvention("north_cw__pitch_nose_up__roll_right_down")
+GNHPR_CANDIDATES = (
+    EXPECTED_GNHPR,
+    GnhprConvention("north_cw__pitch_opposite", -1.0, 1.0, 1.0),
+    GnhprConvention("heading_opposite__pitch_nose_up", 1.0, -1.0, 1.0),
+    GnhprConvention("heading_opposite__pitch_opposite", 1.0, 1.0, 1.0),
+)
+
+
+def gnhpr_to_rotation_enu_rtk(
+    heading_deg: np.ndarray,
+    pitch_deg: np.ndarray,
+    roll_deg: np.ndarray,
+    convention: GnhprConvention = EXPECTED_GNHPR,
+) -> np.ndarray:
+    """Build body-to-ENU matrices with an extrinsic Z-Y-X Euler sequence."""
+
+    heading = np.asarray(heading_deg, dtype=float).reshape(-1)
+    pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
+    roll = np.asarray(roll_deg, dtype=float).reshape(-1)
+    if not (heading.size == pitch.size == roll.size):
+        raise ValueError("heading, pitch and roll must have equal length")
+    yaw_rad = np.deg2rad(90.0 + convention.heading_sign * heading)
+    pitch_rad = np.deg2rad(convention.pitch_sign * pitch)
+    roll_rad = np.deg2rad(convention.roll_sign * roll)
+    angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
+    return Rotation.from_euler('ZYX', angles).as_matrix()
+
+
+def interpolate_rotations(
+    source_t_s: np.ndarray,
+    rotations: np.ndarray,
+    query_t_s: np.ndarray,
+) -> np.ndarray:
+    """Slerp a monotonic SO(3) series without extrapolation."""
+
+    source_t = np.asarray(source_t_s, dtype=float).reshape(-1)
+    query_t = np.asarray(query_t_s, dtype=float).reshape(-1)
+    matrices = np.asarray(rotations, dtype=float).reshape(-1, 3, 3)
+    if source_t.size < 2 or matrices.shape[0] != source_t.size:
+        raise ValueError("need at least two timestamped rotations")
+    if np.any(np.diff(source_t) <= 0):
+        unique_t, unique_indices = np.unique(source_t, return_index=True)
+        source_t = unique_t
+        matrices = matrices[unique_indices]
+    if np.any(query_t < source_t[0]) or np.any(query_t > source_t[-1]):
+        raise ValueError("rotation interpolation does not extrapolate")
+    return Slerp(source_t, Rotation.from_matrix(matrices))(query_t).as_matrix()