978 lines
39 KiB
Python
978 lines
39 KiB
Python
"""Joint extrinsic refinement: Phase-A rotation factors + Phase-C SE(3) IMU factors."""
|
||
|
||
from __future__ import annotations
|
||
|
||
from collections.abc import Callable, Mapping
|
||
from dataclasses import dataclass, field
|
||
from typing import Any
|
||
|
||
import numpy as np
|
||
from scipy.optimize import least_squares
|
||
|
||
from .contracts import ImuSeries, MotionPair
|
||
from .geometry import make_transform, orthonormalize_rotation, so3_exp, so3_log
|
||
from .imu_preintegration import (
|
||
apply_bias_jacobian_correction,
|
||
apply_constant_bias_correction,
|
||
preintegrate_gyro,
|
||
preintegration_rotation_residual,
|
||
residual_whiten_matrix,
|
||
)
|
||
from .observability import ObservabilityReport, analyze_observability
|
||
from .phase_a import phase_a_comparison_to_dict, solve_phase_a_comparison
|
||
from .rotation_handeye import select_strong_rotation_pairs
|
||
|
||
G_NORM = 9.80665
|
||
|
||
|
||
@dataclass(frozen=True)
|
||
class PhaseASessionResult:
|
||
session_id: str
|
||
pair_count: int
|
||
gyro_bias0_rad_s: np.ndarray
|
||
gyro_bias_rad_s: np.ndarray
|
||
residual_rms_deg: float
|
||
residual_median_deg: float
|
||
residual_p95_deg: float
|
||
outlier_fraction_gt_5deg: float
|
||
accepted: bool
|
||
included_in_final: bool
|
||
|
||
|
||
@dataclass(frozen=True)
|
||
class JointExtrinsicResult:
|
||
T_IMU_lidar: np.ndarray
|
||
translation_accepted: bool
|
||
residual_rms_rot_deg: float
|
||
residual_rms_trans_m: float
|
||
observability: ObservabilityReport
|
||
gyro_bias_rad_s: np.ndarray | None = None
|
||
accel_bias_m_s2: np.ndarray | None = None
|
||
gravity_m_s2: np.ndarray | None = None
|
||
gyro_bias_rad_s_per_session: dict[str, np.ndarray] = field(default_factory=dict)
|
||
phase_a_sessions: tuple[PhaseASessionResult, ...] = ()
|
||
phase_a_accepted: bool = False
|
||
phase_a_comparison: dict[str, Any] = field(default_factory=dict)
|
||
notes: tuple[str, ...] = ()
|
||
|
||
|
||
def _pair_weight(pair: MotionPair) -> float:
|
||
weight = float(pair.metadata.get("weight", 1.0))
|
||
if not np.isfinite(weight) or weight <= 0:
|
||
return 1.0
|
||
return weight
|
||
|
||
|
||
def _pair_j_bg(pair: MotionPair) -> np.ndarray | None:
|
||
raw = pair.metadata.get("J_bg")
|
||
if raw is None:
|
||
return None
|
||
return np.asarray(raw, dtype=float).reshape(3, 3)
|
||
|
||
|
||
def _pair_cov(pair: MotionPair) -> np.ndarray:
|
||
raw = pair.metadata.get("cov")
|
||
if raw is None:
|
||
sigma = float(pair.metadata.get("preint_sigma_rad", 1e-2))
|
||
return np.eye(3) * max(sigma, 1e-4) ** 2
|
||
return np.asarray(raw, dtype=float).reshape(3, 3)
|
||
|
||
|
||
def _corrected_delta_r(
|
||
pair: MotionPair,
|
||
delta_bias: np.ndarray,
|
||
*,
|
||
imu: ImuSeries | None,
|
||
bias0: np.ndarray,
|
||
) -> np.ndarray:
|
||
j_bg = _pair_j_bg(pair)
|
||
if j_bg is not None:
|
||
return apply_bias_jacobian_correction(pair.R_A, j_bg, delta_bias)
|
||
if imu is not None and "t_i_imu_s" in pair.metadata and "t_j_imu_s" in pair.metadata:
|
||
preint = preintegrate_gyro(
|
||
imu.t_s,
|
||
imu.gyro_rad_s,
|
||
float(pair.metadata["t_i_imu_s"]),
|
||
float(pair.metadata["t_j_imu_s"]),
|
||
bias0 + delta_bias,
|
||
)
|
||
return preint.delta_R
|
||
duration = float(pair.metadata.get("duration_s", max(pair.t_j_s - pair.t_i_s, 1e-3)))
|
||
return apply_constant_bias_correction(pair.R_A, duration, delta_bias)
|
||
|
||
|
||
def _gravity_basis(g0: np.ndarray) -> np.ndarray:
|
||
"""Return 3×2 orthonormal basis spanning the plane orthogonal to ``g0``."""
|
||
|
||
g = np.asarray(g0, dtype=float).reshape(3)
|
||
n = np.linalg.norm(g)
|
||
if n < 1e-9:
|
||
g = np.array([0.0, 0.0, -G_NORM])
|
||
n = G_NORM
|
||
g = g / n
|
||
axis = np.array([1.0, 0.0, 0.0]) if abs(g[0]) < 0.9 else np.array([0.0, 1.0, 0.0])
|
||
e1 = np.cross(g, axis)
|
||
e1 /= max(np.linalg.norm(e1), 1e-12)
|
||
e2 = np.cross(g, e1)
|
||
return np.column_stack([e1, e2])
|
||
|
||
|
||
def _gravity_from_params(xy: np.ndarray, g0: np.ndarray, basis: np.ndarray) -> np.ndarray:
|
||
raw = np.asarray(g0, dtype=float).reshape(3) + basis @ np.asarray(xy, dtype=float).reshape(2)
|
||
n = float(np.linalg.norm(raw))
|
||
if n < 1e-9:
|
||
return np.asarray(g0, dtype=float).reshape(3)
|
||
return raw * (G_NORM / n)
|
||
|
||
|
||
def _lidar_to_imu_relative(r_x: np.ndarray, t_x: np.ndarray, r_b: np.ndarray, t_b: np.ndarray):
|
||
"""Map LiDAR relative pose to IMU: ``T_A = T_X T_B T_X^{-1}``."""
|
||
|
||
r_a = orthonormalize_rotation(r_x @ r_b @ r_x.T)
|
||
t_a = (np.eye(3) - r_a) @ t_x + r_x @ t_b
|
||
return r_a, t_a
|
||
|
||
|
||
def _corrected_preint_quantities(
|
||
pair: MotionPair,
|
||
bg_i: np.ndarray,
|
||
ba_i: np.ndarray,
|
||
bg0: np.ndarray,
|
||
ba0: np.ndarray,
|
||
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||
"""First-order correct ΔR/Δv/Δp for keyframe biases vs preintegration biases."""
|
||
|
||
dbg = np.asarray(bg_i, dtype=float).reshape(3) - np.asarray(bg0, dtype=float).reshape(3)
|
||
dba = np.asarray(ba_i, dtype=float).reshape(3) - np.asarray(ba0, dtype=float).reshape(3)
|
||
j_bg = pair.metadata.get("J_bg9")
|
||
j_ba = pair.metadata.get("J_ba")
|
||
delta_v0 = np.asarray(pair.metadata.get("delta_v", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
|
||
delta_p0 = (
|
||
np.asarray(pair.t_A_m, dtype=float).reshape(3)
|
||
if pair.t_A_m is not None
|
||
else np.asarray(pair.metadata.get("delta_p", [0.0, 0.0, 0.0]), dtype=float).reshape(3)
|
||
)
|
||
if j_bg is None or j_ba is None:
|
||
delta_r = apply_bias_jacobian_correction(
|
||
pair.R_A,
|
||
_pair_j_bg(pair) if _pair_j_bg(pair) is not None else np.zeros((3, 3)),
|
||
dbg,
|
||
)
|
||
return delta_r, delta_v0, delta_p0
|
||
j_bg_m = np.asarray(j_bg, dtype=float).reshape(9, 3)
|
||
j_ba_m = np.asarray(j_ba, dtype=float).reshape(9, 3)
|
||
delta_r = orthonormalize_rotation(pair.R_A @ so3_exp(j_bg_m[0:3] @ dbg))
|
||
delta_v = delta_v0 + j_bg_m[3:6] @ dbg + j_ba_m[3:6] @ dba
|
||
delta_p = delta_p0 + j_bg_m[6:9] @ dbg + j_ba_m[6:9] @ dba
|
||
return delta_r, delta_v, delta_p
|
||
|
||
|
||
def _build_nav_rotations(
|
||
keyframe_ids: list[int],
|
||
id_to_idx: dict[int, int],
|
||
consecutive_pairs: dict[tuple[int, int], MotionPair],
|
||
r_x: np.ndarray,
|
||
t_x: np.ndarray,
|
||
) -> list[np.ndarray]:
|
||
"""Chain IMU orientations; restart at session/gap boundaries (no cross-link)."""
|
||
|
||
del id_to_idx
|
||
rotations = [np.eye(3) for _ in keyframe_ids]
|
||
for k in range(len(keyframe_ids) - 1):
|
||
a = keyframe_ids[k]
|
||
b = keyframe_ids[k + 1]
|
||
pair = consecutive_pairs.get((a, b))
|
||
if pair is None:
|
||
# Missing link or new session: start a fresh nav chain.
|
||
rotations[k + 1] = np.eye(3)
|
||
continue
|
||
t_b = np.zeros(3) if pair.t_B_m is None else np.asarray(pair.t_B_m, dtype=float)
|
||
r_meas, _ = _lidar_to_imu_relative(r_x, t_x, pair.R_B, t_b)
|
||
rotations[k + 1] = orthonormalize_rotation(rotations[k] @ r_meas)
|
||
return rotations
|
||
|
||
|
||
def _solve_phase_c_se3(
|
||
pairs: list[MotionPair],
|
||
r_x: np.ndarray,
|
||
*,
|
||
gyro_bias_linearization: np.ndarray,
|
||
gyro_bias_init: np.ndarray,
|
||
gravity_init: np.ndarray,
|
||
sigma_bg_rw: float = 1.0e-5,
|
||
sigma_ba_rw: float = 1.0e-3,
|
||
t_init: np.ndarray | None = None,
|
||
t_prior: np.ndarray | None = None,
|
||
t_prior_sigma_m: np.ndarray | float | None = None,
|
||
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, float, float, list[str]]:
|
||
"""Keyframe IMU factor optimization for full SE(3)."""
|
||
|
||
notes: list[str] = []
|
||
usable = [pair for pair in pairs if pair.t_B_m is not None and "delta_v" in pair.metadata]
|
||
if len(usable) < 3:
|
||
notes.append("phase-C skipped: need pairs with full preintegration metadata")
|
||
t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
|
||
return r_x, t0, gravity_init, gyro_bias_init, np.zeros(3), 1e9, 1e9, notes
|
||
|
||
# Keyframes: group by session, sort each session by IMU time (no cross-session chain).
|
||
stamp: dict[int, float] = {}
|
||
kf_session: dict[int, str] = {}
|
||
for pair in usable:
|
||
stamp[pair.i] = float(pair.metadata.get("t_i_imu_s", pair.t_i_s))
|
||
stamp[pair.j] = float(pair.metadata.get("t_j_imu_s", pair.t_j_s))
|
||
kf_session[pair.i] = pair.session_id
|
||
kf_session[pair.j] = pair.session_id
|
||
session_ids = sorted(set(kf_session.values()))
|
||
keyframe_ids: list[int] = []
|
||
for sid in session_ids:
|
||
local = [kid for kid, sess in kf_session.items() if sess == sid]
|
||
local.sort(key=lambda kid: stamp[kid])
|
||
keyframe_ids.extend(local)
|
||
k_count = len(keyframe_ids)
|
||
id_to_idx = {kid: idx for idx, kid in enumerate(keyframe_ids)}
|
||
|
||
consecutive_pairs: dict[tuple[int, int], MotionPair] = {}
|
||
for pair in usable:
|
||
if kf_session.get(pair.i) != kf_session.get(pair.j):
|
||
continue
|
||
if id_to_idx[pair.j] == id_to_idx[pair.i] + 1:
|
||
consecutive_pairs[(pair.i, pair.j)] = pair
|
||
notes.append(
|
||
f"phase-C multi-session graph: sessions={len(session_ids)}, "
|
||
f"keyframes={k_count}, consecutive_links={len(consecutive_pairs)}"
|
||
)
|
||
|
||
g0 = np.asarray(gravity_init, dtype=float).reshape(3)
|
||
if np.linalg.norm(g0) < 1e-6:
|
||
g0 = np.array([0.0, 0.0, -G_NORM])
|
||
g0 = g0 * (G_NORM / max(np.linalg.norm(g0), 1e-9))
|
||
basis = _gravity_basis(g0)
|
||
ba0 = np.zeros(3)
|
||
bg0 = np.asarray(gyro_bias_linearization, dtype=float).reshape(3)
|
||
bg_init = np.asarray(gyro_bias_init, dtype=float).reshape(3)
|
||
|
||
# State: dθ(3), t(3), g_xy(2), v(3K), bg(3K), ba(3K)
|
||
n_v = 3 * k_count
|
||
n_b = 3 * k_count
|
||
dim = 3 + 3 + 2 + n_v + n_b + n_b
|
||
x0 = np.zeros(dim)
|
||
t0 = np.zeros(3) if t_init is None else np.asarray(t_init, dtype=float).reshape(3)
|
||
x0[3:6] = t0
|
||
t_prior_vec = None if t_prior is None else np.asarray(t_prior, dtype=float).reshape(3)
|
||
if t_prior_sigma_m is None:
|
||
t_sigma = np.array([0.05, 0.05, 0.05], dtype=float)
|
||
else:
|
||
t_sigma = np.asarray(t_prior_sigma_m, dtype=float).reshape(-1)
|
||
if t_sigma.size == 1:
|
||
t_sigma = np.full(3, float(t_sigma[0]), dtype=float)
|
||
# velocities start at 0; biases at prior
|
||
for idx in range(k_count):
|
||
x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg_init
|
||
|
||
whitened = []
|
||
for pair in usable:
|
||
cov9 = pair.metadata.get("cov9")
|
||
if cov9 is None:
|
||
cov = _pair_cov(pair)
|
||
cov9_m = np.eye(9)
|
||
cov9_m[0:3, 0:3] = cov
|
||
cov9_m[3:6, 3:6] = np.eye(3) * 0.25
|
||
cov9_m[6:9, 6:9] = np.eye(3) * 1.0
|
||
else:
|
||
cov9_m = np.asarray(cov9, dtype=float).reshape(9, 9)
|
||
whitened.append(residual_whiten_matrix(cov9_m))
|
||
|
||
def unpack(vec: np.ndarray):
|
||
r_opt = orthonormalize_rotation(so3_exp(vec[0:3]) @ r_x)
|
||
t_opt = vec[3:6]
|
||
g_opt = _gravity_from_params(vec[6:8], g0, basis)
|
||
base = 8
|
||
vels = vec[base : base + n_v].reshape(k_count, 3)
|
||
base += n_v
|
||
bgs = vec[base : base + n_b].reshape(k_count, 3)
|
||
base += n_b
|
||
bas = vec[base : base + n_b].reshape(k_count, 3)
|
||
return r_opt, t_opt, g_opt, vels, bgs, bas
|
||
|
||
def residuals(vec: np.ndarray) -> np.ndarray:
|
||
r_opt, t_opt, g_opt, vels, bgs, bas = unpack(vec)
|
||
nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
|
||
out: list[np.ndarray] = []
|
||
|
||
for pair, whiten in zip(usable, whitened):
|
||
i_idx = id_to_idx[pair.i]
|
||
j_idx = id_to_idx[pair.j]
|
||
dt = float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s))
|
||
dt = max(dt, 1e-3)
|
||
delta_r, delta_v, delta_p = _corrected_preint_quantities(
|
||
pair, bgs[i_idx], bas[i_idx], bg0, ba0
|
||
)
|
||
t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
|
||
r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
|
||
r_i = nav_r[i_idx]
|
||
v_i = vels[i_idx]
|
||
v_j = vels[j_idx]
|
||
|
||
err_r = so3_log(delta_r.T @ r_meas)
|
||
err_v = v_j - v_i - g_opt * dt - r_i @ delta_v
|
||
err_p = r_i @ (t_meas - delta_p) - v_i * dt - 0.5 * g_opt * (dt**2)
|
||
err = np.concatenate([err_r, err_v, err_p])
|
||
w = np.sqrt(_pair_weight(pair))
|
||
out.append(w * (whiten @ err))
|
||
|
||
# Bias random-walk between consecutive keyframes (same session only).
|
||
for k in range(k_count - 1):
|
||
a = keyframe_ids[k]
|
||
b = keyframe_ids[k + 1]
|
||
if kf_session.get(a) != kf_session.get(b):
|
||
continue
|
||
dt = max(stamp[b] - stamp[a], 1e-3)
|
||
scale_g = 1.0 / (max(sigma_bg_rw, 1e-8) * np.sqrt(dt))
|
||
scale_a = 1.0 / (max(sigma_ba_rw, 1e-8) * np.sqrt(dt))
|
||
out.append(scale_g * (bgs[k + 1] - bgs[k]))
|
||
out.append(scale_a * (bas[k + 1] - bas[k]))
|
||
|
||
# Weak priors: first keyframe of each session + CAD/installation translation.
|
||
for sid in session_ids:
|
||
first = next(kid for kid in keyframe_ids if kf_session[kid] == sid)
|
||
idx0 = id_to_idx[first]
|
||
out.append(50.0 * (bgs[idx0] - bg_init))
|
||
out.append(20.0 * bas[idx0])
|
||
if t_prior_vec is not None:
|
||
out.append((t_opt - t_prior_vec) / np.maximum(t_sigma, 1e-3))
|
||
else:
|
||
out.append(0.2 * t_opt) # soft |t|~0 prior when no CAD prior
|
||
return np.concatenate(out)
|
||
|
||
# Cap evaluations: Phase-C is high-dimensional; synthetic ICP already dominates runtime.
|
||
opt = least_squares(residuals, x0, loss="huber", f_scale=0.05, max_nfev=80)
|
||
r_opt, t_opt, g_opt, vels, bgs, bas = unpack(opt.x)
|
||
|
||
rot_errs = []
|
||
trans_errs = []
|
||
nav_r = _build_nav_rotations(keyframe_ids, id_to_idx, consecutive_pairs, r_opt, t_opt)
|
||
for pair in usable:
|
||
i_idx = id_to_idx[pair.i]
|
||
j_idx = id_to_idx[pair.j]
|
||
dt = max(float(pair.metadata.get("duration_s", pair.t_j_s - pair.t_i_s)), 1e-3)
|
||
delta_r, delta_v, delta_p = _corrected_preint_quantities(
|
||
pair, bgs[i_idx], bas[i_idx], bg0, ba0
|
||
)
|
||
t_b = np.asarray(pair.t_B_m, dtype=float).reshape(3)
|
||
r_meas, t_meas = _lidar_to_imu_relative(r_opt, t_opt, pair.R_B, t_b)
|
||
r_i = nav_r[i_idx]
|
||
err_r = so3_log(delta_r.T @ r_meas)
|
||
err_p = r_i @ (t_meas - delta_p) - vels[i_idx] * dt - 0.5 * g_opt * (dt**2)
|
||
rot_errs.append(np.degrees(np.linalg.norm(err_r)))
|
||
trans_errs.append(float(np.linalg.norm(err_p)))
|
||
del delta_v, j_idx
|
||
|
||
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
|
||
trans_rms = float(np.sqrt(np.mean(np.square(trans_errs)))) if trans_errs else 1e9
|
||
bg_mean = np.mean(bgs, axis=0)
|
||
ba_mean = np.mean(bas, axis=0)
|
||
notes.append(
|
||
"phase-C SE3 (Δv/Δp + g + keyframe v/bias RW): "
|
||
f"keyframes={k_count}, pairs={len(usable)}, "
|
||
f"|t|={float(np.linalg.norm(t_opt)):.3f} m, "
|
||
f"|g|={float(np.linalg.norm(g_opt)):.3f}, "
|
||
f"trans_rms={trans_rms:.3f} m"
|
||
)
|
||
return r_opt, t_opt, g_opt, bg_mean, ba_mean, rot_rms, trans_rms, notes
|
||
|
||
|
||
def _pair_gyro_bias0(pair: MotionPair, fallback: np.ndarray) -> np.ndarray:
|
||
raw = pair.metadata.get("gyro_bias0_rad_s")
|
||
if raw is None:
|
||
return np.asarray(fallback, dtype=float).reshape(3)
|
||
return np.asarray(raw, dtype=float).reshape(3)
|
||
|
||
|
||
def _phase_a_bias_bases(
|
||
pairs: list[MotionPair],
|
||
*,
|
||
gyro_bias_rad_s: np.ndarray | None,
|
||
gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None,
|
||
) -> dict[str, np.ndarray]:
|
||
session_ids = sorted({pair.session_id for pair in pairs})
|
||
scalar = None
|
||
if gyro_bias_rad_s is not None:
|
||
scalar = np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||
supplied = {} if gyro_bias_rad_s_by_session is None else gyro_bias_rad_s_by_session
|
||
bases: dict[str, np.ndarray] = {}
|
||
for sid in session_ids:
|
||
if sid in supplied:
|
||
bases[sid] = np.asarray(supplied[sid], dtype=float).reshape(3)
|
||
continue
|
||
pair = next(
|
||
(
|
||
item
|
||
for item in pairs
|
||
if item.session_id == sid and "gyro_bias0_rad_s" in item.metadata
|
||
),
|
||
None,
|
||
)
|
||
if pair is not None:
|
||
bases[sid] = np.asarray(pair.metadata["gyro_bias0_rad_s"], dtype=float).reshape(3)
|
||
elif scalar is not None:
|
||
bases[sid] = scalar.copy()
|
||
else:
|
||
bases[sid] = np.zeros(3)
|
||
return bases
|
||
|
||
|
||
def _rotation_distribution(errs_deg: list[float]) -> tuple[float, float, float, float, bool]:
|
||
if not errs_deg:
|
||
return 1e9, 1e9, 1e9, 1.0, False
|
||
errs = np.asarray(errs_deg, dtype=float)
|
||
rms = float(np.sqrt(np.mean(errs**2)))
|
||
median = float(np.median(errs))
|
||
p95 = float(np.percentile(errs, 95.0))
|
||
outlier_fraction = float(np.mean(errs > 5.0))
|
||
accepted = (
|
||
len(errs) >= 3
|
||
and rms < 1.5
|
||
and median < 0.5
|
||
and p95 < 1.5
|
||
and outlier_fraction <= 0.005
|
||
)
|
||
return rms, median, p95, outlier_fraction, accepted
|
||
|
||
|
||
def _solve_phase_a_rotation(
|
||
pairs: list[MotionPair],
|
||
r_seed: np.ndarray,
|
||
*,
|
||
bias_bases: Mapping[str, np.ndarray],
|
||
imu: ImuSeries | None,
|
||
bias_prior_sigma_rad_s: float,
|
||
preexcluded_session_ids: set[str] | None = None,
|
||
) -> tuple[
|
||
np.ndarray,
|
||
dict[str, np.ndarray],
|
||
tuple[PhaseASessionResult, ...],
|
||
list[MotionPair],
|
||
float,
|
||
bool,
|
||
list[str],
|
||
]:
|
||
notes: list[str] = []
|
||
all_session_ids = sorted({pair.session_id for pair in pairs})
|
||
prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
|
||
|
||
def optimize(
|
||
active_pairs: list[MotionPair],
|
||
r0: np.ndarray,
|
||
bias_seed: Mapping[str, np.ndarray],
|
||
) -> tuple[np.ndarray, dict[str, np.ndarray]]:
|
||
session_ids = sorted({pair.session_id for pair in active_pairs})
|
||
session_index = {sid: index for index, sid in enumerate(session_ids)}
|
||
whiten = [residual_whiten_matrix(_pair_cov(pair)) for pair in active_pairs]
|
||
x0 = np.zeros(3 + 3 * len(session_ids))
|
||
for sid, index in session_index.items():
|
||
x0[3 + 3 * index : 6 + 3 * index] = np.asarray(bias_seed[sid], dtype=float)
|
||
|
||
def residual(vec: np.ndarray) -> np.ndarray:
|
||
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r0)
|
||
out: list[np.ndarray] = []
|
||
for pair, sqrt_info in zip(active_pairs, whiten):
|
||
index = session_index[pair.session_id]
|
||
bias = vec[3 + 3 * index : 6 + 3 * index]
|
||
base = _pair_gyro_bias0(pair, bias_bases[pair.session_id])
|
||
delta_r = _corrected_delta_r(
|
||
pair, bias - base, imu=imu, bias0=base
|
||
)
|
||
out.append(
|
||
sqrt_info
|
||
@ preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||
)
|
||
for sid, index in session_index.items():
|
||
bias = vec[3 + 3 * index : 6 + 3 * index]
|
||
out.append(prior_w * (bias - bias_bases[sid]))
|
||
return np.concatenate(out)
|
||
|
||
opt = least_squares(residual, x0, loss="huber", f_scale=1.0, max_nfev=200)
|
||
r_opt = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r0)
|
||
biases = {
|
||
sid: opt.x[3 + 3 * index : 6 + 3 * index].copy()
|
||
for sid, index in session_index.items()
|
||
}
|
||
return r_opt, biases
|
||
|
||
def summarize(
|
||
r_opt: np.ndarray,
|
||
biases: Mapping[str, np.ndarray],
|
||
included: set[str],
|
||
) -> tuple[PhaseASessionResult, ...]:
|
||
results: list[PhaseASessionResult] = []
|
||
for sid in all_session_ids:
|
||
local_pairs = [pair for pair in pairs if pair.session_id == sid]
|
||
bias = np.asarray(biases.get(sid, bias_bases[sid]), dtype=float).reshape(3)
|
||
errs: list[float] = []
|
||
for pair in local_pairs:
|
||
base = _pair_gyro_bias0(pair, bias_bases[sid])
|
||
delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base)
|
||
err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||
errs.append(float(np.degrees(np.linalg.norm(err))))
|
||
rms, median, p95, outlier, accepted = _rotation_distribution(errs)
|
||
results.append(
|
||
PhaseASessionResult(
|
||
session_id=sid,
|
||
pair_count=len(local_pairs),
|
||
gyro_bias0_rad_s=np.asarray(bias_bases[sid], dtype=float),
|
||
gyro_bias_rad_s=bias,
|
||
residual_rms_deg=rms,
|
||
residual_median_deg=median,
|
||
residual_p95_deg=p95,
|
||
outlier_fraction_gt_5deg=outlier,
|
||
accepted=accepted,
|
||
included_in_final=sid in included,
|
||
)
|
||
)
|
||
return tuple(results)
|
||
|
||
if not pairs:
|
||
return r_seed, dict(bias_bases), (), [], 1e9, False, ["no pairs for phase-A"]
|
||
|
||
r_first, biases_first = optimize(pairs, r_seed, bias_bases)
|
||
first = summarize(r_first, biases_first, set(all_session_ids))
|
||
accepted_ids = {item.session_id for item in first if item.accepted}
|
||
preexcluded = set() if preexcluded_session_ids is None else set(preexcluded_session_ids)
|
||
accepted_ids -= preexcluded
|
||
active_ids = set(all_session_ids)
|
||
r_final = r_first
|
||
biases_final = dict(biases_first)
|
||
if preexcluded and not accepted_ids:
|
||
active_ids = set()
|
||
notes.append(f"phase-A pre-gate excluded all sessions: {sorted(preexcluded)}")
|
||
elif accepted_ids and accepted_ids != active_ids:
|
||
active_ids = accepted_ids
|
||
active_pairs = [pair for pair in pairs if pair.session_id in active_ids]
|
||
r_final, active_biases = optimize(active_pairs, r_first, biases_first)
|
||
biases_final.update(active_biases)
|
||
excluded = sorted(set(all_session_ids) - active_ids)
|
||
notes.append(f"phase-A excluded sessions after local/pre residual gate: {excluded}")
|
||
active_pairs = [pair for pair in pairs if pair.session_id in active_ids]
|
||
final = summarize(r_final, biases_final, active_ids)
|
||
active_results = [item for item in final if item.included_in_final]
|
||
global_errs: list[float] = []
|
||
for pair in active_pairs:
|
||
bias = biases_final[pair.session_id]
|
||
base = _pair_gyro_bias0(pair, bias_bases[pair.session_id])
|
||
delta_r = _corrected_delta_r(pair, bias - base, imu=imu, bias0=base)
|
||
err = preintegration_rotation_residual(delta_r, r_final, pair.R_B)
|
||
global_errs.append(float(np.degrees(np.linalg.norm(err))))
|
||
rot_rms, _, _, _, global_ok = _rotation_distribution(global_errs)
|
||
accepted = bool(active_results and global_ok and all(item.accepted for item in active_results))
|
||
notes.append(
|
||
f"phase-A session-local bias refine: sessions={len(active_ids)}/{len(all_session_ids)}, "
|
||
f"pairs={len(active_pairs)}, rms={rot_rms:.3f} deg"
|
||
)
|
||
return r_final, biases_final, final, active_pairs, rot_rms, accepted, notes
|
||
|
||
|
||
def _solve_joint_extrinsic_legacy(
|
||
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||
r_x: np.ndarray,
|
||
*,
|
||
force_rotation_only: bool = False,
|
||
imu: ImuSeries | None = None,
|
||
delta_t_s: float = 0.0,
|
||
gyro_bias_rad_s: np.ndarray | None = None,
|
||
gravity_init_m_s2: np.ndarray | None = None,
|
||
gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None,
|
||
time_offset_s_by_session: Mapping[str, float] | None = None,
|
||
bias_prior_sigma_rad_s: float = 0.02,
|
||
enable_phase_c: bool | None = None,
|
||
t_init_m: np.ndarray | None = None,
|
||
t_prior_m: np.ndarray | None = None,
|
||
t_prior_sigma_m: np.ndarray | float | None = None,
|
||
) -> JointExtrinsicResult:
|
||
"""Refine extrinsic using Phase-A whitened rotation factors, optional Phase-C SE(3)."""
|
||
|
||
del delta_t_s # reserved for future SE(3) time coupling
|
||
if enable_phase_c is None:
|
||
enable_phase_c = not force_rotation_only
|
||
|
||
usable = [pair for pair in pairs if pair.t_B_m is not None]
|
||
observability = analyze_observability(usable, r_x)
|
||
notes = list(observability.notes)
|
||
|
||
r = orthonormalize_rotation(np.asarray(r_x, dtype=float))
|
||
bias0 = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float).reshape(3)
|
||
t_seed = None if t_init_m is None else np.asarray(t_init_m, dtype=float).reshape(3)
|
||
weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
|
||
whitens = [residual_whiten_matrix(_pair_cov(pair)) for pair in usable]
|
||
prior_w = 1.0 / max(bias_prior_sigma_rad_s, 1e-4)
|
||
|
||
def rotation_residuals(r_opt: np.ndarray, delta_bias: np.ndarray) -> np.ndarray:
|
||
residuals = []
|
||
for pair, whiten in zip(usable, whitens):
|
||
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||
err = preintegration_rotation_residual(delta_r, r_opt, pair.R_B)
|
||
residuals.append(whiten @ err)
|
||
residuals.append(prior_w * delta_bias)
|
||
return np.concatenate(residuals) if residuals else np.zeros(0)
|
||
|
||
def residual_rot_bias(vec: np.ndarray) -> np.ndarray:
|
||
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
|
||
return rotation_residuals(r_opt, vec[3:])
|
||
|
||
if usable:
|
||
opt = least_squares(
|
||
residual_rot_bias,
|
||
np.zeros(6),
|
||
loss="huber",
|
||
f_scale=1.0,
|
||
max_nfev=200,
|
||
)
|
||
r = orthonormalize_rotation(so3_exp(opt.x[:3]) @ r)
|
||
delta_bias = opt.x[3:]
|
||
bias_out = bias0 + delta_bias
|
||
notes.append(
|
||
"phase-A joint refine (single Σ whitening + J_bg): "
|
||
f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, "
|
||
f"pairs={len(usable)}"
|
||
)
|
||
else:
|
||
bias_out = bias0
|
||
delta_bias = np.zeros(3)
|
||
notes.append("no pairs for joint refine")
|
||
|
||
rot_errs = []
|
||
for pair in usable:
|
||
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||
err = preintegration_rotation_residual(delta_r, r, pair.R_B)
|
||
rot_errs.append(np.degrees(np.linalg.norm(err)))
|
||
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs)))) if rot_errs else 1e9
|
||
|
||
t = np.zeros(3) if t_seed is None else t_seed.copy()
|
||
translation_accepted = False
|
||
trans_rms = 1e9
|
||
gravity_out: np.ndarray | None = None
|
||
accel_bias_out: np.ndarray | None = None
|
||
|
||
if gravity_init_m_s2 is None:
|
||
gravity_init = np.array([0.0, 0.0, -G_NORM])
|
||
else:
|
||
gravity_init = np.asarray(gravity_init_m_s2, dtype=float).reshape(3)
|
||
|
||
if t_prior_m is not None:
|
||
notes.append(
|
||
"using CAD/installation translation prior "
|
||
f"t={np.asarray(t_prior_m, dtype=float).reshape(3).tolist()}"
|
||
)
|
||
|
||
if (
|
||
enable_phase_c
|
||
and not force_rotation_only
|
||
and observability.translation_observable
|
||
and observability.rotation_observable
|
||
and len(usable) >= 5
|
||
):
|
||
r, t, gravity_out, bias_out, accel_bias_out, rot_rms, trans_rms, c_notes = _solve_phase_c_se3(
|
||
usable,
|
||
r,
|
||
gyro_bias_linearization=bias0,
|
||
gyro_bias_init=bias_out,
|
||
gravity_init=gravity_init,
|
||
t_init=t_seed if t_seed is not None else t_prior_m,
|
||
t_prior=t_prior_m,
|
||
t_prior_sigma_m=t_prior_sigma_m,
|
||
)
|
||
notes.extend(c_notes)
|
||
translation_accepted = bool(trans_rms < 0.75 and np.linalg.norm(t) > 1e-4)
|
||
if not translation_accepted:
|
||
# Prefer CAD prior over silent zero when motion SE3 is rejected.
|
||
if t_prior_m is not None:
|
||
t = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||
notes.append(
|
||
"phase-C translation residual/gate failed; CAD translation is reported "
|
||
"as a prior only and is not accepted as calibration"
|
||
)
|
||
else:
|
||
notes.append("phase-C translation residual/gate failed; keeping translation at zero")
|
||
t = np.zeros(3)
|
||
elif (
|
||
not force_rotation_only
|
||
and observability.translation_observable
|
||
and observability.rotation_observable
|
||
and len(usable) >= 5
|
||
):
|
||
# Legacy hand-eye translation fallback when Phase-C metadata missing.
|
||
def residual_se3(vec: np.ndarray) -> np.ndarray:
|
||
r_opt = orthonormalize_rotation(so3_exp(vec[:3]) @ r)
|
||
t_opt = vec[3:]
|
||
residuals = []
|
||
for pair, weight, whiten in zip(usable, weights, whitens):
|
||
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||
residuals.append(
|
||
np.sqrt(weight) * (whiten @ preintegration_rotation_residual(delta_r, r_opt, pair.R_B))
|
||
)
|
||
pred = (pair.R_A - np.eye(3)) @ t_opt
|
||
meas = r_opt @ np.asarray(pair.t_B_m, dtype=float)
|
||
residuals.append(np.sqrt(weight) * (pred - meas))
|
||
if t_prior_m is not None:
|
||
sigma = np.asarray(t_prior_sigma_m if t_prior_sigma_m is not None else 0.05, dtype=float)
|
||
if sigma.size == 1:
|
||
sigma = np.full(3, float(sigma), dtype=float)
|
||
residuals.append((t_opt - np.asarray(t_prior_m, dtype=float).reshape(3)) / np.maximum(sigma, 1e-3))
|
||
return np.concatenate(residuals)
|
||
|
||
x_se3 = np.zeros(6)
|
||
if t_seed is not None:
|
||
x_se3[3:] = t_seed
|
||
elif t_prior_m is not None:
|
||
x_se3[3:] = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||
opt_t = least_squares(residual_se3, x_se3, loss="huber", f_scale=0.05, max_nfev=200)
|
||
r = orthonormalize_rotation(so3_exp(opt_t.x[:3]) @ r)
|
||
t = opt_t.x[3:]
|
||
rot_errs = []
|
||
trans_errs = []
|
||
for pair in usable:
|
||
delta_r = _corrected_delta_r(pair, delta_bias, imu=imu, bias0=bias0)
|
||
rot_errs.append(np.degrees(np.linalg.norm(preintegration_rotation_residual(delta_r, r, pair.R_B))))
|
||
pred = (pair.R_A - np.eye(3)) @ t
|
||
meas = r @ np.asarray(pair.t_B_m, dtype=float)
|
||
trans_errs.append(np.linalg.norm(pred - meas))
|
||
rot_rms = float(np.sqrt(np.mean(np.square(rot_errs))))
|
||
trans_rms = float(np.sqrt(np.mean(np.square(trans_errs))))
|
||
translation_accepted = trans_rms < 0.5
|
||
notes.append(f"legacy translation refine rms={trans_rms:.3f} m")
|
||
if not translation_accepted:
|
||
notes.append("translation residual too large; keeping translation at zero")
|
||
t = np.zeros(3)
|
||
elif not force_rotation_only and t_prior_m is not None:
|
||
t = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||
translation_accepted = False
|
||
notes.append(
|
||
"SE3 motion solve gated off; CAD translation is reported as a prior only "
|
||
"and is not accepted as calibration"
|
||
)
|
||
else:
|
||
notes.append("rotation-only extrinsic returned (phase-A; phase-C SE3 gated off)")
|
||
|
||
return JointExtrinsicResult(
|
||
T_IMU_lidar=make_transform(t, r),
|
||
translation_accepted=bool(translation_accepted and np.linalg.norm(t) > 0),
|
||
residual_rms_rot_deg=rot_rms,
|
||
residual_rms_trans_m=trans_rms,
|
||
observability=observability,
|
||
gyro_bias_rad_s=np.asarray(bias_out, dtype=float),
|
||
accel_bias_m_s2=None if accel_bias_out is None else np.asarray(accel_bias_out, dtype=float),
|
||
gravity_m_s2=None if gravity_out is None else np.asarray(gravity_out, dtype=float),
|
||
notes=tuple(notes),
|
||
)
|
||
|
||
|
||
def solve_joint_extrinsic(
|
||
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||
r_x: np.ndarray,
|
||
*,
|
||
force_rotation_only: bool = False,
|
||
imu: ImuSeries | None = None,
|
||
delta_t_s: float = 0.0,
|
||
gyro_bias_rad_s: np.ndarray | None = None,
|
||
gyro_bias_rad_s_by_session: Mapping[str, np.ndarray] | None = None,
|
||
time_offset_s_by_session: Mapping[str, float] | None = None,
|
||
preexcluded_session_ids: set[str] | None = None,
|
||
gravity_init_m_s2: np.ndarray | None = None,
|
||
bias_prior_sigma_rad_s: float = 0.002,
|
||
rotation_prior: np.ndarray | None = None,
|
||
rotation_prior_sigma_deg: float = 15.0,
|
||
phase_a_yaw_std_max_deg: float = 0.5,
|
||
phase_a_loo_yaw_range_max_deg: float = 1.0,
|
||
phase_a_data_prior_difference_max_deg: float = 1.0,
|
||
run_phase_a_leave_one_out: bool = True,
|
||
phase_a_progress_callback: (
|
||
Callable[[str, dict[str, Any]], None] | None
|
||
) = None,
|
||
enable_phase_c: bool | None = None,
|
||
t_init_m: np.ndarray | None = None,
|
||
t_prior_m: np.ndarray | None = None,
|
||
t_prior_sigma_m: np.ndarray | float | None = None,
|
||
) -> JointExtrinsicResult:
|
||
"""Run the corrected session-aware Phase-A and gate unfinished SE(3) stages."""
|
||
|
||
del gravity_init_m_s2, t_init_m, t_prior_sigma_m, imu, r_x
|
||
usable_input = [pair for pair in pairs if pair.t_B_m is not None]
|
||
bias_bases = _phase_a_bias_bases(
|
||
usable_input,
|
||
gyro_bias_rad_s=gyro_bias_rad_s,
|
||
gyro_bias_rad_s_by_session=gyro_bias_rad_s_by_session,
|
||
)
|
||
comparison = solve_phase_a_comparison(
|
||
usable_input,
|
||
gyro_bias_rad_s_by_session=bias_bases,
|
||
rotation_prior=rotation_prior,
|
||
rotation_prior_sigma_deg=rotation_prior_sigma_deg,
|
||
preexcluded_session_ids=preexcluded_session_ids,
|
||
bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
|
||
yaw_std_max_deg=phase_a_yaw_std_max_deg,
|
||
leave_one_out_yaw_range_max_deg=(
|
||
phase_a_loo_yaw_range_max_deg
|
||
),
|
||
data_prior_difference_max_deg=(
|
||
phase_a_data_prior_difference_max_deg
|
||
),
|
||
run_leave_one_out=run_phase_a_leave_one_out,
|
||
progress_callback=phase_a_progress_callback,
|
||
)
|
||
primary = comparison.session_bg_data_only
|
||
r = primary.R_IMU_lidar
|
||
biases = primary.gyro_bias_rad_s_per_session
|
||
rot_rms = primary.residual_rms_deg
|
||
phase_a_accepted = comparison.accepted
|
||
notes = list(comparison.notes)
|
||
notes.append(
|
||
"phase-A primary=A1_session_bg_data_only; "
|
||
f"A0 RPY={comparison.fixed_bg_data_only.rpy_deg_xyz.tolist()}, "
|
||
f"A1 RPY={primary.rpy_deg_xyz.tolist()}, "
|
||
"A2 RPY="
|
||
f"{comparison.session_bg_with_rotation_prior.rpy_deg_xyz.tolist()}"
|
||
)
|
||
notes.append(
|
||
f"phase-A marginal yaw_std={comparison.marginal_observability.yaw_std_deg:.3f} deg, "
|
||
f"LOO yaw range={comparison.leave_one_out_yaw_range_deg:.3f} deg"
|
||
)
|
||
|
||
session_results_list: list[PhaseASessionResult] = [
|
||
PhaseASessionResult(
|
||
session_id=item.session_id,
|
||
pair_count=item.pair_count,
|
||
gyro_bias0_rad_s=item.gyro_bias0_rad_s,
|
||
gyro_bias_rad_s=item.gyro_bias_rad_s,
|
||
residual_rms_deg=item.residual_rms_deg,
|
||
residual_median_deg=item.residual_median_deg,
|
||
residual_p95_deg=item.residual_p95_deg,
|
||
outlier_fraction_gt_5deg=item.outlier_fraction_gt_5deg,
|
||
accepted=item.accepted,
|
||
included_in_final=True,
|
||
)
|
||
for item in primary.sessions
|
||
]
|
||
preexcluded = (
|
||
set()
|
||
if preexcluded_session_ids is None
|
||
else set(preexcluded_session_ids)
|
||
)
|
||
strong_all = select_strong_rotation_pairs(usable_input)
|
||
for session_id in sorted(preexcluded):
|
||
local_pairs = [
|
||
pair for pair in strong_all if pair.session_id == session_id
|
||
]
|
||
errors = [
|
||
float(
|
||
np.degrees(
|
||
np.linalg.norm(
|
||
preintegration_rotation_residual(
|
||
pair.R_A, r, pair.R_B
|
||
)
|
||
)
|
||
)
|
||
)
|
||
for pair in local_pairs
|
||
]
|
||
rms, median, p95, outlier, accepted = _rotation_distribution(
|
||
errors
|
||
)
|
||
base = np.asarray(
|
||
bias_bases.get(session_id, np.zeros(3)), dtype=float
|
||
).reshape(3)
|
||
session_results_list.append(
|
||
PhaseASessionResult(
|
||
session_id=session_id,
|
||
pair_count=len(local_pairs),
|
||
gyro_bias0_rad_s=base,
|
||
gyro_bias_rad_s=base,
|
||
residual_rms_deg=rms,
|
||
residual_median_deg=median,
|
||
residual_p95_deg=p95,
|
||
outlier_fraction_gt_5deg=outlier,
|
||
accepted=accepted,
|
||
included_in_final=False,
|
||
)
|
||
)
|
||
session_results = tuple(
|
||
sorted(session_results_list, key=lambda item: item.session_id)
|
||
)
|
||
usable = [
|
||
pair
|
||
for pair in strong_all
|
||
if pair.session_id not in preexcluded
|
||
]
|
||
base_observability = analyze_observability(usable, r)
|
||
marginal = comparison.marginal_observability
|
||
observability = ObservabilityReport(
|
||
rotation_observable=bool(
|
||
marginal.rank == 3
|
||
and marginal.yaw_std_deg <= phase_a_yaw_std_max_deg
|
||
),
|
||
translation_observable=base_observability.translation_observable,
|
||
condition_rotation=marginal.condition,
|
||
condition_translation=base_observability.condition_translation,
|
||
notes=tuple(
|
||
list(marginal.notes)
|
||
+ list(base_observability.notes)
|
||
),
|
||
)
|
||
notes.extend(observability.notes)
|
||
if time_offset_s_by_session is None:
|
||
notes.append(
|
||
f"legacy scalar time offset fixed during pair construction: {float(delta_t_s):.6f}s"
|
||
)
|
||
else:
|
||
fixed_offsets = {
|
||
str(sid): float(value) for sid, value in time_offset_s_by_session.items()
|
||
}
|
||
notes.append(
|
||
f"time offsets fixed during pair construction (not optimized): {fixed_offsets}"
|
||
)
|
||
|
||
for item in session_results:
|
||
notes.append(
|
||
f"phase-A session {item.session_id}: included={item.included_in_final}, "
|
||
f"pairs={item.pair_count}, rms={item.residual_rms_deg:.3f} deg, "
|
||
f"p95={item.residual_p95_deg:.3f} deg, "
|
||
f"|bias-bias0|={float(np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)):.3e}"
|
||
)
|
||
|
||
phase_c_requested = (not force_rotation_only) if enable_phase_c is None else bool(enable_phase_c)
|
||
t = np.zeros(3)
|
||
if not force_rotation_only:
|
||
if phase_c_requested:
|
||
notes.append(
|
||
"phase-B/C gated off: session-aware translation/gravity/navigation "
|
||
"states are not implemented yet"
|
||
)
|
||
else:
|
||
notes.append("phase-C disabled; translation is not accepted")
|
||
if t_prior_m is not None:
|
||
t = np.asarray(t_prior_m, dtype=float).reshape(3)
|
||
notes.append(
|
||
"CAD translation is reported as a prior only and is not accepted as calibration"
|
||
)
|
||
else:
|
||
notes.append("rotation-only extrinsic returned after corrected phase-A")
|
||
|
||
single_bias = None
|
||
if len(biases) == 1:
|
||
single_bias = np.asarray(next(iter(biases.values())), dtype=float)
|
||
return JointExtrinsicResult(
|
||
T_IMU_lidar=make_transform(t, r),
|
||
translation_accepted=False,
|
||
residual_rms_rot_deg=rot_rms,
|
||
residual_rms_trans_m=1e9,
|
||
observability=observability,
|
||
gyro_bias_rad_s=single_bias,
|
||
accel_bias_m_s2=None,
|
||
gravity_m_s2=None,
|
||
gyro_bias_rad_s_per_session={
|
||
sid: np.asarray(value, dtype=float) for sid, value in biases.items()
|
||
},
|
||
phase_a_sessions=session_results,
|
||
phase_a_accepted=phase_a_accepted,
|
||
phase_a_comparison=phase_a_comparison_to_dict(comparison),
|
||
notes=tuple(notes),
|
||
)
|