迁移RTK-IMU标定到独立顶层包
This commit is contained in:
lichun.qu
@@ -0,0 +1,11 @@
|
||||
# RTK-IMU Calibration Package
|
||||
|
||||
`rtk_imu/` contains the independent RTK-IMU calibration pipeline: G90 observation I/O, antenna-baseline rotation, V1 replay, V3 multisource stages, engineering 6DoF, and the node-state factor graph.
|
||||
|
||||
## Boundary
|
||||
|
||||
This package may import only shared `imu_lidar` utilities: `contracts`, `geometry`, `geodesy`, `imu_io`, `imu_preintegration`, and the generic `rotation_handeye` initializer.
|
||||
|
||||
It must not import LiDAR-specific modules such as `lidar_io`, `lidar_deskew`, `registration`, `pipeline`, `phase_a`, or `joint_optimizer`. The LiDAR-IMU pipeline remains in `imu_lidar/`.
|
||||
|
||||
## Entrypoints
|
||||
@@ -0,0 +1 @@
|
||||
"""RTK-IMU extrinsic calibration algorithms and data interfaces."""
|
||||
@@ -0,0 +1,105 @@
|
||||
"""GNHPR dual-antenna baseline 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 the GNHPR ANT1-to-ANT2 baseline.
|
||||
|
||||
Heading is clockwise from north. In this vehicle the RTK ``+X`` axis is
|
||||
the baseline from the main/left antenna (ANT1) to the secondary/right
|
||||
antenna (ANT2), so it points to vehicle right rather than vehicle forward.
|
||||
GNHPR pitch is the elevation of that baseline. A two-antenna receiver
|
||||
cannot observe rotation about the baseline; the reported roll is therefore
|
||||
not treated as a third independent attitude measurement.
|
||||
"""
|
||||
|
||||
name: str
|
||||
heading_sign: float = -1.0
|
||||
pitch_sign: float = -1.0
|
||||
roll_sign: float = 1.0
|
||||
|
||||
|
||||
EXPECTED_GNHPR = GnhprConvention("ant1_to_ant2__north_cw__elevation_up")
|
||||
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 a zero-roll mathematical completion of the baseline frame.
|
||||
|
||||
Only the first column (the ANT1-to-ANT2 unit vector) is physically observed
|
||||
by GNHPR. The remaining columns are a convenient gauge completion and must
|
||||
not be used as a measured full vehicle attitude.
|
||||
"""
|
||||
|
||||
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)
|
||||
# A dual-antenna baseline has no independent roll observation. Keep the
|
||||
# argument for wire-format compatibility, but never inject it into SO(3).
|
||||
roll_rad = np.zeros_like(roll)
|
||||
angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
|
||||
return Rotation.from_euler('ZYX', angles).as_matrix()
|
||||
|
||||
|
||||
def gnhpr_to_baseline_enu(
|
||||
heading_deg: np.ndarray,
|
||||
pitch_deg: np.ndarray,
|
||||
convention: GnhprConvention = EXPECTED_GNHPR,
|
||||
) -> np.ndarray:
|
||||
"""Return ANT1-to-ANT2 unit vectors expressed in ENU.
|
||||
|
||||
The result is independent of the unobservable GNHPR roll field.
|
||||
"""
|
||||
|
||||
heading = np.asarray(heading_deg, dtype=float).reshape(-1)
|
||||
pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
|
||||
if heading.size != pitch.size:
|
||||
raise ValueError("heading and pitch must have equal length")
|
||||
azimuth = np.deg2rad(heading)
|
||||
elevation = np.deg2rad(-convention.pitch_sign * pitch)
|
||||
horizontal = np.cos(elevation)
|
||||
east = np.sin(azimuth) * horizontal
|
||||
north = np.cos(azimuth) * horizontal
|
||||
up = np.sin(elevation)
|
||||
return np.column_stack([east, north, up])
|
||||
|
||||
|
||||
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()
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,558 @@
|
||||
"""Observable RTK--IMU rotation stages using native asynchronous measurements."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
from dataclasses import asdict, dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
from imu_lidar.contracts import ImuSeries
|
||||
from imu_lidar.geometry import so3_exp, so3_log
|
||||
from imu_lidar.imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
|
||||
from .rtk_attitude import gnhpr_to_baseline_enu
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class UnifiedSession:
|
||||
session_id: str
|
||||
batch_id: str
|
||||
imu: ImuSeries
|
||||
imu_rpy_deg: np.ndarray
|
||||
imu_quaternion_wxyz: np.ndarray
|
||||
imu_host_receive_utc_s: np.ndarray
|
||||
rtk_by_type: dict[str, list[dict[str, str]]]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class R1bResult:
|
||||
baseline_axis_imu: np.ndarray
|
||||
tilt_yz_deg: np.ndarray
|
||||
pair_count: int
|
||||
residual_rms_deg: float
|
||||
residual_p95_deg: float
|
||||
covariance_deg2: np.ndarray
|
||||
std_deg: np.ndarray
|
||||
information_singular_values: np.ndarray
|
||||
per_session_rms_deg: dict[str, float]
|
||||
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CompletedRotationResult:
|
||||
method: str
|
||||
R_RTK_IMU: np.ndarray | None
|
||||
rpy_deg: np.ndarray | None
|
||||
sample_count: int
|
||||
session_count: int
|
||||
residual_rms_deg: float
|
||||
residual_p95_deg: float
|
||||
covariance_deg2: np.ndarray
|
||||
std_deg: np.ndarray
|
||||
per_session_rms_deg: dict[str, float]
|
||||
leave_one_session_delta_deg: dict[str, float]
|
||||
block_out_delta_deg: dict[str, float]
|
||||
convention: str
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class R3Result:
|
||||
r1b: R1bResult
|
||||
r2v: CompletedRotationResult
|
||||
r2g: CompletedRotationResult
|
||||
r2v_r2g_delta_deg: float
|
||||
full_rotation_accepted: bool
|
||||
translation_unlocked: bool
|
||||
blockers: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _BaselinePair:
|
||||
session_index: int
|
||||
session_id: str
|
||||
world_angle_rad: float
|
||||
delta_R: np.ndarray
|
||||
J_bg: np.ndarray
|
||||
|
||||
|
||||
def _f(row: dict[str, str], key: str, default: float = np.nan) -> float:
|
||||
try:
|
||||
return float(row.get(key, ""))
|
||||
except (TypeError, ValueError):
|
||||
return default
|
||||
|
||||
|
||||
def _truth(row: dict[str, str], key: str) -> bool:
|
||||
return str(row.get(key, "")).strip().lower() in {"1", "true", "yes"}
|
||||
|
||||
|
||||
def load_unified_sessions(
|
||||
manifest_path: Path | str,
|
||||
*,
|
||||
selected_session_ids: set[str] | None = None,
|
||||
) -> list[UnifiedSession]:
|
||||
manifest_source = Path(manifest_path)
|
||||
manifest = json.loads(manifest_source.read_text(encoding="utf-8"))
|
||||
sessions: list[UnifiedSession] = []
|
||||
for entry in manifest["sessions"]:
|
||||
session_id = str(entry["session_id"])
|
||||
if selected_session_ids and session_id not in selected_session_ids:
|
||||
continue
|
||||
directory = Path(entry["directory"])
|
||||
if not directory.is_absolute():
|
||||
directory = manifest_source.parent / directory
|
||||
with np.load(directory / "imu.npz") as payload:
|
||||
t = np.asarray(payload["system_time_s"], dtype=float)
|
||||
gyro = np.asarray(payload["gyro_rad_s"], dtype=float)
|
||||
accel = np.asarray(payload["accel_m_s2"], dtype=float)
|
||||
rpy = np.asarray(payload["rpy_deg"], dtype=float)
|
||||
quaternion = np.asarray(payload["quaternion_wxyz"], dtype=float)
|
||||
host = np.asarray(payload["host_receive_utc_s"], dtype=float)
|
||||
by_type: dict[str, list[dict[str, str]]] = {}
|
||||
with (directory / "rtk.csv").open("r", encoding="utf-8", newline="") as stream:
|
||||
for row in csv.DictReader(stream):
|
||||
by_type.setdefault(row["message_type"], []).append(row)
|
||||
sessions.append(
|
||||
UnifiedSession(
|
||||
session_id=session_id,
|
||||
batch_id=str(entry["batch_id"]),
|
||||
imu=ImuSeries(t_s=t, gyro_rad_s=gyro, acc_m_s2=accel),
|
||||
imu_rpy_deg=rpy,
|
||||
imu_quaternion_wxyz=quaternion,
|
||||
imu_host_receive_utc_s=host,
|
||||
rtk_by_type=by_type,
|
||||
)
|
||||
)
|
||||
return sessions
|
||||
|
||||
|
||||
def _valid_hpr(session: UnifiedSession) -> tuple[np.ndarray, np.ndarray]:
|
||||
rows = [
|
||||
row for row in session.rtk_by_type.get("GNHPR", [])
|
||||
if _truth(row, "checksum_valid") and int(_f(row, "heading_quality", -1)) == 4
|
||||
]
|
||||
if not rows:
|
||||
return np.zeros(0), np.zeros((0, 3))
|
||||
t = np.asarray([_f(row, "t_device_s") for row in rows])
|
||||
baseline = gnhpr_to_baseline_enu(
|
||||
np.asarray([_f(row, "heading_deg") for row in rows]),
|
||||
np.asarray([_f(row, "pitch_deg") for row in rows]),
|
||||
)
|
||||
finite = np.isfinite(t) & np.all(np.isfinite(baseline), axis=1)
|
||||
t, baseline = t[finite], baseline[finite]
|
||||
order = np.argsort(t)
|
||||
t, baseline = t[order], baseline[order]
|
||||
unique, indices = np.unique(t, return_index=True)
|
||||
return unique, baseline[indices]
|
||||
|
||||
|
||||
def _baseline_pairs(sessions: list[UnifiedSession]) -> list[_BaselinePair]:
|
||||
pairs: list[_BaselinePair] = []
|
||||
for session_index, session in enumerate(sessions):
|
||||
t, baseline = _valid_hpr(session)
|
||||
if t.size < 3:
|
||||
continue
|
||||
dt = np.diff(t)
|
||||
jump = np.degrees(
|
||||
np.arccos(np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0))
|
||||
)
|
||||
continuous = (dt >= 0.03) & (dt <= 0.25) & (jump / np.maximum(dt, 1e-6) <= 45.0)
|
||||
last_anchor = -np.inf
|
||||
for index, t0 in enumerate(t[:-1]):
|
||||
if t0 - last_anchor < 1.0:
|
||||
continue
|
||||
last_anchor = t0
|
||||
for duration in (0.75, 1.5, 3.0):
|
||||
target = t0 + duration
|
||||
end = int(np.searchsorted(t, target))
|
||||
candidates = [candidate for candidate in (end - 1, end) if index < candidate < t.size]
|
||||
if not candidates:
|
||||
continue
|
||||
j = min(candidates, key=lambda candidate: abs(t[candidate] - target))
|
||||
if abs((t[j] - t0) - duration) > 0.12 or not np.all(continuous[index:j]):
|
||||
continue
|
||||
try:
|
||||
pre = preintegrate_gyro(
|
||||
session.imu.t_s, session.imu.gyro_rad_s, float(t0), float(t[j])
|
||||
)
|
||||
except ValueError:
|
||||
continue
|
||||
world_angle = float(
|
||||
np.arccos(np.clip(np.dot(baseline[index], baseline[j]), -1.0, 1.0))
|
||||
)
|
||||
if np.degrees(world_angle) < 0.4:
|
||||
continue
|
||||
pairs.append(
|
||||
_BaselinePair(
|
||||
session_index=session_index,
|
||||
session_id=session.session_id,
|
||||
world_angle_rad=world_angle,
|
||||
delta_R=pre.delta_R,
|
||||
J_bg=pre.J_bg,
|
||||
)
|
||||
)
|
||||
return pairs
|
||||
|
||||
|
||||
def _axis_from_parameters(parameters: np.ndarray) -> np.ndarray:
|
||||
axis = np.asarray([1.0, parameters[0], parameters[1]], dtype=float)
|
||||
return axis / np.linalg.norm(axis)
|
||||
|
||||
|
||||
def solve_r1b(sessions: list[UnifiedSession]) -> R1bResult:
|
||||
pairs = _baseline_pairs(sessions)
|
||||
if len(pairs) < 20:
|
||||
raise ValueError("R1b needs at least 20 continuous baseline/gyro motion pairs")
|
||||
session_count = len(sessions)
|
||||
pair_counts = {
|
||||
session.session_id: sum(pair.session_id == session.session_id for pair in pairs)
|
||||
for session in sessions
|
||||
}
|
||||
active_sessions = sum(count > 0 for count in pair_counts.values())
|
||||
target_count = len(pairs) / max(active_sessions, 1)
|
||||
|
||||
def residual(parameters: np.ndarray) -> np.ndarray:
|
||||
axis = _axis_from_parameters(parameters[:2])
|
||||
biases = parameters[2:].reshape(session_count, 3)
|
||||
values = []
|
||||
for pair in pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R, pair.J_bg, biases[pair.session_index]
|
||||
)
|
||||
body_angle = np.arccos(
|
||||
np.clip(np.dot(axis, corrected @ axis), -1.0, 1.0)
|
||||
)
|
||||
weight = np.sqrt(target_count / pair_counts[pair.session_id])
|
||||
values.append(weight * (body_angle - pair.world_angle_rad))
|
||||
values.extend((biases / 0.01).reshape(-1))
|
||||
# Weak 20-degree installation prior selects the physically known +X hemisphere.
|
||||
values.extend(np.asarray(parameters[:2]) / np.tan(np.deg2rad(20.0)))
|
||||
return np.asarray(values)
|
||||
|
||||
initial = np.zeros(2 + 3 * session_count)
|
||||
optimum = least_squares(
|
||||
residual, initial, loss="huber", f_scale=np.deg2rad(0.25), max_nfev=120
|
||||
)
|
||||
axis = _axis_from_parameters(optimum.x[:2])
|
||||
biases = optimum.x[2:].reshape(session_count, 3)
|
||||
errors = []
|
||||
per_session_values: dict[str, list[float]] = {}
|
||||
for pair in pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R, pair.J_bg, biases[pair.session_index]
|
||||
)
|
||||
body_angle = np.arccos(np.clip(np.dot(axis, corrected @ axis), -1.0, 1.0))
|
||||
error = float(np.degrees(body_angle - pair.world_angle_rad))
|
||||
errors.append(error)
|
||||
per_session_values.setdefault(pair.session_id, []).append(error)
|
||||
errors_array = np.asarray(errors)
|
||||
data_rows = len(pairs)
|
||||
jacobian = optimum.jac[:data_rows, :2]
|
||||
information = jacobian.T @ jacobian
|
||||
residual_variance = float(np.mean(np.deg2rad(errors_array) ** 2))
|
||||
covariance = residual_variance * np.linalg.pinv(information, rcond=1e-12)
|
||||
covariance_deg2 = np.degrees(1.0) ** 2 * covariance
|
||||
std_deg = np.sqrt(np.maximum(np.diag(covariance_deg2), 0.0))
|
||||
singular = np.linalg.svd(information, compute_uv=False)
|
||||
rms = float(np.sqrt(np.mean(errors_array**2)))
|
||||
p95 = float(np.percentile(np.abs(errors_array), 95.0))
|
||||
return R1bResult(
|
||||
baseline_axis_imu=axis,
|
||||
tilt_yz_deg=np.degrees(np.arctan(optimum.x[:2])),
|
||||
pair_count=len(pairs),
|
||||
residual_rms_deg=rms,
|
||||
residual_p95_deg=p95,
|
||||
covariance_deg2=covariance_deg2,
|
||||
std_deg=std_deg,
|
||||
information_singular_values=singular,
|
||||
per_session_rms_deg={
|
||||
key: float(np.sqrt(np.mean(np.asarray(value) ** 2)))
|
||||
for key, value in per_session_values.items()
|
||||
},
|
||||
gyro_bias_by_session_rad_s={
|
||||
session.session_id: biases[index] for index, session in enumerate(sessions)
|
||||
},
|
||||
ok=bool(rms <= 1.0 and p95 <= 2.0 and np.min(singular) >= 1e-3),
|
||||
notes=(
|
||||
"2DoF ANT1-to-ANT2 direction; +X hemisphere selected by installation knowledge",
|
||||
"weak 20 deg prior is reported and prevents sign/gauge branch switching",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def _rotation_mean(matrices: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
mean = Rotation.from_matrix(matrices).mean().as_matrix()
|
||||
errors = np.asarray(
|
||||
[np.degrees(np.linalg.norm(so3_log(mean.T @ matrix))) for matrix in matrices]
|
||||
)
|
||||
return mean, errors
|
||||
|
||||
|
||||
def _result_from_samples(
|
||||
method: str,
|
||||
samples: list[tuple[str, str, np.ndarray]],
|
||||
convention: str,
|
||||
notes: tuple[str, ...],
|
||||
) -> CompletedRotationResult:
|
||||
if not samples:
|
||||
return CompletedRotationResult(
|
||||
method, None, None, 0, 0, np.nan, np.nan, np.full((3, 3), np.nan),
|
||||
np.full(3, np.nan),
|
||||
{}, {}, {}, convention, False, notes + ("no qualifying samples",)
|
||||
)
|
||||
matrices = np.asarray([item[2] for item in samples])
|
||||
mean, errors = _rotation_mean(matrices)
|
||||
rotvec = np.asarray([so3_log(mean.T @ matrix) for matrix in matrices])
|
||||
rotvec_deg = np.degrees(rotvec)
|
||||
std = np.std(rotvec_deg, axis=0, ddof=1) if len(samples) > 1 else np.full(3, np.nan)
|
||||
covariance = (
|
||||
np.cov(rotvec_deg, rowvar=False, ddof=1) / len(samples)
|
||||
if len(samples) > 1 else np.full((3, 3), np.nan)
|
||||
)
|
||||
ids = sorted({item[0] for item in samples})
|
||||
per_session = {}
|
||||
loo = {}
|
||||
for session_id in ids:
|
||||
selected = [item[2] for item in samples if item[0] == session_id]
|
||||
_, local_errors = _rotation_mean(np.asarray(selected))
|
||||
per_session[session_id] = float(np.sqrt(np.mean(local_errors**2)))
|
||||
kept = np.asarray([item[2] for item in samples if item[0] != session_id])
|
||||
if kept.size:
|
||||
kept_mean, _ = _rotation_mean(kept)
|
||||
loo[session_id] = float(np.degrees(np.linalg.norm(so3_log(mean.T @ kept_mean))))
|
||||
block_groups = sorted({(item[0], item[1]) for item in samples})
|
||||
block_out = {}
|
||||
for session_id, block_id in block_groups:
|
||||
kept = np.asarray(
|
||||
[item[2] for item in samples if (item[0], item[1]) != (session_id, block_id)]
|
||||
)
|
||||
if kept.size:
|
||||
kept_mean, _ = _rotation_mean(kept)
|
||||
block_out[f"{session_id}:{block_id}"] = float(
|
||||
np.degrees(np.linalg.norm(so3_log(mean.T @ kept_mean)))
|
||||
)
|
||||
rms = float(np.sqrt(np.mean(errors**2)))
|
||||
p95 = float(np.percentile(errors, 95.0))
|
||||
max_loo = max(loo.values(), default=np.inf)
|
||||
ok = bool(
|
||||
len(samples) >= 10 and len(ids) >= 2 and rms <= 2.0 and p95 <= 3.0
|
||||
and np.nanmax(std) <= 1.0 and max_loo <= 1.0
|
||||
)
|
||||
return CompletedRotationResult(
|
||||
method=method,
|
||||
R_RTK_IMU=mean,
|
||||
rpy_deg=Rotation.from_matrix(mean).as_euler("xyz", degrees=True),
|
||||
sample_count=len(samples),
|
||||
session_count=len(ids),
|
||||
residual_rms_deg=rms,
|
||||
residual_p95_deg=p95,
|
||||
covariance_deg2=covariance,
|
||||
std_deg=std,
|
||||
per_session_rms_deg=per_session,
|
||||
leave_one_session_delta_deg=loo,
|
||||
block_out_delta_deg=block_out,
|
||||
convention=convention,
|
||||
ok=ok,
|
||||
notes=notes,
|
||||
)
|
||||
|
||||
|
||||
def solve_r2g(
|
||||
sessions: list[UnifiedSession],
|
||||
r1b: R1bResult,
|
||||
*,
|
||||
level_static_session_ids: set[str],
|
||||
block_duration_s: float = 10.0,
|
||||
) -> CompletedRotationResult:
|
||||
samples: list[tuple[str, str, np.ndarray]] = []
|
||||
right = r1b.baseline_axis_imu
|
||||
for session in sessions:
|
||||
if session.session_id not in level_static_session_ids:
|
||||
continue
|
||||
gyro_norm = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
|
||||
accel_norm = np.linalg.norm(session.imu.acc_m_s2, axis=1)
|
||||
valid = (gyro_norm <= np.deg2rad(0.35)) & (np.abs(accel_norm - 9.80665) <= 0.15)
|
||||
block = np.floor(
|
||||
(session.imu.t_s - session.imu.t_s[0]) / block_duration_s
|
||||
).astype(int)
|
||||
for block_id in np.unique(block[valid]):
|
||||
selected = valid & (block == block_id)
|
||||
if np.count_nonzero(selected) < 200:
|
||||
continue
|
||||
up = np.median(session.imu.acc_m_s2[selected], axis=0)
|
||||
up /= np.linalg.norm(up)
|
||||
up -= right * np.dot(up, right)
|
||||
if np.linalg.norm(up) < 0.9:
|
||||
continue
|
||||
up /= np.linalg.norm(up)
|
||||
forward = np.cross(up, right)
|
||||
forward /= np.linalg.norm(forward)
|
||||
C_IMU_RTK = np.column_stack([right, forward, up])
|
||||
samples.append((session.session_id, str(int(block_id)), C_IMU_RTK.T))
|
||||
return _result_from_samples(
|
||||
"R2G_baseline_plus_level_gravity",
|
||||
samples,
|
||||
"accelerometer specific-force points vehicle up on explicit level-static blocks",
|
||||
(
|
||||
"only caller-declared level-static sessions are eligible",
|
||||
"result is level/gravity-prior constrained, not dual-antenna-only",
|
||||
),
|
||||
)
|
||||
|
||||
|
||||
def _nearest_index(t: np.ndarray, value: float, tolerance: float) -> int | None:
|
||||
index = int(np.searchsorted(t, value))
|
||||
candidates = [item for item in (index - 1, index) if 0 <= item < t.size]
|
||||
if not candidates:
|
||||
return None
|
||||
best = min(candidates, key=lambda item: abs(t[item] - value))
|
||||
return best if abs(t[best] - value) <= tolerance else None
|
||||
|
||||
|
||||
def solve_r2v(
|
||||
sessions: list[UnifiedSession],
|
||||
*,
|
||||
min_speed_m_s: float = 1.5,
|
||||
max_yaw_rate_deg_s: float = 3.0,
|
||||
max_baseline_course_error_deg: float = 15.0,
|
||||
) -> CompletedRotationResult:
|
||||
candidates: dict[str, list[tuple[str, str, np.ndarray]]] = {
|
||||
"HI13_q_body_to_ENU": [],
|
||||
"NED_to_ENU_times_HI13_q": [],
|
||||
"HI13_q_inverse_as_body_to_ENU": [],
|
||||
"NED_to_ENU_times_HI13_q_inverse": [],
|
||||
}
|
||||
NED_TO_ENU = np.asarray([[0.0, 1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, -1.0]])
|
||||
for session in sessions:
|
||||
hpr_t, hpr_baseline = _valid_hpr(session)
|
||||
if hpr_t.size == 0:
|
||||
continue
|
||||
quaternion = session.imu_quaternion_wxyz
|
||||
norm = np.linalg.norm(quaternion, axis=1)
|
||||
valid_quaternion = np.isfinite(norm) & (np.abs(norm - 1.0) <= 0.02)
|
||||
normalized = quaternion / np.maximum(norm[:, None], 1e-12)
|
||||
imu_rotations = Rotation.from_quat(normalized[:, [1, 2, 3, 0]]).as_matrix()
|
||||
for row_index, row in enumerate(session.rtk_by_type.get("BESTNAVA", [])):
|
||||
if not (
|
||||
_truth(row, "checksum_valid")
|
||||
and _truth(row, "position_fixed")
|
||||
and _truth(row, "doppler_velocity_valid")
|
||||
and _f(row, "horizontal_speed_m_s") >= min_speed_m_s
|
||||
and _f(row, "horizontal_speed_std_m_s") <= 0.25
|
||||
):
|
||||
continue
|
||||
t = _f(row, "t_device_s")
|
||||
hpr_index = _nearest_index(hpr_t, t, 0.15)
|
||||
imu_index = _nearest_index(session.imu.t_s, t, 0.03)
|
||||
if hpr_index is None or imu_index is None or not valid_quaternion[imu_index]:
|
||||
continue
|
||||
if abs(np.degrees(session.imu.gyro_rad_s[imu_index, 2])) > max_yaw_rate_deg_s:
|
||||
continue
|
||||
right = hpr_baseline[hpr_index]
|
||||
forward = np.asarray(
|
||||
[_f(row, "velocity_east_m_s"), _f(row, "velocity_north_m_s"), 0.0]
|
||||
)
|
||||
forward /= np.linalg.norm(forward)
|
||||
course_error = np.degrees(
|
||||
np.arcsin(np.clip(abs(np.dot(right, forward)), 0.0, 1.0))
|
||||
)
|
||||
if course_error > max_baseline_course_error_deg:
|
||||
continue
|
||||
forward -= right * np.dot(right, forward)
|
||||
forward /= np.linalg.norm(forward)
|
||||
up = np.cross(right, forward)
|
||||
if up[2] < 0:
|
||||
forward = -forward
|
||||
up = -up
|
||||
up /= np.linalg.norm(up)
|
||||
R_ENU_RTK = np.column_stack([right, forward, up])
|
||||
q = imu_rotations[imu_index]
|
||||
world_candidates = {
|
||||
"HI13_q_body_to_ENU": q,
|
||||
"NED_to_ENU_times_HI13_q": NED_TO_ENU @ q,
|
||||
"HI13_q_inverse_as_body_to_ENU": q.T,
|
||||
"NED_to_ENU_times_HI13_q_inverse": NED_TO_ENU @ q.T,
|
||||
}
|
||||
block_id = str(row_index // 10)
|
||||
for name, R_ENU_IMU in world_candidates.items():
|
||||
candidates[name].append(
|
||||
(session.session_id, block_id, R_ENU_RTK.T @ R_ENU_IMU)
|
||||
)
|
||||
diagnostics = {
|
||||
name: _result_from_samples(
|
||||
"R2V_baseline_plus_doppler_velocity",
|
||||
values,
|
||||
name,
|
||||
(
|
||||
"RTK fixed + Doppler velocity + speed + low-yaw + baseline/course gates",
|
||||
"HI13 absolute quaternion may contain magnetic/navigation yaw bias",
|
||||
),
|
||||
)
|
||||
for name, values in candidates.items()
|
||||
}
|
||||
finite = [result for result in diagnostics.values() if result.sample_count]
|
||||
if not finite:
|
||||
return diagnostics["HI13_q_body_to_ENU"]
|
||||
return min(finite, key=lambda result: result.residual_rms_deg)
|
||||
|
||||
|
||||
def solve_r3(
|
||||
sessions: list[UnifiedSession],
|
||||
*,
|
||||
level_static_session_ids: set[str],
|
||||
) -> R3Result:
|
||||
dynamic_sessions = [
|
||||
session for session in sessions if session.session_id not in level_static_session_ids
|
||||
]
|
||||
r1b = solve_r1b(dynamic_sessions)
|
||||
r2v = solve_r2v(dynamic_sessions)
|
||||
r2g = solve_r2g(sessions, r1b, level_static_session_ids=level_static_session_ids)
|
||||
if r2v.R_RTK_IMU is None or r2g.R_RTK_IMU is None:
|
||||
delta = np.nan
|
||||
else:
|
||||
delta = float(
|
||||
np.degrees(np.linalg.norm(so3_log(r2v.R_RTK_IMU.T @ r2g.R_RTK_IMU)))
|
||||
)
|
||||
blockers = []
|
||||
if not r1b.ok:
|
||||
blockers.append("R1b baseline direction failed residual/observability gates")
|
||||
if not r2v.ok:
|
||||
blockers.append("R2V velocity-completed rotation failed stability gates")
|
||||
if not r2g.ok:
|
||||
blockers.append("R2G gravity-completed rotation failed stability gates")
|
||||
if not np.isfinite(delta) or delta > 2.0:
|
||||
blockers.append("R2V and R2G disagree by more than 2 deg")
|
||||
accepted = not blockers
|
||||
return R3Result(
|
||||
r1b=r1b,
|
||||
r2v=r2v,
|
||||
r2g=r2g,
|
||||
r2v_r2g_delta_deg=delta,
|
||||
full_rotation_accepted=accepted,
|
||||
translation_unlocked=accepted,
|
||||
blockers=tuple(blockers),
|
||||
)
|
||||
|
||||
|
||||
def result_to_jsonable(result: R3Result) -> dict:
|
||||
def convert(value):
|
||||
if isinstance(value, np.ndarray):
|
||||
return value.tolist()
|
||||
if isinstance(value, np.generic):
|
||||
return value.item()
|
||||
if hasattr(value, "__dataclass_fields__"):
|
||||
return {key: convert(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): convert(item) for key, item in value.items()}
|
||||
if isinstance(value, (tuple, list)):
|
||||
return [convert(item) for item in value]
|
||||
return value
|
||||
return convert(result)
|
||||
@@ -0,0 +1,529 @@
|
||||
'''Per-GNSS-node RTK/IMU state graph used after legacy propagation deprecation.'''
|
||||
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 imu_lidar.geometry import so3_exp, so3_log
|
||||
from imu_lidar.imu_preintegration import apply_bias_correction_imu, residual_whiten_matrix
|
||||
from .rtk_imu_engineering import (
|
||||
G_ENU, HPR_DIRECT_ANGULAR_SIGMA_RAD, _Segment, _world_rtk)
|
||||
|
||||
NODE_DOF = 15
|
||||
SIGMA_BG_RW = 1e-5
|
||||
SIGMA_BA_RW = 1e-3
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class NodeGraphProblem:
|
||||
segment: _Segment
|
||||
R_seed_WI: tuple[np.ndarray,...]
|
||||
fixed_l_I_m: np.ndarray
|
||||
R_RTK_IMU: np.ndarray
|
||||
hpr_direct_angular_sigma_rad: float = HPR_DIRECT_ANGULAR_SIGMA_RAD
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class NodeGraphResult:
|
||||
success: bool
|
||||
message: str
|
||||
node_count: int
|
||||
duration_s: float
|
||||
fixed_l_I_m: np.ndarray
|
||||
initial_cost: float
|
||||
final_cost: float
|
||||
cost_reduction: float
|
||||
nfev: int
|
||||
optimality: float
|
||||
gradient_norm: float
|
||||
residual_dimension: int
|
||||
state_dimension: int
|
||||
statistical_dof: int
|
||||
total_nis: float
|
||||
chi_square_per_dof: float
|
||||
cost_per_dof: float
|
||||
initial_residual_by_factor: dict[str,dict[str,float]]
|
||||
final_residual_by_factor: dict[str,dict[str,float]]
|
||||
final_position_residual_m: dict[str,object]
|
||||
final_velocity_residual_m_s: dict[str,object]
|
||||
max_bg_step_rad_s: float
|
||||
max_ba_step_m_s2: float
|
||||
preintegration_covariance_sigma: dict[str,dict[str,float]]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class FreeLeverResult:
|
||||
success: bool
|
||||
message: str
|
||||
initial_l_I_m: np.ndarray
|
||||
final_l_I_m: np.ndarray
|
||||
lever_step_norm_m: float
|
||||
initial_cost: float
|
||||
final_cost: float
|
||||
nfev: int
|
||||
optimality: float
|
||||
chi_square_per_dof: float
|
||||
position_residual_m: dict[str,object]
|
||||
velocity_residual_m_s: dict[str,object]
|
||||
residual_by_factor: dict[str,dict[str,float]]
|
||||
lever_covariance_m2: np.ndarray
|
||||
lever_information_singular_values: np.ndarray
|
||||
lever_information_condition_number: float
|
||||
lever_precision_rank: int
|
||||
weakest_lever_direction_I: np.ndarray
|
||||
|
||||
def _rotation(problem,index,x):
|
||||
offset = NODE_DOF*index
|
||||
return problem.R_seed_WI[index] @ so3_exp(x[offset:offset+3])
|
||||
|
||||
def initial_parameters(problem, lever_override=None):
|
||||
nodes = problem.segment.nodes
|
||||
x = np.zeros(NODE_DOF*len(nodes))
|
||||
lever = (problem.fixed_l_I_m if lever_override is None
|
||||
else np.asarray(lever_override,dtype=float))
|
||||
previous_p, previous_v = np.zeros(3), np.zeros(3)
|
||||
for index,node in enumerate(nodes):
|
||||
offset = NODE_DOF*index
|
||||
R = problem.R_seed_WI[index]
|
||||
previous_p = node.p_enu_m-R@lever
|
||||
if index and not node.position_mask[2]:
|
||||
previous_p[2] = x[offset-NODE_DOF+5]
|
||||
if node.velocity_enu_m_s is not None:
|
||||
previous_v = node.velocity_enu_m_s-R@np.cross(node.gyro_rad_s,lever)
|
||||
x[offset+3:offset+6] = previous_p
|
||||
x[offset+6:offset+9] = previous_v
|
||||
return x
|
||||
|
||||
def _stats(values, effective_dof=None):
|
||||
a = np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'dof':0,'rms':np.nan,'p50_abs':np.nan,
|
||||
'p95_abs':np.nan,'p99_abs':np.nan,'nis':np.nan,
|
||||
'chi_square_per_dof':np.nan}
|
||||
nis = float(np.dot(a,a))
|
||||
dof = int(a.size if effective_dof is None else effective_dof)
|
||||
return {'count':int(a.size),'dof':dof,
|
||||
'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50_abs':float(np.percentile(np.abs(a),50.)),
|
||||
'p95_abs':float(np.percentile(np.abs(a),95.)),
|
||||
'p99_abs':float(np.percentile(np.abs(a),99.)),
|
||||
'nis':nis,'chi_square_per_dof':nis/max(dof,1)}
|
||||
|
||||
def _hpr_sigma(problem, node):
|
||||
old = float(node.hpr_angular_sigma_rad)
|
||||
extra_var = max(old*old-HPR_DIRECT_ANGULAR_SIGMA_RAD**2, 0.)
|
||||
return float(np.sqrt(problem.hpr_direct_angular_sigma_rad**2+extra_var))
|
||||
|
||||
|
||||
def _distribution(values):
|
||||
a = np.asarray(values,dtype=float).reshape(-1)
|
||||
if not a.size:
|
||||
return {'count':0,'rms':np.nan,'p50':np.nan,'p95':np.nan,'p99':np.nan}
|
||||
return {'count':len(a),'rms':float(np.sqrt(np.mean(a*a))),
|
||||
'p50':float(np.percentile(a,50.)),'p95':float(np.percentile(a,95.)),
|
||||
'p99':float(np.percentile(a,99.))}
|
||||
|
||||
def _vector_stats(values):
|
||||
a = np.asarray(values,dtype=float).reshape(-1,3)
|
||||
if not a.size:
|
||||
return {'count':0,'axis_rms':[np.nan]*3,'axis_p95_abs':[np.nan]*3,
|
||||
'vector_rms':np.nan,'vector_p95':np.nan}
|
||||
norm = np.linalg.norm(a,axis=1)
|
||||
return {'count':len(a),'axis_rms':np.sqrt(np.mean(a*a,axis=0)),
|
||||
'axis_p95_abs':np.percentile(np.abs(a),95.,axis=0),
|
||||
'vector_rms':float(np.sqrt(np.mean(norm*norm))),
|
||||
'vector_p95':float(np.percentile(norm,95.))}
|
||||
|
||||
def residual(problem,x,details=None,dependencies=None,lever_override=None):
|
||||
nodes, preints = problem.segment.nodes, problem.segment.preintegrations
|
||||
lever = (problem.fixed_l_I_m if lever_override is None
|
||||
else np.asarray(lever_override,dtype=float))
|
||||
baseline_I = problem.R_RTK_IMU.T[:,0]
|
||||
values = []
|
||||
def add(value,label,node_indices,raw=None):
|
||||
a = np.asarray(value,dtype=float).reshape(-1)
|
||||
values.extend(a)
|
||||
if dependencies is not None:
|
||||
dependencies.extend([tuple(node_indices)]*len(a))
|
||||
if details is not None:
|
||||
details.setdefault(label,[]).extend(a.tolist())
|
||||
if raw is not None: details.setdefault(label+'_physical',[]).append(np.asarray(raw))
|
||||
for index,node in enumerate(nodes):
|
||||
offset = NODE_DOF*index
|
||||
R = _rotation(problem,index,x)
|
||||
p, v = x[offset+3:offset+6], x[offset+6:offset+9]
|
||||
bg, ba = x[offset+9:offset+12], x[offset+12:offset+15]
|
||||
p_error = p+R@lever-node.p_enu_m
|
||||
if node.source == 'GGA':
|
||||
add(p_error[:2]/.06,'gga_xy',(index,))
|
||||
if details is not None: details.setdefault('position_physical',[]).append(
|
||||
np.array([p_error[0],p_error[1],np.nan]))
|
||||
else:
|
||||
add(p_error/np.array([.06,.06,.12]),'best_position',(index,),p_error)
|
||||
if details is not None: details.setdefault('position_physical',[]).append(p_error)
|
||||
if node.velocity_enu_m_s is not None:
|
||||
v_error = v+R@np.cross(node.gyro_rad_s-bg,lever)-node.velocity_enu_m_s
|
||||
add(v_error/np.array([.15,.15,.30]),'doppler',(index,),v_error)
|
||||
if details is not None: details.setdefault('velocity_physical',[]).append(v_error)
|
||||
if node.hpr_factor_valid:
|
||||
hpr_error=np.cross(R@baseline_I,node.baseline_enu)
|
||||
add(hpr_error/_hpr_sigma(problem,node),'hpr',(index,),hpr_error)
|
||||
if node.gravity_candidate:
|
||||
gravity = node.accel_m_s2-ba-R.T@(-G_ENU)
|
||||
add(gravity/.12,'gravity',(index,))
|
||||
if index == len(nodes)-1: continue
|
||||
right = index+1
|
||||
right_offset = NODE_DOF*right
|
||||
Rj = _rotation(problem,right,x)
|
||||
pj = x[right_offset+3:right_offset+6]
|
||||
vj = x[right_offset+6:right_offset+9]
|
||||
bgj = x[right_offset+9:right_offset+12]
|
||||
baj = x[right_offset+12:right_offset+15]
|
||||
pre = preints[index]
|
||||
dR,dv,dp = apply_bias_correction_imu(pre,bg,ba)
|
||||
dt = pre.duration_s
|
||||
imu_error = np.concatenate([
|
||||
so3_log(dR.T@R.T@Rj),
|
||||
R.T@(vj-v-G_ENU*dt)-dv,
|
||||
R.T@(pj-p-v*dt-.5*G_ENU*dt*dt)-dp])
|
||||
add(residual_whiten_matrix(pre.cov)@imu_error,'imu_preintegration',
|
||||
(index,right),imu_error)
|
||||
add((bgj-bg)/(SIGMA_BG_RW*np.sqrt(dt)),'gyro_bias_random_walk',(index,right))
|
||||
add((baj-ba)/(SIGMA_BA_RW*np.sqrt(dt)),'accel_bias_random_walk',(index,right))
|
||||
add(x[:3]/np.deg2rad(5.),'initial_attitude_gauge',(0,))
|
||||
add(x[9:12]/.02,'initial_gyro_bias',(0,))
|
||||
add(x[12:15]/.5,'initial_accel_bias',(0,))
|
||||
return np.asarray(values)
|
||||
|
||||
def jacobian_sparsity(problem,x):
|
||||
dependencies = []
|
||||
base = residual(problem,x,dependencies=dependencies)
|
||||
sparsity = lil_matrix((len(base),len(x)),dtype=int)
|
||||
for row,node_indices in enumerate(dependencies):
|
||||
for index in node_indices:
|
||||
start = NODE_DOF*index
|
||||
sparsity[row,start:start+NODE_DOF] = 1
|
||||
return sparsity.tocsr()
|
||||
|
||||
def _factor_stats(details):
|
||||
return {key:_stats(value,2*len(value)//3 if key=='hpr' else None)
|
||||
for key,value in details.items()
|
||||
if not key.endswith('_physical') and key not in ('position_physical','velocity_physical')}
|
||||
|
||||
|
||||
def _effective_residual_dimension(details):
|
||||
return sum(2*len(v)//3 if k=='hpr' else len(v)
|
||||
for k,v in details.items() if not k.endswith('_physical')
|
||||
and k not in ('position_physical','velocity_physical'))
|
||||
|
||||
def _preintegration_covariance_stats(problem):
|
||||
blocks = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
|
||||
for pre in problem.segment.preintegrations:
|
||||
sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
|
||||
blocks['rotation_rad'].extend(sigma[:3])
|
||||
blocks['velocity_m_s'].extend(sigma[3:6])
|
||||
blocks['position_m'].extend(sigma[6:9])
|
||||
return {key:_distribution(value) for key,value in blocks.items()}
|
||||
|
||||
def build_problem(segment,R_RTK_IMU,fixed_l_I_m,
|
||||
hpr_direct_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD):
|
||||
seeds = []
|
||||
for index,node in enumerate(segment.nodes):
|
||||
if node.hpr_factor_valid:
|
||||
seeds.append(_world_rtk(node.baseline_enu)@R_RTK_IMU)
|
||||
elif index:
|
||||
seeds.append(seeds[-1]@segment.preintegrations[index-1].delta_R)
|
||||
else:
|
||||
seeds.append(segment.R_WRTK_initial@R_RTK_IMU)
|
||||
return NodeGraphProblem(segment,tuple(seeds),np.asarray(fixed_l_I_m,dtype=float),
|
||||
np.asarray(R_RTK_IMU,dtype=float),
|
||||
float(hpr_direct_angular_sigma_rad))
|
||||
|
||||
def solve_fixed_lever(problem,max_nfev=30):
|
||||
x0 = initial_parameters(problem)
|
||||
initial_detail = {}
|
||||
r0 = residual(problem,x0,initial_detail)
|
||||
fit = least_squares(
|
||||
lambda value:residual(problem,value),x0,jac='2-point',
|
||||
jac_sparsity=jacobian_sparsity(problem,x0),method='trf',
|
||||
tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
|
||||
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
final_detail = {}
|
||||
rf = residual(problem,fit.x,final_detail)
|
||||
statistical_dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
|
||||
bg = fit.x.reshape(-1,NODE_DOF)[:,9:12]
|
||||
ba = fit.x.reshape(-1,NODE_DOF)[:,12:15]
|
||||
bg_step = np.diff(bg,axis=0)
|
||||
ba_step = np.diff(ba,axis=0)
|
||||
return NodeGraphResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
node_count=len(problem.segment.nodes),
|
||||
duration_s=problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s,
|
||||
fixed_l_I_m=problem.fixed_l_I_m.copy(),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
|
||||
cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
|
||||
nfev=int(fit.nfev),optimality=float(fit.optimality),
|
||||
gradient_norm=float(np.linalg.norm(fit.grad)),
|
||||
residual_dimension=len(rf),state_dimension=len(fit.x),
|
||||
statistical_dof=statistical_dof,total_nis=float(np.dot(rf,rf)),
|
||||
chi_square_per_dof=float(np.dot(rf,rf)/statistical_dof),
|
||||
cost_per_dof=.5*float(np.dot(rf,rf)/statistical_dof),
|
||||
initial_residual_by_factor=_factor_stats(initial_detail),
|
||||
final_residual_by_factor=_factor_stats(final_detail),
|
||||
final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
|
||||
final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
|
||||
max_bg_step_rad_s=float(np.max(np.linalg.norm(bg_step,axis=1))) if len(bg_step) else 0.,
|
||||
max_ba_step_m_s2=float(np.max(np.linalg.norm(ba_step,axis=1))) if len(ba_step) else 0.,
|
||||
preintegration_covariance_sigma=_preintegration_covariance_stats(problem))
|
||||
|
||||
|
||||
def fit_states_at_fixed_lever(problem,l_I_m,max_nfev=50,initial_state_values=None):
|
||||
lever=np.asarray(l_I_m,dtype=float)
|
||||
x0=(initial_parameters(problem,lever) if initial_state_values is None
|
||||
else np.asarray(initial_state_values,dtype=float))
|
||||
r0=residual(problem,x0,lever_override=lever)
|
||||
fit=least_squares(
|
||||
lambda value:residual(problem,value,lever_override=lever),x0,jac='2-point',
|
||||
jac_sparsity=jacobian_sparsity(problem,x0),method='trf',tr_solver='lsmr',
|
||||
loss='linear',max_nfev=max_nfev,x_scale='jac',
|
||||
ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
return fit.x,{'success':bool(fit.success),'message':str(fit.message),
|
||||
'nfev':int(fit.nfev),'initial_cost':.5*float(r0@r0),
|
||||
'cost':float(fit.cost)}
|
||||
|
||||
|
||||
def summarize_fixed_state_values(problems,state_values,l_I_m):
|
||||
details={}; residuals=[]
|
||||
for problem,value in zip(problems,state_values):
|
||||
local={}
|
||||
residuals.append(residual(problem,np.asarray(value),details=local,
|
||||
lever_override=l_I_m))
|
||||
for key,items in local.items(): details.setdefault(key,[]).extend(items)
|
||||
joined=np.concatenate(residuals)
|
||||
state_dimension=sum(len(value) for value in state_values)
|
||||
dof=max(_effective_residual_dimension(details)-state_dimension,1)
|
||||
return {'cost':.5*float(np.dot(joined,joined)),
|
||||
'total_nis':float(np.dot(joined,joined)),
|
||||
'chi_square_per_dof':float(np.dot(joined,joined)/dof),
|
||||
'statistical_dof':dof,'residual_by_factor':_factor_stats(details),
|
||||
'best_position_physical_m':_vector_stats(details.get('best_position_physical',[])),
|
||||
'doppler_physical_m_s':_vector_stats(details.get('doppler_physical',[])),
|
||||
'hpr_physical_rad':_vector_stats(details.get('hpr_physical',[]))}
|
||||
|
||||
def solve_fixed_lever_many(problems,max_nfev=30):
|
||||
problems = tuple(problems)
|
||||
sizes = [NODE_DOF*len(problem.segment.nodes) for problem in problems]
|
||||
offsets = np.cumsum([0,*sizes])
|
||||
x0 = np.concatenate([initial_parameters(problem) for problem in problems])
|
||||
def evaluate(value,details=None):
|
||||
chunks = []
|
||||
for index,problem in enumerate(problems):
|
||||
local_details = {} if details is not None else None
|
||||
chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
|
||||
local_details))
|
||||
if details is not None:
|
||||
for key,items in local_details.items():
|
||||
details.setdefault(key,[]).extend(items)
|
||||
return np.concatenate(chunks)
|
||||
initial_detail = {}
|
||||
r0 = evaluate(x0,initial_detail)
|
||||
sparsity = lil_matrix((len(r0),len(x0)),dtype=int)
|
||||
row = 0
|
||||
for index,problem in enumerate(problems):
|
||||
local_x = x0[offsets[index]:offsets[index+1]]
|
||||
local = jacobian_sparsity(problem,local_x)
|
||||
sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]] = local
|
||||
row += local.shape[0]
|
||||
fit = least_squares(
|
||||
lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
|
||||
method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
|
||||
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
final_detail = {}
|
||||
rf = evaluate(fit.x,final_detail)
|
||||
bg_steps, ba_steps = [], []
|
||||
for index,problem in enumerate(problems):
|
||||
states = fit.x[offsets[index]:offsets[index+1]].reshape(-1,NODE_DOF)
|
||||
bg_steps.extend(np.linalg.norm(np.diff(states[:,9:12],axis=0),axis=1))
|
||||
ba_steps.extend(np.linalg.norm(np.diff(states[:,12:15],axis=0),axis=1))
|
||||
covariance = {'rotation_rad':[],'velocity_m_s':[],'position_m':[]}
|
||||
for problem in problems:
|
||||
for pre in problem.segment.preintegrations:
|
||||
sigma = np.sqrt(np.maximum(np.diag(pre.cov),0.))
|
||||
covariance['rotation_rad'].extend(sigma[:3])
|
||||
covariance['velocity_m_s'].extend(sigma[3:6])
|
||||
covariance['position_m'].extend(sigma[6:9])
|
||||
dof = max(_effective_residual_dimension(final_detail)-len(fit.x),1)
|
||||
return NodeGraphResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
node_count=sum(len(problem.segment.nodes) for problem in problems),
|
||||
duration_s=sum(problem.segment.nodes[-1].t_s-problem.segment.nodes[0].t_s
|
||||
for problem in problems),
|
||||
fixed_l_I_m=problems[0].fixed_l_I_m.copy(),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
|
||||
cost_reduction=.5*float(np.dot(r0,r0)-np.dot(rf,rf)),
|
||||
nfev=int(fit.nfev),optimality=float(fit.optimality),
|
||||
gradient_norm=float(np.linalg.norm(fit.grad)),
|
||||
residual_dimension=len(rf),state_dimension=len(fit.x),
|
||||
statistical_dof=dof,total_nis=float(np.dot(rf,rf)),
|
||||
chi_square_per_dof=float(np.dot(rf,rf)/dof),
|
||||
cost_per_dof=.5*float(np.dot(rf,rf)/dof),
|
||||
initial_residual_by_factor=_factor_stats(initial_detail),
|
||||
final_residual_by_factor=_factor_stats(final_detail),
|
||||
final_position_residual_m=_vector_stats(final_detail.get('position_physical',[])),
|
||||
final_velocity_residual_m_s=_vector_stats(final_detail.get('velocity_physical',[])),
|
||||
max_bg_step_rad_s=float(max(bg_steps,default=0.)),
|
||||
max_ba_step_m_s2=float(max(ba_steps,default=0.)),
|
||||
preintegration_covariance_sigma={key:_distribution(value) for key,value in covariance.items()})
|
||||
|
||||
|
||||
def _free_residual(problem,value,details=None):
|
||||
return residual(problem,value[3:],details=details,lever_override=value[:3])
|
||||
|
||||
|
||||
def _free_sparsity(problem,value):
|
||||
local = jacobian_sparsity(problem,value[3:])
|
||||
result = lil_matrix((local.shape[0],local.shape[1]+3),dtype=int)
|
||||
result[:,:3] = 1
|
||||
result[:,3:] = local
|
||||
return result.tocsr()
|
||||
|
||||
|
||||
def _marginal_lever_information(jacobian):
|
||||
J = jacobian.toarray() if hasattr(jacobian,'toarray') else np.asarray(jacobian)
|
||||
H = J.T@J
|
||||
Hll,Hln,Hnn = H[:3,:3],H[:3,3:],H[3:,3:]
|
||||
marginal = Hll-Hln@np.linalg.pinv(Hnn,rcond=1e-10)@Hln.T
|
||||
return .5*(marginal+marginal.T)
|
||||
|
||||
|
||||
def _additive_marginal_lever_information(jacobian,row_offsets,state_offsets):
|
||||
total=np.zeros((3,3))
|
||||
for index in range(len(row_offsets)-1):
|
||||
rows=slice(row_offsets[index],row_offsets[index+1])
|
||||
columns=np.r_[0:3,state_offsets[index]:state_offsets[index+1]]
|
||||
local=jacobian[rows,:][:,columns]
|
||||
total+=_marginal_lever_information(local)
|
||||
return .5*(total+total.T)
|
||||
|
||||
|
||||
def linearized_lever_information(problem,l_I_m):
|
||||
lever=np.asarray(l_I_m,dtype=float)
|
||||
value=np.concatenate([lever,initial_parameters(problem,lever)])
|
||||
fit=least_squares(lambda x:_free_residual(problem,x),value,jac='2-point',
|
||||
jac_sparsity=_free_sparsity(problem,value),method='trf',tr_solver='lsmr',
|
||||
loss='linear',max_nfev=1,x_scale='jac')
|
||||
information=_marginal_lever_information(fit.jac)
|
||||
_,singular,Vt=np.linalg.svd(information)
|
||||
covariance=np.linalg.pinv(information,rcond=1e-9)
|
||||
return information,covariance,singular,Vt[-1]
|
||||
|
||||
|
||||
def solve_free_lever(problem,initial_l_I_m,max_nfev=120):
|
||||
initial_l = np.asarray(initial_l_I_m,dtype=float)
|
||||
x0 = np.concatenate([initial_l,initial_parameters(problem,initial_l)])
|
||||
r0 = _free_residual(problem,x0)
|
||||
fit = least_squares(
|
||||
lambda value:_free_residual(problem,value),x0,jac='2-point',
|
||||
jac_sparsity=_free_sparsity(problem,x0),method='trf',tr_solver='lsmr',
|
||||
loss='linear',max_nfev=max_nfev,x_scale='jac',
|
||||
ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
detail = {}
|
||||
rf = _free_residual(problem,fit.x,detail)
|
||||
information = _marginal_lever_information(fit.jac)
|
||||
_,singular_values,Vt = np.linalg.svd(information)
|
||||
tolerance = max(singular_values[0]*1e-9,1e-10)
|
||||
rank = int(np.sum(singular_values>tolerance))
|
||||
covariance = np.linalg.pinv(information,rcond=1e-9)
|
||||
dof = max(_effective_residual_dimension(detail)-len(fit.x),1)
|
||||
condition = (float(singular_values[0]/singular_values[-1])
|
||||
if singular_values[-1]>tolerance else np.inf)
|
||||
return FreeLeverResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
|
||||
lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),
|
||||
final_cost=.5*float(np.dot(rf,rf)),nfev=int(fit.nfev),
|
||||
optimality=float(fit.optimality),chi_square_per_dof=float(np.dot(rf,rf)/dof),
|
||||
position_residual_m=_vector_stats(detail.get('position_physical',[])),
|
||||
velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
|
||||
residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
|
||||
lever_information_singular_values=singular_values,
|
||||
lever_information_condition_number=condition,lever_precision_rank=rank,
|
||||
weakest_lever_direction_I=Vt[-1].copy())
|
||||
|
||||
|
||||
def solve_free_lever_many(problems,initial_l_I_m,max_nfev=120,
|
||||
initial_state_values=None,lever_prior_mean_m=None,
|
||||
lever_prior_covariance_m2=None,return_state_values=False):
|
||||
problems=tuple(problems)
|
||||
initial_l=np.asarray(initial_l_I_m,dtype=float)
|
||||
sizes=[NODE_DOF*len(problem.segment.nodes) for problem in problems]
|
||||
offsets=np.cumsum([3,*sizes])
|
||||
states=([initial_parameters(problem,initial_l) for problem in problems]
|
||||
if initial_state_values is None else
|
||||
[np.asarray(value,dtype=float) for value in initial_state_values])
|
||||
prior_mean=(None if lever_prior_mean_m is None else
|
||||
np.asarray(lever_prior_mean_m,dtype=float))
|
||||
prior_cov=(None if lever_prior_covariance_m2 is None else
|
||||
np.asarray(lever_prior_covariance_m2,dtype=float))
|
||||
prior_whitener=(None if prior_cov is None else
|
||||
np.linalg.inv(np.linalg.cholesky(prior_cov)))
|
||||
x0=np.concatenate([initial_l,*states])
|
||||
def evaluate(value,details=None):
|
||||
chunks=[]
|
||||
for index,problem in enumerate(problems):
|
||||
local={} if details is not None else None
|
||||
chunks.append(residual(problem,value[offsets[index]:offsets[index+1]],
|
||||
details=local,lever_override=value[:3]))
|
||||
if details is not None:
|
||||
for key,items in local.items(): details.setdefault(key,[]).extend(items)
|
||||
if prior_whitener is not None:
|
||||
prior_error=prior_whitener@(value[:3]-prior_mean)
|
||||
chunks.append(prior_error)
|
||||
if details is not None:
|
||||
details.setdefault('lever_prior',[]).extend(prior_error.tolist())
|
||||
return np.concatenate(chunks)
|
||||
r0=evaluate(x0)
|
||||
sparsity=lil_matrix((len(r0),len(x0)),dtype=int)
|
||||
row=0; row_offsets=[0]
|
||||
for index,problem in enumerate(problems):
|
||||
local=jacobian_sparsity(problem,states[index])
|
||||
sparsity[row:row+local.shape[0],:3]=1
|
||||
sparsity[row:row+local.shape[0],offsets[index]:offsets[index+1]]=local
|
||||
row+=local.shape[0]
|
||||
row_offsets.append(row)
|
||||
if prior_whitener is not None:
|
||||
sparsity[row:row+3,:3]=1
|
||||
fit=least_squares(
|
||||
lambda value:evaluate(value),x0,jac='2-point',jac_sparsity=sparsity.tocsr(),
|
||||
method='trf',tr_solver='lsmr',loss='linear',max_nfev=max_nfev,
|
||||
x_scale='jac',ftol=1e-6,xtol=1e-6,gtol=1e-6)
|
||||
detail={}
|
||||
rf=evaluate(fit.x,detail)
|
||||
information=_additive_marginal_lever_information(
|
||||
fit.jac,row_offsets,offsets)
|
||||
if prior_cov is not None:
|
||||
information+=np.linalg.inv(prior_cov)
|
||||
_,singular_values,Vt=np.linalg.svd(information)
|
||||
tolerance=max(singular_values[0]*1e-9,1e-10)
|
||||
rank=int(np.sum(singular_values>tolerance))
|
||||
covariance=np.linalg.pinv(information,rcond=1e-9)
|
||||
dof=max(_effective_residual_dimension(detail)-len(fit.x),1)
|
||||
condition=(float(singular_values[0]/singular_values[-1])
|
||||
if singular_values[-1]>tolerance else np.inf)
|
||||
result=FreeLeverResult(
|
||||
success=bool(fit.success),message=str(fit.message),
|
||||
initial_l_I_m=initial_l,final_l_I_m=fit.x[:3].copy(),
|
||||
lever_step_norm_m=float(np.linalg.norm(fit.x[:3]-initial_l)),
|
||||
initial_cost=.5*float(np.dot(r0,r0)),final_cost=.5*float(np.dot(rf,rf)),
|
||||
nfev=int(fit.nfev),optimality=float(fit.optimality),
|
||||
chi_square_per_dof=float(np.dot(rf,rf)/dof),
|
||||
position_residual_m=_vector_stats(detail.get('position_physical',[])),
|
||||
velocity_residual_m_s=_vector_stats(detail.get('velocity_physical',[])),
|
||||
residual_by_factor=_factor_stats(detail),lever_covariance_m2=covariance,
|
||||
lever_information_singular_values=singular_values,
|
||||
lever_information_condition_number=condition,lever_precision_rank=rank,
|
||||
weakest_lever_direction_I=Vt[-1].copy())
|
||||
if return_state_values:
|
||||
states=[fit.x[offsets[i]:offsets[i+1]].copy()
|
||||
for i in range(len(problems))]
|
||||
return result,states
|
||||
return result
|
||||
@@ -0,0 +1,184 @@
|
||||
"""End-to-end orchestration and JSON reporting for RTK--IMU calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
from dataclasses import asdict, dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from imu_lidar.imu_io import load_imu_samples
|
||||
from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, solve_rtk_imu_rotation
|
||||
from .rtk_imu_translation import TranslationCalibrationResult, solve_rtk_imu_translation
|
||||
from .rtk_io import load_rtk_csv
|
||||
|
||||
|
||||
DEFAULT_RTK_FRAME_DEFINITION = (
|
||||
"right-handed vehicle-fixed frame: +X ANT1(main,left)->ANT2(secondary,right), "
|
||||
"+Y vehicle forward/IMU +Y, +Z vehicle up/IMU +Z"
|
||||
)
|
||||
DEFAULT_RTK_REFERENCE_POINT = (
|
||||
"GGA ANT1/main-antenna phase center, 1.916499878 m above ground"
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class InventoryEntry:
|
||||
session_id: str
|
||||
batch_id: str
|
||||
imu_csv: Path
|
||||
rtk_csv: Path
|
||||
|
||||
|
||||
def load_inventory(path: Path | str) -> list[InventoryEntry]:
|
||||
"""Load the project RTK inventory and derive each paired IMU path."""
|
||||
|
||||
source = Path(path)
|
||||
entries: list[InventoryEntry] = []
|
||||
with source.open("r", encoding="utf-8-sig", newline="") as handle:
|
||||
for row in csv.DictReader(handle):
|
||||
rtk_csv = Path(row["current_rtk_csv"])
|
||||
imu_csv = rtk_csv.with_name("imu.csv")
|
||||
entries.append(
|
||||
InventoryEntry(
|
||||
session_id=row["session"],
|
||||
batch_id=row["batch"],
|
||||
imu_csv=imu_csv,
|
||||
rtk_csv=rtk_csv,
|
||||
)
|
||||
)
|
||||
if not entries:
|
||||
raise ValueError(f"empty RTK inventory: {source}")
|
||||
return entries
|
||||
|
||||
|
||||
def load_sessions(entries: list[InventoryEntry] | tuple[InventoryEntry, ...]) -> list[RotationSession]:
|
||||
sessions = []
|
||||
for entry in entries:
|
||||
sessions.append(
|
||||
RotationSession(
|
||||
session_id=entry.session_id,
|
||||
batch_id=entry.batch_id,
|
||||
imu=load_imu_samples(entry.imu_csv),
|
||||
rtk=load_rtk_csv(entry.rtk_csv),
|
||||
)
|
||||
)
|
||||
return sessions
|
||||
|
||||
|
||||
def _jsonable(value: Any) -> Any:
|
||||
if isinstance(value, np.ndarray):
|
||||
return value.tolist()
|
||||
if isinstance(value, np.generic):
|
||||
return value.item()
|
||||
if isinstance(value, Path):
|
||||
return str(value)
|
||||
if hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (list, tuple)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def dataset_audit(sessions: list[RotationSession]) -> dict[str, Any]:
|
||||
rows = []
|
||||
for session in sessions:
|
||||
rtk = session.rtk
|
||||
valid_position = rtk.position_valid
|
||||
valid_attitude = rtk.attitude_valid & valid_position
|
||||
float_attitude = rtk.attitude_float & valid_position
|
||||
rows.append(
|
||||
{
|
||||
"session_id": session.session_id,
|
||||
"batch_id": session.batch_id,
|
||||
"imu_samples": int(session.imu.t_s.size),
|
||||
"rtk_samples": int(rtk.t_s.size),
|
||||
"fixed_position_ratio": float(np.mean(valid_position)),
|
||||
"fixed_attitude_ratio": float(np.mean(valid_attitude)),
|
||||
"float_attitude_ratio": float(np.mean(float_attitude)),
|
||||
"checksum_valid_ratio": float(np.mean(rtk.checksum_valid)),
|
||||
"common_time_span_s": [
|
||||
float(max(session.imu.t_s[0], rtk.t_s[0])),
|
||||
float(min(session.imu.t_s[-1], rtk.t_s[-1])),
|
||||
],
|
||||
"origin_geodetic": list(rtk.origin_geodetic),
|
||||
"imu_source": str(session.imu.t_s.size) + " normalized samples",
|
||||
"rtk_source": str(rtk.source),
|
||||
}
|
||||
)
|
||||
return {"session_count": len(sessions), "sessions": rows}
|
||||
|
||||
|
||||
def run_calibration(
|
||||
sessions: list[RotationSession],
|
||||
output_directory: Path | str,
|
||||
*,
|
||||
rotation_only: bool = False,
|
||||
compute_loo: bool = True,
|
||||
knot_step_s: float = 2.0,
|
||||
rtk_frame_definition: str = DEFAULT_RTK_FRAME_DEFINITION,
|
||||
rtk_reference_point: str = DEFAULT_RTK_REFERENCE_POINT,
|
||||
) -> tuple[RotationCalibrationResult, TranslationCalibrationResult | None]:
|
||||
"""Run calibration and publish human-readable JSON artifacts."""
|
||||
|
||||
output = Path(output_directory)
|
||||
output.mkdir(parents=True, exist_ok=True)
|
||||
rotation = solve_rtk_imu_rotation(sessions, compute_loo=compute_loo)
|
||||
translation = None
|
||||
if not rotation_only and rotation.ok:
|
||||
translation = solve_rtk_imu_translation(
|
||||
sessions,
|
||||
rotation,
|
||||
knot_step_s=knot_step_s,
|
||||
compute_loo=compute_loo,
|
||||
)
|
||||
audit_payload = dataset_audit(sessions)
|
||||
rotation_payload = _jsonable(rotation)
|
||||
translation_payload = None if translation is None else _jsonable(translation)
|
||||
(output / "dataset_audit.json").write_text(
|
||||
json.dumps(audit_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
(output / "rotation_result.json").write_text(
|
||||
json.dumps(rotation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
if translation_payload is not None:
|
||||
(output / "translation_result.json").write_text(
|
||||
json.dumps(translation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
interpretation_complete = bool(rtk_frame_definition.strip() and rtk_reference_point.strip())
|
||||
accepted = bool(
|
||||
rotation.ok and translation is not None and translation.ok and interpretation_complete
|
||||
)
|
||||
blockers = []
|
||||
if not rotation.ok:
|
||||
blockers.append('full RTK-to-IMU rotation is not observable from the lateral dual-antenna baseline')
|
||||
if translation is None or not translation.ok:
|
||||
blockers.append('translation is frozen until a full rotation is observable and accepted')
|
||||
if not rtk_frame_definition.strip():
|
||||
blockers.append('RTK frame_definition is empty')
|
||||
if not rtk_reference_point.strip():
|
||||
blockers.append('RTK reference_point is empty')
|
||||
summary = {
|
||||
"status": "accepted" if accepted else "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"rtk_frame_definition": rtk_frame_definition,
|
||||
"rtk_reference_point": rtk_reference_point,
|
||||
"interpretation_blockers": blockers,
|
||||
"R_RTK_IMU": rotation.R_RTK_IMU.tolist(),
|
||||
"t_RTK_IMU_m": None if translation is None else translation.t_RTK_IMU_m.tolist(),
|
||||
"T_RTK_IMU": None if translation is None else translation.T_RTK_IMU.tolist(),
|
||||
"rotation_ok": rotation.ok,
|
||||
"translation_ok": None if translation is None else translation.ok,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": None if translation is None else "translation_result.json",
|
||||
"dataset_audit": "dataset_audit.json",
|
||||
}
|
||||
(output / "summary.json").write_text(
|
||||
json.dumps(summary, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
return rotation, translation
|
||||
@@ -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),
|
||||
)
|
||||
@@ -0,0 +1,389 @@
|
||||
"""Lever-arm calibration from RTK positions and full IMU preintegration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.sparse import coo_matrix, csr_matrix, eye
|
||||
from scipy.sparse.linalg import lsqr, splu
|
||||
from scipy.spatial.transform import Rotation, Slerp
|
||||
|
||||
from imu_lidar.geometry import make_transform, orthonormalize_rotation, so3_exp
|
||||
from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, _attitude_rows
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TranslationCalibrationResult:
|
||||
lever_IMU_to_RTK_in_IMU_m: np.ndarray
|
||||
t_RTK_IMU_m: np.ndarray
|
||||
T_RTK_IMU: np.ndarray
|
||||
translation_std_m: np.ndarray
|
||||
lever_information_singular_values: np.ndarray
|
||||
lever_precision_rank: int
|
||||
position_residual_rms_xyz_m: np.ndarray
|
||||
velocity_residual_rms_xyz_m_s: np.ndarray
|
||||
accel_bias_by_session_m_s2: dict[str, np.ndarray]
|
||||
knot_count_by_session: dict[str, int]
|
||||
loo_delta_m: dict[str, np.ndarray]
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _SessionFactors:
|
||||
session: RotationSession
|
||||
knot_t_s: np.ndarray
|
||||
position_enu_m: np.ndarray
|
||||
R_ENU_IMU: np.ndarray
|
||||
delta_p: tuple[np.ndarray, ...]
|
||||
delta_v: tuple[np.ndarray, ...]
|
||||
J_p_ba: tuple[np.ndarray, ...]
|
||||
J_v_ba: tuple[np.ndarray, ...]
|
||||
duration_s: np.ndarray
|
||||
|
||||
|
||||
def _preintegrate_translation_interval(
|
||||
times_s: np.ndarray,
|
||||
gyro_rad_s: np.ndarray,
|
||||
acc_m_s2: np.ndarray,
|
||||
t0: float,
|
||||
t1: float,
|
||||
gyro_bias_rad_s: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, float]:
|
||||
"""Fast nominal ``delta_p/delta_v`` and accel-bias Jacobians.
|
||||
|
||||
Rotation covariance and gyro-bias Jacobians are deliberately omitted here:
|
||||
rotation and gyro bias have already been fixed by Phase R1, while the
|
||||
translation linear system only consumes the accelerometer-bias Jacobians.
|
||||
"""
|
||||
|
||||
left = max(int(np.searchsorted(times_s, t0, side='left') - 1), 0)
|
||||
right = min(int(np.searchsorted(times_s, t1, side='right')), times_s.size - 1)
|
||||
delta_r = np.eye(3)
|
||||
delta_v = np.zeros(3)
|
||||
delta_p = np.zeros(3)
|
||||
j_v_ba = np.zeros((3, 3))
|
||||
j_p_ba = np.zeros((3, 3))
|
||||
for index in range(left, right):
|
||||
sample_t0 = float(times_s[index])
|
||||
sample_t1 = float(times_s[index + 1])
|
||||
if sample_t1 <= t0 or sample_t0 >= t1:
|
||||
continue
|
||||
segment_t0 = max(sample_t0, t0)
|
||||
segment_t1 = min(sample_t1, t1)
|
||||
dt = segment_t1 - segment_t0
|
||||
if dt <= 0.0:
|
||||
continue
|
||||
sample_dt = max(sample_t1 - sample_t0, 1e-12)
|
||||
u0 = (segment_t0 - sample_t0) / sample_dt
|
||||
u1 = (segment_t1 - sample_t0) / sample_dt
|
||||
gyro0 = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
|
||||
gyro1 = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
|
||||
acc0 = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
|
||||
acc1 = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
|
||||
omega = 0.5 * (gyro0 + gyro1) - gyro_bias_rad_s
|
||||
acc = 0.5 * (acc0 + acc1)
|
||||
r_i = delta_r
|
||||
delta_p = delta_p + delta_v * dt + 0.5 * r_i @ acc * dt**2
|
||||
delta_v = delta_v + r_i @ acc * dt
|
||||
j_p_ba = j_p_ba + j_v_ba * dt - 0.5 * r_i * dt**2
|
||||
j_v_ba = j_v_ba - r_i * dt
|
||||
delta_r = orthonormalize_rotation(delta_r @ so3_exp(omega * dt))
|
||||
return delta_p, delta_v, j_p_ba, j_v_ba, float(max(t1 - t0, 0.0))
|
||||
|
||||
|
||||
def _make_session_factors(
|
||||
session: RotationSession,
|
||||
rotation: RotationCalibrationResult,
|
||||
*,
|
||||
knot_step_s: float,
|
||||
) -> _SessionFactors:
|
||||
t_attitude, r_enu_rtk = _attitude_rows(session.rtk, rotation.convention)
|
||||
position_valid = session.rtk.position_valid
|
||||
t_position = session.rtk.t_s[position_valid]
|
||||
position = session.rtk.position_enu_m[position_valid]
|
||||
time_offset_s = rotation.applied_time_offset_s
|
||||
start = max(float(t_attitude[0]), float(t_position[0]), float(session.imu.t_s[0] - time_offset_s))
|
||||
end = min(float(t_attitude[-1]), float(t_position[-1]), float(session.imu.t_s[-1] - time_offset_s))
|
||||
if end - start < 5.0:
|
||||
raise ValueError(f"{session.session_id}: less than 5 s common RTK/IMU support")
|
||||
knot_t = np.arange(start + 0.25, end - 0.25, knot_step_s)
|
||||
if knot_t.size < 4:
|
||||
raise ValueError(f"{session.session_id}: not enough translation knots")
|
||||
position_knots = np.column_stack(
|
||||
[np.interp(knot_t, t_position, position[:, axis]) for axis in range(3)]
|
||||
)
|
||||
r_enu_rtk_knots = Slerp(t_attitude, Rotation.from_matrix(r_enu_rtk))(knot_t).as_matrix()
|
||||
r_enu_imu = r_enu_rtk_knots @ rotation.R_RTK_IMU
|
||||
bg = rotation.gyro_bias_by_session_rad_s[session.session_id]
|
||||
delta_p: list[np.ndarray] = []
|
||||
delta_v: list[np.ndarray] = []
|
||||
j_p_ba: list[np.ndarray] = []
|
||||
j_v_ba: list[np.ndarray] = []
|
||||
durations = []
|
||||
for t0, t1 in zip(knot_t[:-1], knot_t[1:]):
|
||||
dp, dv, jp, jv, duration = _preintegrate_translation_interval(
|
||||
session.imu.t_s,
|
||||
session.imu.gyro_rad_s,
|
||||
session.imu.acc_m_s2,
|
||||
float(t0 + time_offset_s),
|
||||
float(t1 + time_offset_s),
|
||||
bg,
|
||||
)
|
||||
delta_p.append(dp)
|
||||
delta_v.append(dv)
|
||||
j_p_ba.append(jp)
|
||||
j_v_ba.append(jv)
|
||||
durations.append(duration)
|
||||
return _SessionFactors(
|
||||
session=session,
|
||||
knot_t_s=knot_t,
|
||||
position_enu_m=position_knots,
|
||||
R_ENU_IMU=r_enu_imu,
|
||||
delta_p=tuple(delta_p),
|
||||
delta_v=tuple(delta_v),
|
||||
J_p_ba=tuple(j_p_ba),
|
||||
J_v_ba=tuple(j_v_ba),
|
||||
duration_s=np.asarray(durations),
|
||||
)
|
||||
|
||||
|
||||
def _append_block(
|
||||
rows: list[int],
|
||||
cols: list[int],
|
||||
values: list[float],
|
||||
rhs: list[float],
|
||||
groups: list[int],
|
||||
matrix_blocks: list[tuple[int, np.ndarray]],
|
||||
vector: np.ndarray,
|
||||
sigma: np.ndarray,
|
||||
group: int,
|
||||
) -> None:
|
||||
row0 = len(rhs)
|
||||
for axis in range(3):
|
||||
rhs.append(float(vector[axis] / sigma[axis]))
|
||||
groups.append(group)
|
||||
for col0, block in matrix_blocks:
|
||||
for local_col in range(block.shape[1]):
|
||||
value = float(block[axis, local_col] / sigma[axis])
|
||||
if value != 0.0:
|
||||
rows.append(row0 + axis)
|
||||
cols.append(col0 + local_col)
|
||||
values.append(value)
|
||||
|
||||
|
||||
def _build_system(
|
||||
factors: list[_SessionFactors],
|
||||
*,
|
||||
position_sigma_xyz_m: np.ndarray,
|
||||
velocity_sigma_xyz_m_s: np.ndarray,
|
||||
) -> tuple[csr_matrix, np.ndarray, np.ndarray, dict[str, tuple[int, int]], list[tuple[str, int, str]]]:
|
||||
# x = [shared lever(3), per-session ba(3), per-knot velocities(3*K)]
|
||||
offsets: dict[str, tuple[int, int]] = {}
|
||||
variable_count = 3
|
||||
for item in factors:
|
||||
ba_offset = variable_count
|
||||
velocity_offset = ba_offset + 3
|
||||
offsets[item.session.session_id] = (ba_offset, velocity_offset)
|
||||
variable_count = velocity_offset + 3 * item.knot_t_s.size
|
||||
rows: list[int] = []
|
||||
cols: list[int] = []
|
||||
values: list[float] = []
|
||||
rhs: list[float] = []
|
||||
groups: list[int] = []
|
||||
factor_labels: list[tuple[str, int, str]] = []
|
||||
gravity = np.array([0.0, 0.0, -9.80665])
|
||||
group = 0
|
||||
for item in factors:
|
||||
ba_offset, velocity_offset = offsets[item.session.session_id]
|
||||
for index, dt in enumerate(item.duration_s):
|
||||
r_i = item.R_ENU_IMU[index]
|
||||
r_j = item.R_ENU_IMU[index + 1]
|
||||
dp_rtk = item.position_enu_m[index + 1] - item.position_enu_m[index]
|
||||
constant_p = dp_rtk - 0.5 * gravity * dt**2 - r_i @ item.delta_p[index]
|
||||
_append_block(
|
||||
rows,
|
||||
cols,
|
||||
values,
|
||||
rhs,
|
||||
groups,
|
||||
[
|
||||
(0, r_i - r_j),
|
||||
(ba_offset, -r_i @ item.J_p_ba[index]),
|
||||
(velocity_offset + 3 * index, -dt * np.eye(3)),
|
||||
],
|
||||
-constant_p,
|
||||
position_sigma_xyz_m,
|
||||
group,
|
||||
)
|
||||
factor_labels.append((item.session.session_id, group, "position"))
|
||||
group += 1
|
||||
constant_v = -gravity * dt - r_i @ item.delta_v[index]
|
||||
_append_block(
|
||||
rows,
|
||||
cols,
|
||||
values,
|
||||
rhs,
|
||||
groups,
|
||||
[
|
||||
(ba_offset, -r_i @ item.J_v_ba[index]),
|
||||
(velocity_offset + 3 * index, -np.eye(3)),
|
||||
(velocity_offset + 3 * (index + 1), np.eye(3)),
|
||||
],
|
||||
-constant_v,
|
||||
velocity_sigma_xyz_m_s,
|
||||
group,
|
||||
)
|
||||
factor_labels.append((item.session.session_id, group, "velocity"))
|
||||
group += 1
|
||||
# Loose physical bias prior. It prevents an unobservable constant
|
||||
# acceleration from masquerading as gravity while remaining data-led.
|
||||
_append_block(
|
||||
rows,
|
||||
cols,
|
||||
values,
|
||||
rhs,
|
||||
groups,
|
||||
[(ba_offset, np.eye(3))],
|
||||
np.zeros(3),
|
||||
np.full(3, 0.5),
|
||||
group,
|
||||
)
|
||||
factor_labels.append((item.session.session_id, group, "bias_prior"))
|
||||
group += 1
|
||||
matrix = coo_matrix((values, (rows, cols)), shape=(len(rhs), variable_count)).tocsr()
|
||||
return matrix, np.asarray(rhs), np.asarray(groups), offsets, factor_labels
|
||||
|
||||
|
||||
def _irls(matrix: csr_matrix, rhs: np.ndarray, groups: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
row_weights = np.ones(rhs.size)
|
||||
solution = np.zeros(matrix.shape[1])
|
||||
for _ in range(5):
|
||||
weighted = matrix.multiply(row_weights[:, None])
|
||||
solution = lsqr(weighted, rhs * row_weights, atol=1e-10, btol=1e-10, iter_lim=3000)[0]
|
||||
residual = matrix @ solution - rhs
|
||||
new_weights = np.ones_like(row_weights)
|
||||
for group in np.unique(groups):
|
||||
selection = groups == group
|
||||
norm = float(np.linalg.norm(residual[selection]))
|
||||
if norm > 3.0:
|
||||
new_weights[selection] = np.sqrt(3.0 / norm)
|
||||
if np.max(np.abs(new_weights - row_weights)) < 1e-3:
|
||||
row_weights = new_weights
|
||||
break
|
||||
row_weights = new_weights
|
||||
return solution, row_weights
|
||||
|
||||
|
||||
def _solve_factors(
|
||||
factors: list[_SessionFactors],
|
||||
position_sigma: np.ndarray,
|
||||
velocity_sigma: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray, csr_matrix, np.ndarray, dict[str, tuple[int, int]], np.ndarray, np.ndarray]:
|
||||
matrix, rhs, groups, offsets, labels = _build_system(
|
||||
factors,
|
||||
position_sigma_xyz_m=position_sigma,
|
||||
velocity_sigma_xyz_m_s=velocity_sigma,
|
||||
)
|
||||
solution, row_weights = _irls(matrix, rhs, groups)
|
||||
weighted = matrix.multiply(row_weights[:, None]).tocsr()
|
||||
residual = matrix @ solution - rhs
|
||||
data_groups = {group for _, group, kind in labels if kind != "bias_prior"}
|
||||
data_rows = np.isin(groups, list(data_groups))
|
||||
variance = float(np.sum((residual[data_rows] * row_weights[data_rows]) ** 2) / max(np.count_nonzero(data_rows) - solution.size, 1))
|
||||
information = (weighted.T @ weighted).tocsc() + eye(weighted.shape[1], format="csc") * 1e-10
|
||||
h_ll = information[:3, :3].toarray()
|
||||
h_ln = information[:3, 3:]
|
||||
h_nn = information[3:, 3:]
|
||||
nuisance_solve = splu(h_nn).solve(h_ln.T.toarray())
|
||||
schur = h_ll - h_ln.toarray() @ nuisance_solve
|
||||
covariance_lever = np.linalg.pinv(schur, rcond=1e-10) * variance
|
||||
return solution, covariance_lever, matrix, rhs, offsets, groups, residual
|
||||
|
||||
|
||||
def solve_rtk_imu_translation(
|
||||
sessions: list[RotationSession] | tuple[RotationSession, ...],
|
||||
rotation: RotationCalibrationResult,
|
||||
*,
|
||||
knot_step_s: float = 2.0,
|
||||
compute_loo: bool = True,
|
||||
) -> TranslationCalibrationResult:
|
||||
"""Estimate the shared IMU-to-RTK lever arm and return ``T_RTK_IMU``."""
|
||||
|
||||
items = list(sessions)
|
||||
factors = [_make_session_factors(session, rotation, knot_step_s=knot_step_s) for session in items]
|
||||
position_sigma = np.array([0.025, 0.025, 0.060])
|
||||
velocity_sigma = np.array([0.08, 0.08, 0.12])
|
||||
solution, covariance_l, matrix, rhs, offsets, groups, residual = _solve_factors(
|
||||
factors, position_sigma, velocity_sigma
|
||||
)
|
||||
lever = solution[:3]
|
||||
t_rtk_imu = -rotation.R_RTK_IMU @ lever
|
||||
covariance_t = rotation.R_RTK_IMU @ covariance_l @ rotation.R_RTK_IMU.T
|
||||
std_t = np.sqrt(np.maximum(np.diag(covariance_t), 0.0))
|
||||
schur_information = np.linalg.pinv(covariance_l, rcond=1e-12)
|
||||
singular_values = np.linalg.svd(schur_information, compute_uv=False)
|
||||
threshold = max(float(singular_values[0]) * 1e-4, 1e-9)
|
||||
rank = int(np.count_nonzero(singular_values > threshold))
|
||||
|
||||
# Recover physical residuals: system rows are grouped in XYZ triples and
|
||||
# alternate position/velocity, followed by one bias prior per session.
|
||||
position_errors: list[np.ndarray] = []
|
||||
velocity_errors: list[np.ndarray] = []
|
||||
cursor = 0
|
||||
for item in factors:
|
||||
for _ in range(item.knot_t_s.size - 1):
|
||||
position_errors.append(residual[cursor : cursor + 3] * position_sigma)
|
||||
cursor += 3
|
||||
velocity_errors.append(residual[cursor : cursor + 3] * velocity_sigma)
|
||||
cursor += 3
|
||||
cursor += 3
|
||||
pos_rms = np.sqrt(np.mean(np.asarray(position_errors) ** 2, axis=0))
|
||||
vel_rms = np.sqrt(np.mean(np.asarray(velocity_errors) ** 2, axis=0))
|
||||
biases = {
|
||||
item.session.session_id: solution[offsets[item.session.session_id][0] : offsets[item.session.session_id][0] + 3].copy()
|
||||
for item in factors
|
||||
}
|
||||
loo: dict[str, np.ndarray] = {}
|
||||
if compute_loo and len(factors) >= 3:
|
||||
for omitted in factors:
|
||||
kept = [item for item in factors if item.session.session_id != omitted.session.session_id]
|
||||
loo_solution, *_ = _solve_factors(kept, position_sigma, velocity_sigma)
|
||||
loo[omitted.session.session_id] = (-rotation.R_RTK_IMU @ loo_solution[:3]) - t_rtk_imu
|
||||
max_loo_xy = max((float(np.linalg.norm(value[:2])) for value in loo.values()), default=0.0)
|
||||
max_loo_z = max((abs(float(value[2])) for value in loo.values()), default=0.0)
|
||||
ok = bool(
|
||||
rank == 3
|
||||
and float(np.max(std_t[:2])) <= 0.05
|
||||
and float(std_t[2]) <= 0.10
|
||||
and float(np.max(pos_rms[:2])) <= 0.10
|
||||
and float(pos_rms[2]) <= 0.20
|
||||
and max_loo_xy <= 0.10
|
||||
and max_loo_z <= 0.20
|
||||
)
|
||||
notes = [
|
||||
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
|
||||
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
|
||||
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
|
||||
]
|
||||
if not rotation.ok:
|
||||
notes.append("upstream rotation is not accepted, so translation is diagnostic only")
|
||||
ok = False
|
||||
if not ok:
|
||||
notes.append("translation failed one or more strict acceptance gates")
|
||||
return TranslationCalibrationResult(
|
||||
lever_IMU_to_RTK_in_IMU_m=lever,
|
||||
t_RTK_IMU_m=t_rtk_imu,
|
||||
T_RTK_IMU=make_transform(t_rtk_imu, rotation.R_RTK_IMU),
|
||||
translation_std_m=std_t,
|
||||
lever_information_singular_values=singular_values,
|
||||
lever_precision_rank=rank,
|
||||
position_residual_rms_xyz_m=pos_rms,
|
||||
velocity_residual_rms_xyz_m_s=vel_rms,
|
||||
accel_bias_by_session_m_s2=biases,
|
||||
knot_count_by_session={item.session.session_id: int(item.knot_t_s.size) for item in factors},
|
||||
loo_delta_m=loo,
|
||||
ok=ok,
|
||||
notes=tuple(notes),
|
||||
)
|
||||
@@ -0,0 +1,139 @@
|
||||
"""RTK CSV loading for the independent RTK--IMU calibration path."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from imu_lidar.geodesy import geodetic_to_enu
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RtkSeries:
|
||||
"""Normalized RTK observations on the IMU device clock."""
|
||||
|
||||
t_s: np.ndarray
|
||||
attitude_t_s: np.ndarray
|
||||
position_enu_m: np.ndarray
|
||||
heading_deg: np.ndarray
|
||||
pitch_deg: np.ndarray
|
||||
roll_deg: np.ndarray
|
||||
fix_quality: np.ndarray
|
||||
heading_quality: np.ndarray
|
||||
heading_satellites: np.ndarray
|
||||
heading_age_s: np.ndarray
|
||||
hdop: np.ndarray
|
||||
checksum_valid: np.ndarray
|
||||
origin_geodetic: tuple[float, float, float]
|
||||
source: Path
|
||||
|
||||
@property
|
||||
def attitude_valid(self) -> np.ndarray:
|
||||
"""Strict fixed dual-antenna solutions suitable for calibration."""
|
||||
|
||||
return (
|
||||
np.isfinite(self.heading_deg)
|
||||
& np.isfinite(self.pitch_deg)
|
||||
& np.isfinite(self.roll_deg)
|
||||
& (self.heading_quality == 4.0)
|
||||
& self.checksum_valid
|
||||
)
|
||||
|
||||
@property
|
||||
def attitude_float(self) -> np.ndarray:
|
||||
"""Float solutions retained for diagnostics but never calibration."""
|
||||
|
||||
return (
|
||||
np.isfinite(self.heading_deg)
|
||||
& np.isfinite(self.pitch_deg)
|
||||
& np.isfinite(self.roll_deg)
|
||||
& (self.heading_quality == 5.0)
|
||||
& self.checksum_valid
|
||||
)
|
||||
|
||||
@property
|
||||
def position_valid(self) -> np.ndarray:
|
||||
return (
|
||||
np.all(np.isfinite(self.position_enu_m), axis=1)
|
||||
& (self.fix_quality == 4.0)
|
||||
& self.checksum_valid
|
||||
)
|
||||
|
||||
|
||||
def _column(data: np.ndarray, name: str, *, default: float = np.nan) -> np.ndarray:
|
||||
names = set(data.dtype.names or ())
|
||||
if name not in names:
|
||||
return np.full(data.shape[0], default, dtype=float)
|
||||
return np.asarray(data[name], dtype=float).reshape(-1)
|
||||
|
||||
|
||||
def load_rtk_csv(path: Path | str) -> RtkSeries:
|
||||
"""Load an exported G90 RTK CSV and convert its positions to local ENU.
|
||||
|
||||
The required ``t`` column must already be NMEA measurement UTC mapped onto
|
||||
the IMU device clock. Host receive time is deliberately never accepted as
|
||||
a fallback because it is delayed by several seconds in the recorded data.
|
||||
"""
|
||||
|
||||
source = Path(path)
|
||||
if not source.is_file():
|
||||
raise FileNotFoundError(source)
|
||||
data = np.genfromtxt(source, delimiter=",", names=True, dtype=float, encoding="utf-8")
|
||||
if data.ndim == 0:
|
||||
data = np.array([data], dtype=data.dtype)
|
||||
names = set(data.dtype.names or ())
|
||||
required = {"t", "lat_deg", "lon_deg", "altitude_m", "fix_quality"}
|
||||
if not required.issubset(names):
|
||||
raise ValueError(f"RTK CSV must contain {sorted(required)}, got {sorted(names)}")
|
||||
t_s = _column(data, "t")
|
||||
measurement_utc = _column(data, "t_measurement_utc_s")
|
||||
hpr_measurement_utc = _column(data, "hpr_measurement_utc_s")
|
||||
attitude_t = t_s.copy()
|
||||
has_hpr_time = np.isfinite(measurement_utc) & np.isfinite(hpr_measurement_utc)
|
||||
attitude_t[has_hpr_time] += hpr_measurement_utc[has_hpr_time] - measurement_utc[has_hpr_time]
|
||||
order = np.argsort(t_s)
|
||||
position, origin = geodetic_to_enu(
|
||||
_column(data, "lat_deg")[order],
|
||||
_column(data, "lon_deg")[order],
|
||||
_column(data, "altitude_m")[order],
|
||||
)
|
||||
return RtkSeries(
|
||||
t_s=t_s[order],
|
||||
attitude_t_s=attitude_t[order],
|
||||
position_enu_m=position,
|
||||
heading_deg=_column(data, "heading_deg")[order],
|
||||
pitch_deg=_column(data, "pitch_deg")[order],
|
||||
roll_deg=_column(data, "roll_deg")[order],
|
||||
fix_quality=_column(data, "fix_quality", default=0.0)[order],
|
||||
heading_quality=_column(data, "heading_quality", default=0.0)[order],
|
||||
heading_satellites=_column(data, "heading_satellites")[order],
|
||||
heading_age_s=_column(data, "heading_age_s")[order],
|
||||
hdop=_column(data, "hdop")[order],
|
||||
checksum_valid=_column(data, "checksum_valid", default=1.0)[order] == 1.0,
|
||||
origin_geodetic=origin,
|
||||
source=source,
|
||||
)
|
||||
|
||||
|
||||
def longest_valid_interval(t_s: np.ndarray, valid: np.ndarray, *, max_gap_s: float = 0.2) -> tuple[float, float]:
|
||||
"""Return the longest contiguous valid time interval."""
|
||||
|
||||
times = np.asarray(t_s, dtype=float).reshape(-1)
|
||||
mask = np.asarray(valid, dtype=bool).reshape(-1)
|
||||
indices = np.flatnonzero(mask)
|
||||
if indices.size == 0:
|
||||
raise ValueError("no valid RTK samples")
|
||||
best_start = best_end = int(indices[0])
|
||||
start = previous = int(indices[0])
|
||||
for index in indices[1:]:
|
||||
index = int(index)
|
||||
if index != previous + 1 or times[index] - times[previous] > max_gap_s:
|
||||
if times[previous] - times[start] > times[best_end] - times[best_start]:
|
||||
best_start, best_end = start, previous
|
||||
start = index
|
||||
previous = index
|
||||
if times[previous] - times[start] > times[best_end] - times[best_start]:
|
||||
best_start, best_end = start, previous
|
||||
return float(times[best_start]), float(times[best_end])
|
||||
Reference in New Issue
Block a user