迁移RTK-IMU标定到独立顶层包
This commit is contained in:
lichun.qu
@@ -0,0 +1,688 @@
|
||||
"""Rotation and residual time-offset calibration between G90 RTK and HI13 IMU."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, replace
|
||||
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.sparse import lil_matrix
|
||||
|
||||
from imu_lidar.contracts import ImuSeries, MotionPair
|
||||
from imu_lidar.geometry import orthonormalize_rotation, rpy_deg_xyz, so3_exp, so3_log
|
||||
from imu_lidar.imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
|
||||
from imu_lidar.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
|
||||
method: str
|
||||
peak_width_s: tuple[float, float]
|
||||
per_session_offset_s: dict[str, float]
|
||||
per_session_peak_correlation: dict[str, float]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class BaselineConsistencyAudit:
|
||||
"""Two-DOF audit using only the physically observed ANT1-to-ANT2 axis."""
|
||||
|
||||
baseline_axis_imu: np.ndarray
|
||||
pair_count: int
|
||||
residual_rms_deg: float
|
||||
residual_median_deg: float
|
||||
residual_p95_deg: float
|
||||
per_session_rms_deg: dict[str, float]
|
||||
per_session_p95_deg: dict[str, float]
|
||||
per_session_axis_rms_deg: dict[str, np.ndarray]
|
||||
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
|
||||
worst_pairs: tuple[dict[str, object], ...]
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
|
||||
@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]
|
||||
baseline_consistency: BaselineConsistencyAudit
|
||||
observable_rotation_dof: int
|
||||
full_attitude_observable: bool
|
||||
legacy_full_attitude_numeric_ok: bool
|
||||
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
|
||||
t0_s: float
|
||||
t1_s: 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_heading_rate(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""Return signed vehicle yaw rate from the ANT1-to-ANT2 azimuth.
|
||||
|
||||
ANT1-to-ANT2 points vehicle-right, so its clockwise heading increases when
|
||||
mathematical body yaw decreases. Only this signed heading channel is
|
||||
compared with IMU gyro_z; rotation about the baseline is unobservable.
|
||||
"""
|
||||
|
||||
dt = np.diff(t)
|
||||
baseline = rotations[:, :, 0]
|
||||
heading = np.unwrap(np.arctan2(baseline[:, 0], baseline[:, 1]))
|
||||
rate = -np.diff(heading) / np.maximum(dt, 1e-6)
|
||||
valid = (
|
||||
(dt >= 0.03)
|
||||
& (dt <= 0.25)
|
||||
& (np.abs(rate) >= np.deg2rad(0.5))
|
||||
& (np.abs(rate) <= np.deg2rad(30.0))
|
||||
)
|
||||
return 0.5 * (t[:-1] + t[1:])[valid], rate[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:
|
||||
"""Audit residual t_IMU - t_RTK from signed heading rate.
|
||||
|
||||
A broad correlation peak remains diagnostic. It is never applied unless
|
||||
both peak separation and peak width pass.
|
||||
"""
|
||||
|
||||
offsets = np.arange(-search_half_width_s, search_half_width_s + 0.5 * step_s, step_s)
|
||||
session_series: list[tuple[str, 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_rate = _rtk_heading_rate(t_rtk, rotations)
|
||||
imu_rate = session.imu.gyro_rad_s[:, 2]
|
||||
if midpoint.size >= 20:
|
||||
session_series.append(
|
||||
(session.session_id, midpoint, rtk_rate, session.imu.t_s, imu_rate)
|
||||
)
|
||||
total_samples += int(midpoint.size)
|
||||
if not session_series:
|
||||
return TimeOffsetAudit(
|
||||
0.0,
|
||||
np.nan,
|
||||
np.nan,
|
||||
0,
|
||||
False,
|
||||
"signed_heading_rate_vs_imu_gyro_z",
|
||||
(np.nan, np.nan),
|
||||
{},
|
||||
{},
|
||||
)
|
||||
scores = []
|
||||
per_session_scores: dict[str, list[float]] = {
|
||||
session_id: [] for session_id, *_ in session_series
|
||||
}
|
||||
for offset in offsets:
|
||||
per_session = []
|
||||
for session_id, midpoint, rtk_rate, imu_t, imu_rate in session_series:
|
||||
query = midpoint + offset
|
||||
inside = (query >= imu_t[0]) & (query <= imu_t[-1])
|
||||
if np.count_nonzero(inside) < 20:
|
||||
per_session_scores[session_id].append(np.nan)
|
||||
continue
|
||||
interpolated = np.interp(query[inside], imu_t, imu_rate)
|
||||
value = _correlation(rtk_rate[inside], interpolated)
|
||||
per_session_scores[session_id].append(value)
|
||||
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,
|
||||
"signed_heading_rate_vs_imu_gyro_z",
|
||||
(np.nan, np.nan),
|
||||
{},
|
||||
{},
|
||||
)
|
||||
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])
|
||||
near_peak = np.flatnonzero(values >= peak - 0.005)
|
||||
peak_width = (
|
||||
(float(offsets[near_peak[0]]), float(offsets[near_peak[-1]]))
|
||||
if near_peak.size
|
||||
else (np.nan, np.nan)
|
||||
)
|
||||
per_session_offset = {}
|
||||
per_session_peak = {}
|
||||
for session_id, session_values in per_session_scores.items():
|
||||
array = np.asarray(session_values, dtype=float)
|
||||
if np.any(np.isfinite(array)):
|
||||
index = int(np.nanargmax(array))
|
||||
per_session_offset[session_id] = float(offsets[index])
|
||||
per_session_peak[session_id] = float(array[index])
|
||||
reliable = bool(
|
||||
peak >= 0.5
|
||||
and (not np.isfinite(second) or peak - second >= 0.015)
|
||||
and np.isfinite(peak_width[0])
|
||||
and peak_width[1] - peak_width[0] <= 0.03
|
||||
)
|
||||
return TimeOffsetAudit(
|
||||
float(offsets[best_index]),
|
||||
peak,
|
||||
second,
|
||||
total_samples,
|
||||
reliable,
|
||||
"signed_heading_rate_vs_imu_gyro_z",
|
||||
peak_width,
|
||||
per_session_offset,
|
||||
per_session_peak,
|
||||
)
|
||||
|
||||
|
||||
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)
|
||||
dt = np.diff(t)
|
||||
baseline = rotations[:, :, 0]
|
||||
baseline_step = np.arccos(
|
||||
np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0)
|
||||
)
|
||||
broken_edge = (
|
||||
(dt < 0.03)
|
||||
| (dt > 0.25)
|
||||
| (baseline_step / np.maximum(dt, 1e-6) > np.deg2rad(45.0))
|
||||
)
|
||||
broken_prefix = np.concatenate([[0], np.cumsum(broken_edge.astype(int))])
|
||||
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
|
||||
if broken_prefix[j] - broken_prefix[i] != 0:
|
||||
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,
|
||||
t0_s=float(t[i]),
|
||||
t1_s=float(t[j]),
|
||||
)
|
||||
)
|
||||
# Equalize total influence per session. Pair count and excitation otherwise
|
||||
# let long/high-motion sessions dominate the shared rotation.
|
||||
totals = {
|
||||
session.session_id: sum(
|
||||
pair.weight for pair in pairs if pair.session_id == session.session_id
|
||||
)
|
||||
for session in sessions
|
||||
}
|
||||
nonzero = [value for value in totals.values() if value > 0.0]
|
||||
target = float(np.mean(nonzero)) if nonzero else 1.0
|
||||
return [
|
||||
replace(pair, weight=pair.weight * target / totals[pair.session_id])
|
||||
for pair in pairs
|
||||
if totals[pair.session_id] > 0.0
|
||||
]
|
||||
|
||||
|
||||
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_baseline_consistency(
|
||||
sessions: list[RotationSession],
|
||||
pairs: list[_Pair],
|
||||
) -> BaselineConsistencyAudit:
|
||||
"""Audit the two physically observable dual-antenna rotation DOFs.
|
||||
|
||||
The confirmed ANT1-to-ANT2 axis is IMU +X. For every interval, the angle
|
||||
swept by the GNSS baseline must equal the angle swept by IMU +X under gyro
|
||||
preintegration. Rotation about +X cancels from this invariant and is not
|
||||
falsely scored as an RTK attitude residual.
|
||||
"""
|
||||
|
||||
baseline_axis = np.array([1.0, 0.0, 0.0])
|
||||
session_count = len(sessions)
|
||||
|
||||
def raw_errors(parameters: np.ndarray) -> np.ndarray:
|
||||
biases = parameters.reshape(session_count, 3)
|
||||
values = []
|
||||
for pair in pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R_zero_bias,
|
||||
pair.J_bg,
|
||||
biases[pair.session_index],
|
||||
)
|
||||
observed = np.arccos(np.clip(pair.R_A[0, 0], -1.0, 1.0))
|
||||
predicted = np.arccos(
|
||||
np.clip(baseline_axis @ corrected @ baseline_axis, -1.0, 1.0)
|
||||
)
|
||||
values.append(predicted - observed)
|
||||
return np.asarray(values)
|
||||
|
||||
def residual(parameters: np.ndarray) -> np.ndarray:
|
||||
errors = raw_errors(parameters)
|
||||
weighted = errors * np.sqrt(np.asarray([pair.weight for pair in pairs]))
|
||||
return np.concatenate([weighted, parameters / 0.003])
|
||||
|
||||
initial = np.zeros(3 * session_count)
|
||||
opt = least_squares(
|
||||
residual,
|
||||
initial,
|
||||
loss="huber",
|
||||
f_scale=np.deg2rad(0.25),
|
||||
max_nfev=60,
|
||||
)
|
||||
biases = opt.x.reshape(session_count, 3)
|
||||
errors_deg = np.degrees(raw_errors(opt.x))
|
||||
per_session_rms = {}
|
||||
per_session_p95 = {}
|
||||
per_session_axis_rms = {}
|
||||
for session in sessions:
|
||||
selection = np.asarray(
|
||||
[pair.session_id == session.session_id for pair in pairs], dtype=bool
|
||||
)
|
||||
values = errors_deg[selection]
|
||||
per_session_rms[session.session_id] = (
|
||||
float(np.sqrt(np.mean(values**2))) if values.size else np.nan
|
||||
)
|
||||
per_session_p95[session.session_id] = (
|
||||
float(np.percentile(np.abs(values), 95.0)) if values.size else np.nan
|
||||
)
|
||||
axis_errors = []
|
||||
for pair in np.asarray(pairs, dtype=object)[selection]:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R_zero_bias,
|
||||
pair.J_bg,
|
||||
biases[pair.session_index],
|
||||
)
|
||||
axis_errors.append(np.degrees(so3_log(pair.R_A @ corrected.T)))
|
||||
per_session_axis_rms[session.session_id] = (
|
||||
np.sqrt(np.mean(np.asarray(axis_errors) ** 2, axis=0))
|
||||
if axis_errors
|
||||
else np.full(3, np.nan)
|
||||
)
|
||||
worst_indices = np.argsort(np.abs(errors_deg))[-20:][::-1]
|
||||
worst_pairs = tuple(
|
||||
{
|
||||
"session_id": pairs[index].session_id,
|
||||
"t0_s": pairs[index].t0_s,
|
||||
"t1_s": pairs[index].t1_s,
|
||||
"duration_s": pairs[index].t1_s - pairs[index].t0_s,
|
||||
"baseline_angle_residual_deg": float(errors_deg[index]),
|
||||
}
|
||||
for index in worst_indices
|
||||
)
|
||||
rms = float(np.sqrt(np.mean(errors_deg**2)))
|
||||
median = float(np.median(np.abs(errors_deg)))
|
||||
p95 = float(np.percentile(np.abs(errors_deg), 95.0))
|
||||
finite_session_rms = [
|
||||
value for value in per_session_rms.values() if np.isfinite(value)
|
||||
]
|
||||
finite_session_p95 = [
|
||||
value for value in per_session_p95.values() if np.isfinite(value)
|
||||
]
|
||||
ok = bool(
|
||||
len(pairs) >= 20
|
||||
and rms <= 1.0
|
||||
and p95 <= 2.0
|
||||
and (not finite_session_rms or max(finite_session_rms) <= 1.5)
|
||||
and (not finite_session_p95 or max(finite_session_p95) <= 3.0)
|
||||
)
|
||||
return BaselineConsistencyAudit(
|
||||
baseline_axis_imu=baseline_axis,
|
||||
pair_count=len(pairs),
|
||||
residual_rms_deg=rms,
|
||||
residual_median_deg=median,
|
||||
residual_p95_deg=p95,
|
||||
per_session_rms_deg=per_session_rms,
|
||||
per_session_p95_deg=per_session_p95,
|
||||
per_session_axis_rms_deg=per_session_axis_rms,
|
||||
gyro_bias_by_session_rad_s={
|
||||
session.session_id: biases[index].copy()
|
||||
for index, session in enumerate(sessions)
|
||||
},
|
||||
worst_pairs=worst_pairs,
|
||||
ok=ok,
|
||||
notes=(
|
||||
"ANT1(main,left)->ANT2(secondary,right) is fixed to IMU +X",
|
||||
"axis residual XYZ labels are baseline-spin(unobservable), baseline-elevation, heading",
|
||||
"full rotation about the baseline is not identifiable from two antennas",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
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
|
||||
)
|
||||
baseline_audit = _solve_baseline_consistency(items, pairs)
|
||||
loo = {}
|
||||
if compute_loo and len(items) >= 3:
|
||||
for omitted in items:
|
||||
kept_items = [item for item in items if item.session_id != omitted.session_id]
|
||||
kept_index = {item.session_id: index for index, item in enumerate(kept_items)}
|
||||
kept_pairs = [
|
||||
replace(pair, session_index=kept_index[pair.session_id])
|
||||
for pair in pairs
|
||||
if pair.session_id != omitted.session_id
|
||||
]
|
||||
if len(kept_pairs) < 6:
|
||||
loo[omitted.session_id] = np.nan
|
||||
continue
|
||||
try:
|
||||
loo_rotation, _, _, _ = _solve_core(kept_items, kept_pairs)
|
||||
except ValueError:
|
||||
loo[omitted.session_id] = np.nan
|
||||
continue
|
||||
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
|
||||
legacy_numeric_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
|
||||
)
|
||||
# GNHPR supplies the ANT1-to-ANT2 direction but no independent rotation
|
||||
# about that direction. A completed 3-D attitude is useful diagnostically,
|
||||
# but cannot pass the full extrinsic-rotation gate from this dataset alone.
|
||||
full_attitude_observable = False
|
||||
ok = False
|
||||
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 re-optimizes the remaining per-session gyro biases",
|
||||
"legacy full-HPR rotation uses a zero-roll gauge completion and is diagnostic only",
|
||||
"dual antennas do not observe rotation about the ANT1-to-ANT2 baseline",
|
||||
]
|
||||
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 baseline_audit.ok:
|
||||
notes.append("the physically observable baseline consistency failed strict gates")
|
||||
if not legacy_numeric_ok:
|
||||
notes.append("the legacy gauge-completed rotation failed one or more numeric gates")
|
||||
notes.append("full rotation is not accepted; translation must remain frozen")
|
||||
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,
|
||||
baseline_consistency=baseline_audit,
|
||||
observable_rotation_dof=2,
|
||||
full_attitude_observable=full_attitude_observable,
|
||||
legacy_full_attitude_numeric_ok=legacy_numeric_ok,
|
||||
ok=ok,
|
||||
notes=tuple(notes),
|
||||
)
|
||||
Reference in New Issue
Block a user