440 lines
17 KiB
Python
440 lines
17 KiB
Python
"""Rotation and residual time-offset calibration between G90 RTK and HI13 IMU."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
|
|
import numpy as np
|
|
from scipy.optimize import least_squares
|
|
from scipy.sparse import lil_matrix
|
|
|
|
from .contracts import ImuSeries, MotionPair
|
|
from .geometry import orthonormalize_rotation, rpy_deg_xyz, so3_exp, so3_log
|
|
from .imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
|
|
from .rotation_handeye import estimate_rotation_handeye_initial
|
|
from .rtk_attitude import GNHPR_CANDIDATES, GnhprConvention, gnhpr_to_rotation_enu_rtk
|
|
from .rtk_io import RtkSeries
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class RotationSession:
|
|
session_id: str
|
|
batch_id: str
|
|
imu: ImuSeries
|
|
rtk: RtkSeries
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class TimeOffsetAudit:
|
|
offset_s: float
|
|
peak_correlation: float
|
|
second_best_correlation: float
|
|
evaluated_samples: int
|
|
reliable: bool
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class RotationCalibrationResult:
|
|
R_RTK_IMU: np.ndarray
|
|
rpy_deg: np.ndarray
|
|
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
|
|
time_offset: TimeOffsetAudit
|
|
applied_time_offset_s: float
|
|
convention: GnhprConvention
|
|
convention_scores_deg: dict[str, float]
|
|
pair_count: int
|
|
residual_rms_deg: float
|
|
residual_median_deg: float
|
|
residual_p95_deg: float
|
|
rotation_std_deg: np.ndarray
|
|
information_singular_values: np.ndarray
|
|
per_session_rms_deg: dict[str, float]
|
|
loo_delta_deg: dict[str, float]
|
|
ok: bool
|
|
notes: tuple[str, ...]
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class _Pair:
|
|
session_index: int
|
|
session_id: str
|
|
R_A: np.ndarray
|
|
delta_R_zero_bias: np.ndarray
|
|
J_bg: np.ndarray
|
|
weight: float
|
|
|
|
|
|
def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndarray, np.ndarray]:
|
|
valid = rtk.attitude_valid & rtk.position_valid
|
|
t = rtk.attitude_t_s[valid]
|
|
angles = np.column_stack(
|
|
[rtk.heading_deg[valid], rtk.pitch_deg[valid], rtk.roll_deg[valid]]
|
|
)
|
|
if t.size < 2:
|
|
raise ValueError(f"not enough valid RTK attitude rows: {rtk.source}")
|
|
# GGA is faster than HPR, so nearest-neighbour export repeats attitude rows.
|
|
# Keep only changes and place them at the first associated GGA measurement.
|
|
changed = np.ones(t.size, dtype=bool)
|
|
changed[1:] = np.any(np.abs(np.diff(angles, axis=0)) > 1e-10, axis=1)
|
|
t = t[changed]
|
|
angles = angles[changed]
|
|
order = np.argsort(t)
|
|
t = t[order]
|
|
angles = angles[order]
|
|
unique_t, unique_indices = np.unique(t, return_index=True)
|
|
rotations = gnhpr_to_rotation_enu_rtk(
|
|
angles[unique_indices, 0],
|
|
angles[unique_indices, 1],
|
|
angles[unique_indices, 2],
|
|
convention,
|
|
)
|
|
return unique_t, rotations
|
|
|
|
|
|
def _rtk_angular_speed(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
|
dt = np.diff(t)
|
|
valid = (dt >= 0.03) & (dt <= 0.5)
|
|
midpoint = 0.5 * (t[:-1] + t[1:])
|
|
speed = np.array(
|
|
[np.linalg.norm(so3_log(rotations[i].T @ rotations[i + 1])) for i in range(t.size - 1)]
|
|
) / np.maximum(dt, 1e-6)
|
|
return midpoint[valid], speed[valid]
|
|
|
|
|
|
def _correlation(a: np.ndarray, b: np.ndarray) -> float:
|
|
a = np.asarray(a, dtype=float)
|
|
b = np.asarray(b, dtype=float)
|
|
if a.size < 20 or np.std(a) < 1e-5 or np.std(b) < 1e-5:
|
|
return np.nan
|
|
return float(np.corrcoef(a, b)[0, 1])
|
|
|
|
|
|
def audit_time_offset(
|
|
sessions: list[RotationSession] | tuple[RotationSession, ...],
|
|
*,
|
|
search_half_width_s: float = 0.30,
|
|
step_s: float = 0.005,
|
|
) -> TimeOffsetAudit:
|
|
"""Estimate residual ``t_IMU - t_RTK`` from invariant angular-speed norms."""
|
|
|
|
offsets = np.arange(-search_half_width_s, search_half_width_s + 0.5 * step_s, step_s)
|
|
session_series: list[tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
|
|
total_samples = 0
|
|
for session in sessions:
|
|
t_rtk, rotations = _attitude_rows(session.rtk, GNHPR_CANDIDATES[0])
|
|
midpoint, rtk_speed = _rtk_angular_speed(t_rtk, rotations)
|
|
imu_speed = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
|
|
motion = rtk_speed > np.deg2rad(0.5)
|
|
midpoint = midpoint[motion]
|
|
rtk_speed = rtk_speed[motion]
|
|
if midpoint.size >= 20:
|
|
session_series.append((midpoint, rtk_speed, session.imu.t_s, imu_speed))
|
|
total_samples += int(midpoint.size)
|
|
if not session_series:
|
|
return TimeOffsetAudit(0.0, np.nan, np.nan, 0, False)
|
|
scores = []
|
|
for offset in offsets:
|
|
per_session = []
|
|
for midpoint, rtk_speed, imu_t, imu_speed in session_series:
|
|
query = midpoint + offset
|
|
inside = (query >= imu_t[0]) & (query <= imu_t[-1])
|
|
if np.count_nonzero(inside) < 20:
|
|
continue
|
|
interpolated = np.interp(query[inside], imu_t, imu_speed)
|
|
value = _correlation(rtk_speed[inside], interpolated)
|
|
if np.isfinite(value):
|
|
per_session.append(value)
|
|
scores.append(float(np.median(per_session)) if per_session else np.nan)
|
|
values = np.asarray(scores, dtype=float)
|
|
if not np.any(np.isfinite(values)):
|
|
return TimeOffsetAudit(0.0, np.nan, np.nan, total_samples, False)
|
|
best_index = int(np.nanargmax(values))
|
|
exclusion = np.abs(offsets - offsets[best_index]) >= 0.03
|
|
second = float(np.nanmax(values[exclusion])) if np.any(np.isfinite(values[exclusion])) else np.nan
|
|
peak = float(values[best_index])
|
|
reliable = bool(peak >= 0.35 and (not np.isfinite(second) or peak - second >= 0.015))
|
|
return TimeOffsetAudit(float(offsets[best_index]), peak, second, total_samples, reliable)
|
|
|
|
|
|
def _nearest_index(times: np.ndarray, target: float) -> int:
|
|
index = int(np.searchsorted(times, target))
|
|
candidates = [max(0, index - 1), min(times.size - 1, index)]
|
|
return min(candidates, key=lambda item: abs(float(times[item]) - target))
|
|
|
|
|
|
def _make_pairs(
|
|
sessions: list[RotationSession],
|
|
convention: GnhprConvention,
|
|
time_offset_s: float,
|
|
*,
|
|
anchor_step_s: float = 5.0,
|
|
intervals_s: tuple[float, ...] = (0.75, 1.5, 3.0),
|
|
preintegration_cache: dict[tuple[str, float, float], object] | None = None,
|
|
) -> list[_Pair]:
|
|
pairs: list[_Pair] = []
|
|
cache = {} if preintegration_cache is None else preintegration_cache
|
|
for session_index, session in enumerate(sessions):
|
|
t, rotations = _attitude_rows(session.rtk, convention)
|
|
next_anchor = float(t[0])
|
|
for i in range(t.size - 1):
|
|
if t[i] + 1e-9 < next_anchor:
|
|
continue
|
|
next_anchor = float(t[i] + anchor_step_s)
|
|
for duration in intervals_s:
|
|
j = _nearest_index(t, float(t[i] + duration))
|
|
if j <= i or abs(float(t[j] - t[i]) - duration) > 0.18:
|
|
continue
|
|
imu_t0 = float(t[i] + time_offset_s)
|
|
imu_t1 = float(t[j] + time_offset_s)
|
|
if imu_t0 < session.imu.t_s[0] or imu_t1 > session.imu.t_s[-1]:
|
|
continue
|
|
r_a = orthonormalize_rotation(rotations[i].T @ rotations[j])
|
|
cache_key = (session.session_id, round(imu_t0, 6), round(imu_t1, 6))
|
|
preint = cache.get(cache_key)
|
|
if preint is None:
|
|
preint = preintegrate_gyro(
|
|
session.imu.t_s,
|
|
session.imu.gyro_rad_s,
|
|
imu_t0,
|
|
imu_t1,
|
|
)
|
|
cache[cache_key] = preint
|
|
angle_a = np.linalg.norm(so3_log(r_a))
|
|
angle_b = np.linalg.norm(so3_log(preint.delta_R))
|
|
if min(angle_a, angle_b) < np.deg2rad(0.8):
|
|
continue
|
|
weight = float(np.clip(min(angle_a, angle_b) / np.deg2rad(5.0), 0.2, 3.0))
|
|
pairs.append(
|
|
_Pair(
|
|
session_index=session_index,
|
|
session_id=session.session_id,
|
|
R_A=r_a,
|
|
delta_R_zero_bias=preint.delta_R,
|
|
J_bg=preint.J_bg,
|
|
weight=weight,
|
|
)
|
|
)
|
|
return pairs
|
|
|
|
|
|
def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
|
|
if len(pairs) < 6:
|
|
raise ValueError("need at least 6 excited RTK--IMU rotation pairs")
|
|
generic = [
|
|
MotionPair(
|
|
session_id=pair.session_id,
|
|
i=index,
|
|
j=index + 1,
|
|
t_i_s=0.0,
|
|
t_j_s=1.0,
|
|
R_A=pair.R_A,
|
|
R_B=pair.delta_R_zero_bias,
|
|
metadata={"weight": pair.weight},
|
|
)
|
|
for index, pair in enumerate(pairs)
|
|
]
|
|
r0 = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
|
|
session_count = len(sessions)
|
|
|
|
def unpack(parameters: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
|
return orthonormalize_rotation(so3_exp(parameters[:3])), parameters[3:].reshape(session_count, 3)
|
|
|
|
def residual(parameters: np.ndarray) -> np.ndarray:
|
|
r_x, biases = unpack(parameters)
|
|
rows = []
|
|
for pair in pairs:
|
|
corrected = apply_bias_jacobian_correction(
|
|
pair.delta_R_zero_bias,
|
|
pair.J_bg,
|
|
biases[pair.session_index],
|
|
)
|
|
error = so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T)
|
|
rows.append(np.sqrt(pair.weight) * error)
|
|
# HI13 bias is session-specific; this weak prior only removes degenerate
|
|
# bias/extrinsic trades and is much looser than observed static bias.
|
|
rows.append((biases / 0.03).reshape(-1))
|
|
return np.concatenate(rows)
|
|
|
|
initial = np.concatenate([so3_log(r0), np.zeros(3 * session_count)])
|
|
jacobian_pattern = lil_matrix((3 * len(pairs) + 3 * session_count, initial.size), dtype=int)
|
|
for pair_index, pair in enumerate(pairs):
|
|
row = 3 * pair_index
|
|
jacobian_pattern[row : row + 3, 0:3] = 1
|
|
bias_col = 3 + 3 * pair.session_index
|
|
jacobian_pattern[row : row + 3, bias_col : bias_col + 3] = 1
|
|
prior_row = 3 * len(pairs)
|
|
jacobian_pattern[prior_row:, 3:] = 1
|
|
opt = least_squares(
|
|
residual,
|
|
initial,
|
|
loss="huber",
|
|
f_scale=np.deg2rad(0.5),
|
|
jac_sparsity=jacobian_pattern.tocsr(),
|
|
tr_solver='lsmr',
|
|
max_nfev=40,
|
|
)
|
|
r_x, biases = unpack(opt.x)
|
|
errors = []
|
|
for pair in pairs:
|
|
corrected = apply_bias_jacobian_correction(
|
|
pair.delta_R_zero_bias,
|
|
pair.J_bg,
|
|
biases[pair.session_index],
|
|
)
|
|
errors.append(np.degrees(np.linalg.norm(so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T))))
|
|
jacobian = opt.jac.toarray() if hasattr(opt.jac, 'toarray') else np.asarray(opt.jac, dtype=float)
|
|
information = jacobian.T @ jacobian
|
|
dof = max(residual(opt.x).size - opt.x.size, 1)
|
|
variance = float(np.sum(residual(opt.x) ** 2) / dof)
|
|
covariance = np.linalg.pinv(information, rcond=1e-10) * variance
|
|
return r_x, biases, np.asarray(errors), covariance
|
|
|
|
|
|
def solve_rtk_imu_rotation(
|
|
sessions: list[RotationSession] | tuple[RotationSession, ...],
|
|
*,
|
|
compute_loo: bool = True,
|
|
) -> RotationCalibrationResult:
|
|
"""Solve shared ``R_RTK_IMU`` and per-session gyro biases."""
|
|
|
|
items = list(sessions)
|
|
if not items:
|
|
raise ValueError("at least one RTK--IMU session is required")
|
|
time_audit = audit_time_offset(items)
|
|
offset = time_audit.offset_s if time_audit.reliable else 0.0
|
|
candidates: list[tuple[GnhprConvention, list[_Pair]]] = []
|
|
scores: dict[str, float] = {}
|
|
preintegration_cache: dict[tuple[str, float, float], object] = {}
|
|
for convention in GNHPR_CANDIDATES:
|
|
pairs = _make_pairs(items, convention, offset, preintegration_cache=preintegration_cache)
|
|
if len(pairs) < 6:
|
|
scores[convention.name] = 1e9
|
|
continue
|
|
generic = [
|
|
MotionPair(
|
|
session_id=pair.session_id,
|
|
i=index,
|
|
j=index + 1,
|
|
t_i_s=0.0,
|
|
t_j_s=1.0,
|
|
R_A=pair.R_A,
|
|
R_B=pair.delta_R_zero_bias,
|
|
metadata={'weight': pair.weight},
|
|
)
|
|
for index, pair in enumerate(pairs)
|
|
]
|
|
initial_rotation = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
|
|
preliminary_errors = np.asarray(
|
|
[
|
|
np.degrees(
|
|
np.linalg.norm(
|
|
so3_log(
|
|
initial_rotation.T
|
|
@ pair.R_A
|
|
@ initial_rotation
|
|
@ pair.delta_R_zero_bias.T
|
|
)
|
|
)
|
|
)
|
|
for pair in pairs
|
|
]
|
|
)
|
|
scores[convention.name] = float(np.sqrt(np.mean(preliminary_errors**2)))
|
|
candidates.append((convention, pairs))
|
|
if not candidates:
|
|
raise ValueError("no GNHPR convention produced enough rotation pairs")
|
|
convention, pairs = min(candidates, key=lambda item: scores[item[0].name])
|
|
rotation, biases, errors, covariance = _solve_core(items, pairs)
|
|
per_session = {}
|
|
for session in items:
|
|
values = [error for pair, error in zip(pairs, errors) if pair.session_id == session.session_id]
|
|
per_session[session.session_id] = (
|
|
float(np.sqrt(np.mean(np.asarray(values) ** 2))) if values else np.nan
|
|
)
|
|
loo = {}
|
|
if compute_loo and len(items) >= 3:
|
|
for omitted in items:
|
|
kept_pairs = [
|
|
pair
|
|
for pair in pairs
|
|
if pair.session_id != omitted.session_id
|
|
]
|
|
if len(kept_pairs) < 6:
|
|
loo[omitted.session_id] = np.nan
|
|
continue
|
|
def loo_residual(rotvec: np.ndarray) -> np.ndarray:
|
|
candidate = orthonormalize_rotation(so3_exp(rotvec))
|
|
rows = []
|
|
for pair in kept_pairs:
|
|
corrected = apply_bias_jacobian_correction(
|
|
pair.delta_R_zero_bias,
|
|
pair.J_bg,
|
|
biases[pair.session_index],
|
|
)
|
|
rows.append(
|
|
np.sqrt(pair.weight)
|
|
* so3_log(candidate.T @ pair.R_A @ candidate @ corrected.T)
|
|
)
|
|
return np.concatenate(rows)
|
|
|
|
loo_opt = least_squares(
|
|
loo_residual,
|
|
so3_log(rotation),
|
|
loss='huber',
|
|
f_scale=np.deg2rad(0.5),
|
|
max_nfev=30,
|
|
)
|
|
loo_rotation = orthonormalize_rotation(so3_exp(loo_opt.x))
|
|
loo[omitted.session_id] = float(np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation))))
|
|
rotation_cov = covariance[:3, :3]
|
|
std_deg = np.degrees(np.sqrt(np.maximum(np.diag(rotation_cov), 0.0)))
|
|
singular_values = np.linalg.svd(np.linalg.pinv(rotation_cov, rcond=1e-12), compute_uv=False)
|
|
rms = float(np.sqrt(np.mean(errors**2)))
|
|
median = float(np.median(errors))
|
|
p95 = float(np.percentile(errors, 95.0))
|
|
finite_loo = [value for value in loo.values() if np.isfinite(value)]
|
|
sorted_scores = sorted(scores.values())
|
|
convention_gap = sorted_scores[1] - sorted_scores[0] if len(sorted_scores) > 1 else np.inf
|
|
ok = bool(
|
|
len(pairs) >= 20
|
|
and rms <= 1.0
|
|
and p95 <= 2.0
|
|
and float(np.max(std_deg)) <= 0.5
|
|
and (not finite_loo or max(finite_loo) <= 1.0)
|
|
and convention_gap >= 0.05
|
|
)
|
|
notes = [
|
|
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
|
f"residual time convention: t_IMU = t_RTK + {offset:+.6f} s",
|
|
f"GNHPR convention score gap={convention_gap:.4f} deg",
|
|
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
|
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
|
|
]
|
|
if not time_audit.reliable:
|
|
notes.append("time-offset correlation was ambiguous; held residual offset at zero")
|
|
if convention is not GNHPR_CANDIDATES[0]:
|
|
notes.append("empirical best GNHPR convention differs from protocol expectation; manual verification required")
|
|
if not ok:
|
|
notes.append("rotation failed one or more strict acceptance gates")
|
|
return RotationCalibrationResult(
|
|
R_RTK_IMU=rotation,
|
|
rpy_deg=rpy_deg_xyz(rotation),
|
|
gyro_bias_by_session_rad_s={
|
|
session.session_id: biases[index].copy() for index, session in enumerate(items)
|
|
},
|
|
time_offset=time_audit,
|
|
applied_time_offset_s=offset,
|
|
convention=convention,
|
|
convention_scores_deg=scores,
|
|
pair_count=len(pairs),
|
|
residual_rms_deg=rms,
|
|
residual_median_deg=median,
|
|
residual_p95_deg=p95,
|
|
rotation_std_deg=std_deg,
|
|
information_singular_values=singular_values,
|
|
per_session_rms_deg=per_session,
|
|
loo_delta_deg=loo,
|
|
ok=ok,
|
|
notes=tuple(notes),
|
|
)
|