lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

完善Phase-A会话级联合优化并修正雷达相位中心高度先验

Browse Source
This commit is contained in:
lichun.qu
2026-08-19 09:41:39 +08:00
parent 5ac50ad71f
commit 1233f8aafd
20 changed files with 3621 additions and 140 deletions
Download Patch File Download Diff File Expand all files Collapse all files
imu_lidar/cli.py
+202 -3
View File
@@ -3,12 +3,82 @@
from __future__ import annotations
import argparse
from datetime import datetime
from pathlib import Path
from typing import Any
from .contracts import CalibrationMode, CalibrationRequest, CalibrationStatus, SessionInput
from .phase_a_replay import run_phase_a_replay
from .pipeline import describe_pipeline, run_calibration
def _format_progress_value(value: Any) -> str:
if isinstance(value, float):
return f"{value:.3f}"
if isinstance(value, (list, tuple, set)):
return "[" + ",".join(str(item) for item in value) + "]"
return str(value)
def _print_progress(event: dict[str, Any]) -> None:
"""Print one compact, immediately flushed progress line."""
timestamp = datetime.now().strftime("%H:%M:%S")
stage_index = event.get("stage_index", "?")
stage_total = event.get("stage_total", "?")
stage_name = event.get("stage", "unknown")
message = event.get("event", "progress")
fields = " ".join(
f"{key}={_format_progress_value(value)}"
for key, value in event.items()
if key not in {"stage_index", "stage_total", "stage", "event"}
and value is not None
)
suffix = f" | {fields}" if fields else ""
print(
f"[{timestamp}] [stage {stage_index}/{stage_total} {stage_name}] {message}{suffix}",
flush=True,
)
def _parse_session_imu_specs(
specs: list[str] | None,
) -> dict[str, Path]:
result: dict[str, Path] = {}
for spec in specs or []:
if "=" not in spec:
raise SystemExit(
"--session-imu must use SESSION_ID=PATH syntax"
)
session_id, raw_path = spec.split("=", 1)
session_id = session_id.strip()
if not session_id or not raw_path.strip():
raise SystemExit(
"--session-imu must use non-empty SESSION_ID=PATH"
)
if session_id in result:
raise SystemExit(
f"duplicate --session-imu for {session_id}"
)
result[session_id] = Path(raw_path.strip())
return result
def _print_phase_a_progress(
event: str,
fields: dict[str, Any],
) -> None:
_print_progress(
{
"stage_index": "A",
"stage_total": "A",
"stage": "phase_a_replay",
"event": event,
**fields,
}
)
def build_parser() -> argparse.ArgumentParser:
parser = argparse.ArgumentParser(description="LiDAR–IMU extrinsic calibration (V1)")
subcommands = parser.add_subparsers(dest="command", required=True)
@@ -58,6 +128,13 @@ def build_parser() -> argparse.ArgumentParser:
default=None,
help="Skip |ω| δt search and use this constant (use 0 after host-UTC bridge)",
)
run.add_argument(
"--session-time-offset-s",
action="append",
type=float,
default=None,
help="Per-session fixed time offset; repeat once per --imu/--lidar input",
)
run.add_argument(
"--no-signed-time-refine",
action="store_true",
@@ -71,6 +148,69 @@ def build_parser() -> argparse.ArgumentParser:
)
run.add_argument("--min-pair-rotation-deg", type=float, default=3.0)
run.add_argument("--min-pair-translation-m", type=float, default=0.3)
run.add_argument("--min-registration-fitness", type=float, default=0.5)
run.add_argument("--max-imu-gap-s", type=float, default=0.05)
run.add_argument("--max-lidar-gap-s", type=float, default=1.0)
replay = subcommands.add_parser(
"phase-a-replay",
help="Replay Phase-A from cached motion pairs without rerunning GICP",
)
replay.add_argument("--motion-pairs", type=Path, required=True)
replay.add_argument("--vehicle-config", type=Path, required=True)
replay.add_argument("--output", type=Path, required=True)
replay.add_argument(
"--session-imu",
action="append",
default=None,
metavar="SESSION_ID=PATH",
help="Raw IMU mapping used only when cache lacks J_bg/cov",
)
replay.add_argument(
"--exclude-session",
action="append",
default=None,
help="Session ID to exclude; may be repeated",
)
replay.add_argument(
"--strong-rotation-min-deg",
type=float,
default=1.0,
)
replay.add_argument(
"--decorrelation-block-s",
type=float,
default=3.0,
help="Per-session time-block length used to decorrelate factors",
)
replay.add_argument(
"--max-pairs-per-block",
type=int,
default=1,
help="Maximum factors kept in each decorrelation block",
)
replay.add_argument(
"--bias-prior-sigma-rad-s",
type=float,
default=0.002,
)
replay.add_argument(
"--yaw-std-max-deg",
type=float,
default=0.5,
)
replay.add_argument(
"--loo-yaw-range-max-deg",
type=float,
default=1.0,
)
replay.add_argument(
"--data-prior-difference-max-deg",
type=float,
default=1.0,
)
replay.add_argument("--max-nfev", type=int, default=200)
return parser
@@ -87,9 +227,23 @@ def _build_sessions(args: argparse.Namespace) -> tuple[SessionInput, ...]:
raise SystemExit(
f"--session-id count ({len(session_ids)}) must match --imu/--lidar ({len(imus)})"
)
if args.session_time_offset_s is None:
session_offsets: list[float | None] = [None] * len(imus)
else:
session_offsets = list(args.session_time_offset_s)
if len(session_offsets) != len(imus):
raise SystemExit(
f"--session-time-offset-s count ({len(session_offsets)}) must match "
f"--imu/--lidar ({len(imus)})"
)
return tuple(
SessionInput(session_id=sid, imu_source=imu, lidar_source=lidar)
for sid, imu, lidar in zip(session_ids, imus, lidars)
SessionInput(
session_id=sid,
imu_source=imu,
lidar_source=lidar,
fixed_time_offset_s=offset,
)
for sid, imu, lidar, offset in zip(session_ids, imus, lidars, session_offsets)
)
@@ -108,6 +262,48 @@ def main(argv: list[str] | None = None) -> int:
print(f"{index}. {stage.name}: {stage.responsibility}")
return 0
if args.command == "phase-a-replay":
summary = run_phase_a_replay(
motion_pairs_path=args.motion_pairs,
vehicle_config_path=args.vehicle_config,
output_directory=args.output,
imu_paths_by_session=_parse_session_imu_specs(
args.session_imu
),
excluded_sessions=set(args.exclude_session or []),
strong_rotation_min_deg=args.strong_rotation_min_deg,
decorrelation_block_s=args.decorrelation_block_s,
max_pairs_per_block=args.max_pairs_per_block,
bias_prior_sigma_rad_s=args.bias_prior_sigma_rad_s,
yaw_std_max_deg=args.yaw_std_max_deg,
leave_one_out_yaw_range_max_deg=(
args.loo_yaw_range_max_deg
),
data_prior_difference_max_deg=(
args.data_prior_difference_max_deg
),
max_nfev=args.max_nfev,
progress_callback=_print_phase_a_progress,
)
print(f"status: {summary['status']}")
print(f"acceptance_checks: {summary['acceptance_checks']}")
for name, variant in summary["variants"].items():
print(
f"{name}: rpy_deg_xyz={variant['rpy_deg_xyz']} "
f"RMS={variant['residual_rms_deg']:.6f} "
f"P95={variant['residual_p95_deg']:.6f}"
)
print(
"A1 marginalized yaw_std_deg: "
f"{summary['marginal_observability_A1']['yaw_std_deg']}"
)
print(
"leave_one_out_yaw_range_deg: "
f"{summary['leave_one_out_yaw_range_deg']}"
)
print(f"report directory: {args.output}")
return 0 if (summary["accepted"] or summary.get("partial_accepted")) else 2
if args.command == "run":
sessions = _build_sessions(args)
request = CalibrationRequest(
@@ -118,12 +314,15 @@ def main(argv: list[str] | None = None) -> int:
max_iterations=args.max_iterations,
min_pair_rotation_deg=args.min_pair_rotation_deg,
min_pair_translation_m=args.min_pair_translation_m,
min_registration_fitness=args.min_registration_fitness,
max_imu_gap_s=args.max_imu_gap_s,
max_lidar_gap_s=args.max_lidar_gap_s,
time_offset_search_s=args.time_offset_search_s,
fixed_time_offset_s=args.fixed_time_offset_s,
enable_signed_time_refine=not args.no_signed_time_refine,
max_signed_refine_shift_s=args.max_signed_refine_shift_s,
)
result = run_calibration(request)
result = run_calibration(request, progress_callback=_print_progress)
print(f"status: {result.status.value}")
print(f"message: {result.message}")
if result.time_offset_s is not None:
imu_lidar/contracts.py
+7
View File
@@ -25,6 +25,7 @@ class CalibrationStatus(str, Enum):
NOT_RUN = "not_run"
BLOCKED = "blocked"
ROTATION_ONLY_ACCEPTED = "rotation_only_accepted"
ROTATION_ONLY_PRIOR_CONSTRAINED = "rotation_only_prior_constrained"
FULL_SE3_ACCEPTED = "full_se3_accepted"
FULL_SE3_REJECTED = "full_se3_rejected_due_to_observability"
@@ -37,6 +38,9 @@ class SessionInput:
imu_source: Path
lidar_source: Path
board_configuration_id: str | None = None
# Optional session-local override. The request-level value remains a
# backward-compatible fallback for batches whose timelines are all aligned.
fixed_time_offset_s: float | None = None
@dataclass(frozen=True)
@@ -50,6 +54,9 @@ class CalibrationRequest:
max_iterations: int = 2
min_pair_rotation_deg: float = 3.0
min_pair_translation_m: float = 0.3
min_registration_fitness: float = 0.5
max_imu_gap_s: float = 0.05
max_lidar_gap_s: float = 1.0
time_offset_search_s: float = 1.0
# If set, skip |ω| search and use this constant (host-UTC-bridged sessions: 0).
fixed_time_offset_s: float | None = None
imu_lidar/joint_optimizer.py
+451 -19
View File
@@ -2,7 +2,9 @@
from __future__ import annotations
from dataclasses import dataclass
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
@@ -17,10 +19,26 @@ from .imu_preintegration import (
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
@@ -31,6 +49,10 @@ class JointExtrinsicResult:
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, ...] = ()
@@ -174,7 +196,8 @@ def _solve_phase_c_se3(
pairs: list[MotionPair],
r_x: np.ndarray,
*,
gyro_bias0: 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,
@@ -189,7 +212,7 @@ def _solve_phase_c_se3(
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_bias0, np.zeros(3), 1e9, 1e9, notes
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] = {}
@@ -225,7 +248,8 @@ def _solve_phase_c_se3(
g0 = g0 * (G_NORM / max(np.linalg.norm(g0), 1e-9))
basis = _gravity_basis(g0)
ba0 = np.zeros(3)
bg0 = np.asarray(gyro_bias0, dtype=float).reshape(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
@@ -243,7 +267,7 @@ def _solve_phase_c_se3(
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] = bg0
x0[8 + n_v + 3 * idx : 8 + n_v + 3 * idx + 3] = bg_init
whitened = []
for pair in usable:
@@ -312,7 +336,7 @@ def _solve_phase_c_se3(
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] - bg0))
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))
@@ -357,7 +381,197 @@ def _solve_phase_c_se3(
return r_opt, t_opt, g_opt, bg_mean, ba_mean, rot_rms, trans_rms, notes
def solve_joint_extrinsic(
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,
*,
@@ -366,6 +580,8 @@ def solve_joint_extrinsic(
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,
@@ -391,10 +607,10 @@ def solve_joint_extrinsic(
def rotation_residuals(r_opt: np.ndarray, delta_bias: np.ndarray) -> np.ndarray:
residuals = []
for pair, weight, whiten in zip(usable, weights, whitens):
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(np.sqrt(weight) * (whiten @ err))
residuals.append(whiten @ err)
residuals.append(prior_w * delta_bias)
return np.concatenate(residuals) if residuals else np.zeros(0)
@@ -407,16 +623,16 @@ def solve_joint_extrinsic(
residual_rot_bias,
np.zeros(6),
loss="huber",
f_scale=np.deg2rad(1.0),
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 (Σ-whitened + J_bg): "
"phase-A joint refine (single Σ whitening + J_bg): "
f"|δb|={float(np.linalg.norm(delta_bias)):.3e} rad/s, "
f"weighted pairs={len(usable)}"
f"pairs={len(usable)}"
)
else:
bias_out = bias0
@@ -457,7 +673,8 @@ def solve_joint_extrinsic(
r, t, gravity_out, bias_out, accel_bias_out, rot_rms, trans_rms, c_notes = _solve_phase_c_se3(
usable,
r,
gyro_bias0=bias_out,
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,
@@ -469,9 +686,9 @@ def solve_joint_extrinsic(
# 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)
translation_accepted = True
notes.append(
"phase-C translation residual/gate failed; keeping CAD translation prior"
"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")
@@ -520,15 +737,18 @@ def solve_joint_extrinsic(
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 or t_prior_m is not None
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 = True
notes.append("SE3 motion solve gated off; using CAD translation prior with refined rotation")
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)")
@@ -536,10 +756,222 @@ def solve_joint_extrinsic(
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=0.0 if not translation_accepted else trans_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),
)
imu_lidar/keyframes.py
+2 -1
View File
@@ -21,6 +21,7 @@ def build_keyframes(
*,
min_translation_m: float = 0.3,
min_rotation_deg: float = 3.0,
min_registration_fitness: float = 0.5,
max_frame_gap: int = 8,
) -> KeyframeSet:
"""Select keyframes with enough relative motion for hand-eye pairs."""
@@ -36,7 +37,7 @@ def build_keyframes(
last = index
continue
result = register_lidar_pair(frames[index].points_xyz, frames[last].points_xyz)
if not result.ok:
if not result.ok or result.fitness < min_registration_fitness:
continue
if result.translation_m >= min_translation_m or result.rotation_deg >= min_rotation_deg:
selected.append(index)
imu_lidar/motion_pairs.py
+84
View File
@@ -2,7 +2,10 @@
from __future__ import annotations
from collections.abc import Callable
from dataclasses import dataclass
from time import perf_counter
from typing import Any
import numpy as np
@@ -30,7 +33,12 @@ def build_motion_pairs(
acc_bias_m_s2: np.ndarray | None = None,
min_rotation_deg: float = 3.0,
min_translation_m: float = 0.3,
min_registration_fitness: float = 0.5,
max_imu_gap_s: float = 0.05,
max_lidar_gap_s: float = 1.0,
all_frame_times_s: np.ndarray | None = None,
max_index_span: int = 4,
progress_callback: Callable[[dict[str, Any]], None] | None = None,
) -> MotionPairSet:
"""Create A/B motion pairs between nearby keyframes.
@@ -40,21 +48,74 @@ def build_motion_pairs(
notes: list[str] = []
pairs: list[MotionPair] = []
rejected_fitness = 0
rejected_imu_gap = 0
rejected_lidar_gap = 0
frame_times = (
None
if all_frame_times_s is None
else np.asarray(all_frame_times_s, dtype=float).reshape(-1)
)
bias_g = np.zeros(3) if gyro_bias_rad_s is None else np.asarray(gyro_bias_rad_s, dtype=float)
bias_a = np.zeros(3) if acc_bias_m_s2 is None else np.asarray(acc_bias_m_s2, dtype=float)
n = len(keyframes)
if n < 2:
return MotionPairSet((), ("need at least two keyframes",))
total_candidates = sum(max(n - span, 0) for span in range(1, max_index_span + 1))
processed_candidates = 0
started_at = perf_counter()
last_progress_at = started_at
def report_progress(*, event: str, span: int, force: bool = False) -> None:
nonlocal last_progress_at
if progress_callback is None:
return
now = perf_counter()
if not force and processed_candidates > 1 and now - last_progress_at < 10.0:
return
last_progress_at = now
progress_callback(
{
"event": event,
"processed_candidates": processed_candidates,
"total_candidates": total_candidates,
"progress_pct": 100.0 * processed_candidates / max(total_candidates, 1),
"current_span": span,
"max_span": max_index_span,
"accepted_pairs": len(pairs),
"rejected_fitness": rejected_fitness,
"rejected_imu_gap": rejected_imu_gap,
"rejected_lidar_gap": rejected_lidar_gap,
"elapsed_s": now - started_at,
}
)
report_progress(event="start", span=1, force=True)
for span in range(1, max_index_span + 1):
for start in range(0, n - span):
processed_candidates += 1
report_progress(event="running", span=span)
i = start
j = start + span
frame_i = keyframes[i]
frame_j = keyframes[j]
source_i = int(keyframe_indices[i])
source_j = int(keyframe_indices[j])
if frame_times is not None:
lo = min(source_i, source_j)
hi = max(source_i, source_j)
local_times = frame_times[lo : hi + 1]
if local_times.size >= 2 and np.any(np.diff(local_times) > max_lidar_gap_s):
rejected_lidar_gap += 1
continue
reg = register_lidar_pair(frame_j.points_xyz, frame_i.points_xyz)
if not reg.ok:
continue
if reg.fitness < min_registration_fitness:
rejected_fitness += 1
continue
if reg.rotation_deg < min_rotation_deg and reg.translation_m < min_translation_m:
continue
@@ -64,6 +125,16 @@ def build_motion_pairs(
continue
if t_i_imu < imu.t_s[0] or t_j_imu > imu.t_s[-1]:
continue
imu_lo = max(int(np.searchsorted(imu.t_s, t_i_imu, side="right")) - 1, 0)
imu_hi = min(
int(np.searchsorted(imu.t_s, t_j_imu, side="left")) + 1,
imu.t_s.size,
)
if imu_hi - imu_lo >= 2 and np.any(
np.diff(imu.t_s[imu_lo:imu_hi]) > max_imu_gap_s
):
rejected_imu_gap += 1
continue
preint = preintegrate_imu(
imu.t_s,
@@ -111,14 +182,27 @@ def build_motion_pairs(
"delta_p": preint.delta_p.tolist(),
"t_i_imu_s": t_i_imu,
"t_j_imu_s": t_j_imu,
"gyro_bias0_rad_s": bias_g.tolist(),
"accel_bias0_m_s2": bias_a.tolist(),
"time_offset_s": float(delta_t_s),
"keyframe_span": int(span),
"is_consecutive": bool(span == 1),
"modeling": "imu_preintegration_factor_phase_c",
},
)
)
report_progress(event="complete", span=max_index_span, force=True)
notes.append(
f"built {len(pairs)} motion pairs (Phase-C preintegration: ΔR/Δv/Δp, Σ9, J_bg/J_ba)"
)
notes.append(
"quality rejects: "
f"fitness<{min_registration_fitness:.2f}: {rejected_fitness}, "
f"IMU gap>{max_imu_gap_s:.3f}s: {rejected_imu_gap}, "
f"LiDAR gap>{max_lidar_gap_s:.3f}s: {rejected_lidar_gap}"
)
return MotionPairSet(pairs=tuple(pairs), notes=tuple(notes))
imu_lidar/motion_pairs_io.py
+17 -5
View File
@@ -10,9 +10,11 @@ import numpy as np
from .contracts import MotionPair
SCHEMA_VERSION = 1
SCHEMA_VERSION = 2
# Keep viz-relevant fields; drop large cov / Jacobians.
# Keep visualization fields plus the compact 3x3 rotation metadata needed to
# rerun Phase-A without repeating LiDAR registration. Full 9x9 Phase-C matrices
# remain excluded from this cache.
_METADATA_KEEP = frozenset(
{
"backend",
@@ -23,8 +25,17 @@ _METADATA_KEEP = frozenset(
"duration_s",
"mean_gyro_norm",
"preint_sigma_rad",
"cov",
"J_bg",
"phase_a_metadata_rehydrated",
"rehydrated_R_A_error_deg",
"t_i_imu_s",
"t_j_imu_s",
"gyro_bias0_rad_s",
"accel_bias0_m_s2",
"time_offset_s",
"keyframe_span",
"is_consecutive",
"modeling",
}
)
@@ -113,10 +124,11 @@ def save_motion_pairs(path: Path | str, payload: dict[str, Any]) -> Path:
def load_motion_pairs(path: Path | str) -> dict[str, Any]:
payload = json.loads(Path(path).read_text(encoding="utf-8"))
if int(payload.get("schema_version", 0)) != SCHEMA_VERSION:
version = int(payload.get("schema_version", 0))
if version not in {1, SCHEMA_VERSION}:
raise ValueError(
f"unsupported motion_pairs schema_version={payload.get('schema_version')}; "
f"expected {SCHEMA_VERSION}"
f"unsupported motion_pairs schema_version={version}; "
f"expected 1 or {SCHEMA_VERSION}"
)
return payload
imu_lidar/observability.py
+40 -30
View File
@@ -57,43 +57,53 @@ def analyze_observability(
if j_r.size == 0:
return ObservabilityReport(False, False, 1e9, 1e9, ("empty rotation jacobian",))
# Normalize columns.
col_norm = np.linalg.norm(j_r, axis=0) + 1e-12
j_r_n = j_r / col_norm
singular = np.linalg.svd(j_r_n, compute_uv=False)
singular = np.linalg.svd(j_r, compute_uv=False)
cond_r = float(singular[0] / max(singular[-1], 1e-12))
rotation_ok = cond_r < condition_threshold and singular[-1] > 1e-3
rotation_information = float(singular[-1] / np.sqrt(max(len(usable), 1)))
rotation_ok = (
cond_r < condition_threshold
and rotation_information > 1e-3
and singular[-1] > 1e-6
)
# Translation observability proxy: diversity of rotation axes and presence of translation in B.
axes = []
translations = []
for pair in usable:
axis = so3_log(pair.R_B)
n = np.linalg.norm(axis)
if n > 1e-8:
axes.append(axis / n)
if pair.t_B_m is not None:
translations.append(pair.t_B_m)
axis_rank = 0
if axes:
axis_mat = np.asarray(axes, dtype=float)
axis_rank = int(np.linalg.matrix_rank(axis_mat, tol=0.1))
trans_span = 0.0
if translations:
tmat = np.asarray(translations, dtype=float)
trans_span = float(np.linalg.norm(np.std(tmat, axis=0)))
# For planar yaw-mostly motion, translation z is typically weak.
translation_ok = axis_rank >= 2 and trans_span > 0.2 and len(translations) >= 5
cond_t = 1e9 if not translation_ok else float(max(3, 10 - axis_rank * 2) * (0.5 / max(trans_span, 1e-3)))
# Translation lever arm is observable through stacked (R_A - I). Pure
# planar yaw leaves its vertical column in the nullspace and must fail.
translation_rows = [
np.asarray(pair.R_A, dtype=float).reshape(3, 3) - np.eye(3)
for pair in usable
if pair.t_B_m is not None
]
if translation_rows:
j_t = np.vstack(translation_rows)
singular_t = np.linalg.svd(j_t, compute_uv=False)
cond_t = float(singular_t[0] / max(singular_t[-1], 1e-12))
translation_information = float(
singular_t[-1] / np.sqrt(max(len(translation_rows), 1))
)
else:
cond_t = 1e9
translation_information = 0.0
translation_ok = (
len(translation_rows) >= 5
and cond_t < condition_threshold
and translation_information > 0.02
)
if not rotation_ok:
notes.append(f"rotation condition {cond_r:.1f} exceeds threshold {condition_threshold}")
notes.append(
f"rotation not observable: condition={cond_r:.1f}, "
f"min_information={rotation_information:.3e}"
)
else:
notes.append(f"rotation condition {cond_r:.1f}")
notes.append(
f"rotation observable: condition={cond_r:.1f}, "
f"min_information={rotation_information:.3e}"
)
if not translation_ok:
notes.append(
f"translation not observable (axis_rank={axis_rank}, trans_span={trans_span:.3f} m); "
"V1 will reject full SE3 without strong priors"
f"translation not observable: condition={cond_t:.1f}, "
f"min_information={translation_information:.3e}; "
"full SE3 will be rejected"
)
return ObservabilityReport(
rotation_observable=rotation_ok,
imu_lidar/phase_a.py
+1435
View File
File diff suppressed because it is too large Load Diff
imu_lidar/phase_a_replay.py
+290
View File
@@ -0,0 +1,290 @@
"""Cached Phase-A replay: rehydrate gyro factors, compare variants, write reports."""
from __future__ import annotations
import json
from collections import defaultdict
from pathlib import Path
from typing import Any
import numpy as np
from .imu_io import load_imu_samples
from .motion_pairs_io import (
build_motion_pairs_payload,
load_motion_pairs,
pair_from_dict,
save_motion_pairs,
)
from .phase_a import (
ProgressCallback,
phase_a_comparison_to_dict,
phase_a_metadata_complete,
rehydrate_phase_a_pairs,
solve_phase_a_comparison,
)
from .vehicle_config import load_vehicle_config, prior_enabled
def _rotation_prior(
vehicle_config_path: Path,
) -> tuple[np.ndarray | None, float]:
config = load_vehicle_config(vehicle_config_path)
if not prior_enabled(config, "rotation_prior"):
return None, 15.0
prior = (config.get("initialization") or {}).get("rotation_prior") or {}
matrix = prior.get("R_IMU_lidar")
if matrix is None:
return None, float(prior.get("sigma_deg", 15.0))
return (
np.asarray(matrix, dtype=float).reshape(3, 3),
float(prior.get("sigma_deg", 15.0)),
)
def _sanitize_json(value: Any) -> Any:
if isinstance(value, dict):
return {str(key): _sanitize_json(item) for key, item in value.items()}
if isinstance(value, (list, tuple)):
return [_sanitize_json(item) for item in value]
if isinstance(value, np.ndarray):
return _sanitize_json(value.tolist())
if isinstance(value, (np.floating, float)):
number = float(value)
return number if np.isfinite(number) else None
if isinstance(value, (np.integer, np.bool_)):
return value.item()
return value
def _write_json(path: Path, payload: Any) -> None:
path.write_text(
json.dumps(_sanitize_json(payload), indent=2, ensure_ascii=False) + "\n",
encoding="utf-8",
)
def _load_cached_sessions(
motion_pairs_path: Path,
) -> tuple[
dict[str, Any],
list,
dict[str, np.ndarray],
dict[str, float],
]:
payload = load_motion_pairs(motion_pairs_path)
pairs = []
biases: dict[str, np.ndarray] = {}
offsets: dict[str, float] = {}
for session in payload.get("sessions") or []:
session_id = str(session["session_id"])
biases[session_id] = np.asarray(
session.get("gyro_bias_rad_s", np.zeros(3)),
dtype=float,
).reshape(3)
offsets[session_id] = float(session.get("delta_t_s", 0.0))
pairs.extend(
pair_from_dict(item)
for item in session.get("pairs") or []
)
if not pairs:
raise ValueError(f"motion-pair cache is empty: {motion_pairs_path}")
return payload, pairs, biases, offsets
def run_phase_a_replay(
*,
motion_pairs_path: Path,
vehicle_config_path: Path,
output_directory: Path,
imu_paths_by_session: dict[str, Path] | None = None,
excluded_sessions: set[str] | None = None,
strong_rotation_min_deg: float = 1.0,
decorrelation_block_s: float = 3.0,
max_pairs_per_block: int = 1,
bias_prior_sigma_rad_s: float = 0.002,
yaw_std_max_deg: float = 0.5,
leave_one_out_yaw_range_max_deg: float = 1.0,
data_prior_difference_max_deg: float = 1.0,
max_nfev: int = 200,
progress_callback: ProgressCallback | None = None,
) -> dict[str, Any]:
"""Run Phase-A only. Existing LiDAR relative motions are never recomputed."""
output_directory.mkdir(parents=True, exist_ok=True)
source_payload, pairs, bias0, offsets = _load_cached_sessions(
motion_pairs_path
)
session_ids = sorted(bias0)
if progress_callback is not None:
progress_callback(
"cache_loaded",
{
"schema_version": source_payload.get("schema_version"),
"sessions": len(session_ids),
"pairs": len(pairs),
},
)
rehydration_report: dict[str, Any] = {
"required": not phase_a_metadata_complete(pairs),
"pair_count": len(pairs),
}
if not phase_a_metadata_complete(pairs):
supplied_paths = {} if imu_paths_by_session is None else imu_paths_by_session
missing = [sid for sid in session_ids if sid not in supplied_paths]
if missing:
raise ValueError(
"v1 cache lacks J_bg/cov; provide --session-imu for: "
+ ", ".join(missing)
)
imu_by_session = {
sid: load_imu_samples(supplied_paths[sid])
for sid in session_ids
}
pairs, details = rehydrate_phase_a_pairs(
pairs,
imu_by_session=imu_by_session,
bias0_by_session=bias0,
progress_callback=progress_callback,
)
rehydration_report.update(details)
if float(details["max_R_A_error_deg"]) > 0.05:
raise ValueError(
"rehydrated IMU rotations do not match cached R_A: "
f"max error={details['max_R_A_error_deg']:.6f} deg; "
"check session-to-IMU path mapping"
)
grouped: dict[str, list] = defaultdict(list)
for pair in pairs:
grouped[pair.session_id].append(pair)
enriched_payload = build_motion_pairs_payload(
prepared_sessions=[
{
"session_id": sid,
"time_offset_s": offsets[sid],
"gyro_bias_rad_s": bias0[sid],
"pairs": tuple(grouped[sid]),
}
for sid in session_ids
]
)
enriched_cache_path = save_motion_pairs(
output_directory / "motion_pairs_phase_a_v2.json",
enriched_payload,
)
rotation_prior, rotation_prior_sigma_deg = _rotation_prior(
vehicle_config_path
)
comparison = solve_phase_a_comparison(
pairs,
gyro_bias_rad_s_by_session=bias0,
rotation_prior=rotation_prior,
rotation_prior_sigma_deg=rotation_prior_sigma_deg,
preexcluded_session_ids=excluded_sessions,
strong_rotation_min_deg=strong_rotation_min_deg,
decorrelation_block_s=decorrelation_block_s,
max_pairs_per_block=max_pairs_per_block,
bias_prior_sigma_rad_s=bias_prior_sigma_rad_s,
yaw_std_max_deg=yaw_std_max_deg,
leave_one_out_yaw_range_max_deg=(
leave_one_out_yaw_range_max_deg
),
data_prior_difference_max_deg=data_prior_difference_max_deg,
run_leave_one_out=True,
max_nfev=max_nfev,
progress_callback=progress_callback,
)
full = phase_a_comparison_to_dict(comparison)
full["input"] = {
"motion_pairs": str(motion_pairs_path),
"source_schema_version": source_payload.get("schema_version"),
"vehicle_config": str(vehicle_config_path),
"session_imu_paths": {
sid: str(path)
for sid, path in (imu_paths_by_session or {}).items()
},
"excluded_sessions": sorted(excluded_sessions or set()),
}
full["rehydration"] = rehydration_report
full["enriched_cache"] = str(enriched_cache_path)
full["parameters"] = {
"strong_rotation_min_deg": strong_rotation_min_deg,
"decorrelation_block_s": decorrelation_block_s,
"max_pairs_per_block": max_pairs_per_block,
"bias_prior_sigma_rad_s": bias_prior_sigma_rad_s,
"rotation_prior_sigma_deg": rotation_prior_sigma_deg,
"yaw_std_max_deg": yaw_std_max_deg,
"leave_one_out_yaw_range_max_deg": (
leave_one_out_yaw_range_max_deg
),
"data_prior_difference_max_deg": (
data_prior_difference_max_deg
),
"max_nfev": max_nfev,
}
variants = full["variants"]
summary = {
"status": comparison.solution_status,
"accepted": comparison.accepted,
"partial_accepted": comparison.partial_accepted,
"acceptance_checks": comparison.acceptance_checks,
"primary_result": comparison.recommended_result,
"variants": {
name: {
"rpy_deg_xyz": item["rpy_deg_xyz"],
"R_IMU_lidar": item["R_IMU_lidar"],
"residual_rms_deg": item["residual_rms_deg"],
"residual_p95_deg": item["residual_p95_deg"],
"accepted": item["accepted"],
"gyro_bias_rad_s_per_session": item[
"gyro_bias_rad_s_per_session"
],
}
for name, item in variants.items()
if item is not None
},
"marginal_observability_A1": full[
"marginal_observability_A1"
],
"data_vs_prior_yaw_diff_deg": (
comparison.data_vs_prior_yaw_diff_deg
),
"data_vs_prior_geodesic_deg": (
comparison.data_vs_prior_geodesic_deg
),
"leave_one_out_yaw_range_deg": (
comparison.leave_one_out_yaw_range_deg
),
"leave_one_out_observable_max_deg": (
comparison.leave_one_out_observable_max_deg
),
"strong_pair_candidate_count": (
comparison.strong_pair_candidate_count
),
"decorrelated_pair_count": comparison.decorrelated_pair_count,
"strong_pair_counts_per_session": (
comparison.strong_pair_counts_per_session
),
"excluded_sessions": list(comparison.excluded_sessions),
"rehydration": rehydration_report,
"comparison_file": "phase_a_comparison.json",
"observability_file": "phase_a_observability.json",
"leave_one_out_file": "phase_a_leave_one_out.json",
"enriched_cache_file": enriched_cache_path.name,
}
_write_json(output_directory / "phase_a_comparison.json", full)
_write_json(
output_directory / "phase_a_observability.json",
full["marginal_observability_A1"],
)
_write_json(
output_directory / "phase_a_leave_one_out.json",
full["leave_one_out"],
)
_write_json(output_directory / "phase_a_summary.json", summary)
return summary
imu_lidar/pipeline.py
+485 -38
View File
@@ -2,8 +2,10 @@
from __future__ import annotations
from collections.abc import Callable
from dataclasses import asdict, dataclass, replace
from pathlib import Path
from time import perf_counter
from typing import Any
import numpy as np
@@ -58,11 +60,33 @@ STAGES = (
PipelineStage("lidar_motion", "各会话关键帧、可选去畸变与 LiDAR 相对运动"),
PipelineStage("motion_pairs", "各会话构造运动对，再合并"),
PipelineStage("rotation_handeye", "用全部会话运动对联合求解旋转外参"),
PipelineStage("joint_optimizer", "用全部会话运动对联合精修；完整模式估平移"),
PipelineStage("joint_optimizer", "Phase-A 会话级零偏联合精修；Phase-B/C 暂时门控"),
PipelineStage("finalize", "写出结果与质量报告"),
)
ProgressCallback = Callable[[dict[str, Any]], None]
def _emit_progress(
callback: ProgressCallback | None,
stage_index: int,
event: str,
**fields: Any,
) -> None:
if callback is None:
return
callback(
{
"stage_index": stage_index,
"stage_total": len(STAGES),
"stage": STAGES[stage_index - 1].name,
"event": event,
**fields,
}
)
def describe_pipeline(_: CalibrationRequest) -> tuple[PipelineStage, ...]:
"""Return the planned stages."""
@@ -79,12 +103,22 @@ def _build_pairs_and_handeye(
request: CalibrationRequest,
R_prior: np.ndarray | None = None,
prior_sigma_deg: float | None = None,
progress_callback: ProgressCallback | None = None,
):
keyframes = build_keyframes(
working_frames,
min_translation_m=request.min_pair_translation_m,
min_rotation_deg=request.min_pair_rotation_deg,
min_registration_fitness=request.min_registration_fitness,
)
if progress_callback is not None:
progress_callback(
{
"event": "keyframes_ready",
"keyframe_count": len(keyframes.indices),
"lidar_frame_count": len(working_frames),
}
)
pair_set = build_motion_pairs(
session_id=session_id,
keyframes=list(keyframes.frames),
@@ -94,6 +128,11 @@ def _build_pairs_and_handeye(
gyro_bias_rad_s=gyro_bias_rad_s,
min_rotation_deg=request.min_pair_rotation_deg,
min_translation_m=request.min_pair_translation_m,
min_registration_fitness=request.min_registration_fitness,
max_imu_gap_s=request.max_imu_gap_s,
max_lidar_gap_s=request.max_lidar_gap_s,
all_frame_times_s=np.asarray([frame.t_mid_s for frame in working_frames], dtype=float),
progress_callback=progress_callback,
)
handeye = solve_rotation_handeye(
pair_set.pairs,
@@ -133,32 +172,81 @@ def _prepare_session_pairs(
*,
R_prior: np.ndarray | None = None,
prior_sigma_deg: float | None = None,
progress_callback: ProgressCallback | None = None,
session_index: int = 1,
session_total: int = 1,
) -> dict[str, Any]:
"""Per-session: audit, δt, keyframes/pairs. No joint extrinsic yet."""
started_at = perf_counter()
def emit(stage_index: int, event: str, **fields: Any) -> None:
_emit_progress(
progress_callback,
stage_index,
event,
session=session.session_id,
session_index=session_index,
session_total=session_total,
**fields,
)
emit(
2,
"session_start",
imu_source=str(session.imu_source),
lidar_source=str(session.lidar_source),
)
imu = load_imu_samples(session.imu_source)
frames = load_lidar_frames(session.lidar_source)
emit(
2,
"data_loaded",
imu_samples=int(imu.t_s.size),
lidar_frames=len(frames),
imu_span_s=float(imu.t_s[-1] - imu.t_s[0]) if imu.t_s.size >= 2 else 0.0,
lidar_span_s=(
float(frames[-1].t_mid_s - frames[0].t_mid_s) if len(frames) >= 2 else 0.0
),
elapsed_s=perf_counter() - started_at,
)
ts = audit_timestamps(imu, frames)
emit(2, "audit_complete", ok=ts.ok)
if not ts.ok:
emit(2, "blocked", reason="timestamp_audit")
return {"ok": False, "stage": "timestamp_audit", "session_id": session.session_id, "report": asdict(ts)}
imu_report = audit_imu(imu)
emit(
3,
"audit_complete",
ok=imu_report.ok,
gyro_bias_norm_rad_s=float(np.linalg.norm(imu_report.gyro_bias_rad_s)),
)
if not imu_report.ok:
emit(3, "blocked", reason="imu_audit")
return {"ok": False, "stage": "imu_audit", "session_id": session.session_id, "report": asdict(imu_report)}
if request.fixed_time_offset_s is not None:
fixed_time_offset_s = (
session.fixed_time_offset_s
if session.fixed_time_offset_s is not None
else request.fixed_time_offset_s
)
if fixed_time_offset_s is not None:
offset_source = "fixed"
offset = TimeOffsetResult(
delta_t_s=float(request.fixed_time_offset_s),
delta_t_s=float(fixed_time_offset_s),
correlation_peak=1.0,
search_s=0.0,
notes=(
f"fixed_time_offset_s={float(request.fixed_time_offset_s):.6f} "
f"fixed_time_offset_s={float(fixed_time_offset_s):.6f} "
"(skip |ω| search; intended for host-UTC-bridged sessions)",
),
ok=True,
)
else:
offset_source = "estimated"
offset = estimate_time_offset(
imu,
frames,
@@ -166,8 +254,22 @@ def _prepare_session_pairs(
search_s=request.time_offset_search_s,
)
if not offset.ok:
emit(
4,
"blocked",
reason="time_offset",
time_offset_s=float(offset.delta_t_s),
correlation_peak=float(offset.correlation_peak),
)
return {"ok": False, "stage": "time_offset", "session_id": session.session_id, "report": asdict(offset)}
emit(
4,
"offset_ready",
source=offset_source,
time_offset_s=float(offset.delta_t_s),
correlation_peak=float(offset.correlation_peak),
)
coarse_delta_t = float(offset.delta_t_s)
working_frames = frames
r_x = np.eye(3) if R_prior is None else np.asarray(R_prior, dtype=float).reshape(3, 3)
@@ -177,8 +279,35 @@ def _prepare_session_pairs(
pairs_notes: list[str] = []
pair_count = 0
for iteration in range(max(1, request.max_iterations)):
iterations_total = max(1, request.max_iterations)
build_pass = "outer"
def on_build_progress(payload: dict[str, Any]) -> None:
event = str(payload.get("event", "running"))
stage_index = 5 if event == "keyframes_ready" else 6
fields = {key: value for key, value in payload.items() if key != "event"}
emit(
stage_index,
event,
iteration=iteration + 1,
iterations_total=iterations_total,
build_pass=build_pass,
**fields,
)
for iteration in range(iterations_total):
build_pass = "outer"
emit(
5,
"iteration_start",
iteration=iteration + 1,
iterations_total=iterations_total,
deskew=iteration > 0,
time_offset_s=float(offset.delta_t_s),
)
if iteration > 0:
deskew_started_at = perf_counter()
emit(5, "deskew_start", iteration=iteration + 1)
working_frames = deskew_lidar_frames(
frames,
imu,
@@ -186,6 +315,13 @@ def _prepare_session_pairs(
R_IMU_lidar=r_x,
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
)
emit(
5,
"deskew_complete",
iteration=iteration + 1,
lidar_frames=len(working_frames),
elapsed_s=perf_counter() - deskew_started_at,
)
keyframes, pair_set, handeye = _build_pairs_and_handeye(
session_id=session.session_id,
working_frames=working_frames,
@@ -195,10 +331,31 @@ def _prepare_session_pairs(
request=request,
R_prior=R_prior,
prior_sigma_deg=prior_sigma_deg,
progress_callback=on_build_progress,
)
pairs_notes = list(pair_set.notes)
pair_count = len(pair_set.pairs)
emit(
7,
"local_handeye",
iteration=iteration + 1,
build_pass=build_pass,
keyframes=len(keyframes.indices),
pair_count=pair_count,
rms_deg=float(handeye.residual_rms_deg),
p95_deg=float(handeye.residual_p95_deg),
outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
ok=handeye.ok,
)
if pair_count < 3:
emit(
6,
"blocked",
reason="insufficient_motion_pairs",
iteration=iteration + 1,
keyframes=len(keyframes.indices),
pair_count=pair_count,
)
return {
"ok": False,
"stage": "motion_pairs",
@@ -216,7 +373,14 @@ def _prepare_session_pairs(
if not request.enable_signed_time_refine:
continue
for _ in range(2):
for refine_step in range(1, 3):
emit(
4,
"signed_refine_start",
iteration=iteration + 1,
refine_step=refine_step,
time_offset_s=float(offset.delta_t_s),
)
refined = refine_time_offset_signed(
imu,
frames,
@@ -243,8 +407,18 @@ def _prepare_session_pairs(
)
delta_shift = abs(refined.delta_t_s - offset.delta_t_s)
offset = _merge_time_offset(offset, refined)
emit(
4,
"signed_refine_complete",
iteration=iteration + 1,
refine_step=refine_step,
time_offset_s=float(offset.delta_t_s),
shift_s=float(delta_shift),
correlation_peak=float(refined.correlation_peak),
)
if delta_shift < 1e-3:
break
build_pass = f"signed_refine_{refine_step}"
keyframes, pair_set, handeye = _build_pairs_and_handeye(
session_id=session.session_id,
working_frames=working_frames,
@@ -254,10 +428,31 @@ def _prepare_session_pairs(
request=request,
R_prior=R_prior,
prior_sigma_deg=prior_sigma_deg,
progress_callback=on_build_progress,
)
pairs_notes = list(pair_set.notes)
pair_count = len(pair_set.pairs)
emit(
7,
"local_handeye",
iteration=iteration + 1,
build_pass=build_pass,
keyframes=len(keyframes.indices),
pair_count=pair_count,
rms_deg=float(handeye.residual_rms_deg),
p95_deg=float(handeye.residual_p95_deg),
outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
ok=handeye.ok,
)
if pair_count < 3:
emit(
6,
"blocked",
reason="insufficient_motion_pairs_after_signed_refine",
iteration=iteration + 1,
keyframes=len(keyframes.indices),
pair_count=pair_count,
)
return {
"ok": False,
"stage": "motion_pairs",
@@ -271,7 +466,6 @@ def _prepare_session_pairs(
"handeye": asdict(handeye),
}
r_x = handeye.R_IMU_lidar
assert handeye is not None and pair_set is not None and keyframes is not None
acc_mean = np.asarray(imu_report.static_acc_mean_m_s2, dtype=float).reshape(3)
acc_n = float(np.linalg.norm(acc_mean))
@@ -280,6 +474,15 @@ def _prepare_session_pairs(
else:
gravity_init = np.array([0.0, 0.0, -9.80665])
emit(
7,
"session_complete",
keyframes=len(keyframes.indices),
pair_count=pair_count,
time_offset_s=float(offset.delta_t_s),
local_handeye_ok=handeye.ok,
elapsed_s=perf_counter() - started_at,
)
return {
"ok": True,
"session_id": session.session_id,
@@ -300,6 +503,8 @@ def _prepare_session_pairs(
"handeye_local": {
"residual_rms_deg": handeye.residual_rms_deg,
"residual_median_deg": handeye.residual_median_deg,
"residual_p95_deg": handeye.residual_p95_deg,
"outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
"pair_count": handeye.pair_count,
"ok": handeye.ok,
"notes": handeye.notes,
@@ -323,60 +528,152 @@ def _remap_pairs_for_joint(prepared: list[dict[str, Any]]) -> list[MotionPair]:
return merged
def run_calibration(request: CalibrationRequest) -> CalibrationResult:
def run_calibration(
request: CalibrationRequest,
*,
progress_callback: ProgressCallback | None = None,
) -> CalibrationResult:
"""Run the V1 calibration pipeline for one or more sessions.
Multi-session: each session estimates its own δt and builds motion pairs;
rotation hand-eye and joint SE3 are solved once on the merged pair set.
"""
overall_started_at = perf_counter()
def finish(
*,
status: CalibrationStatus,
message: str,
details: dict[str, Any],
T_IMU_lidar: np.ndarray | None = None,
time_offset_s: float | None = None,
motion_pairs_payload: dict[str, Any] | None = None,
) -> CalibrationResult:
_emit_progress(
progress_callback,
9,
"writing_result",
status=status.value,
output_directory=str(request.output_directory),
)
result = finalize_result(
status=status,
message=message,
details=details,
T_IMU_lidar=T_IMU_lidar,
time_offset_s=time_offset_s,
output_directory=request.output_directory,
motion_pairs_payload=motion_pairs_payload,
)
_emit_progress(
progress_callback,
9,
"complete",
status=result.status.value,
elapsed_s=perf_counter() - overall_started_at,
)
return result
_emit_progress(
progress_callback,
1,
"pipeline_start",
mode=request.requested_mode.value,
session_count=len(request.sessions),
max_iterations=max(1, request.max_iterations),
output_directory=str(request.output_directory),
)
if not request.sessions:
return finalize_result(
return finish(
status=CalibrationStatus.BLOCKED,
message="no sessions provided",
details={},
output_directory=request.output_directory,
)
vehicle_config = None
if request.vehicle_config is not None:
_emit_progress(
progress_callback,
1,
"loading_vehicle_config",
path=str(request.vehicle_config),
)
try:
vehicle_config = load_vehicle_config(request.vehicle_config)
except Exception as exc: # noqa: BLE001 - surface config problems as blocked
return finalize_result(
_emit_progress(
progress_callback,
1,
"blocked",
reason="vehicle_config",
error=str(exc),
)
return finish(
status=CalibrationStatus.BLOCKED,
message=f"vehicle config failed: {exc}",
details={},
output_directory=request.output_directory,
)
_emit_progress(
progress_callback,
1,
"vehicle_config_ready",
loaded=vehicle_config is not None,
)
r_prior, prior_sigma_deg = _rotation_prior_from_config(vehicle_config)
prepared: list[dict[str, Any]] = []
for session in request.sessions:
session_total = len(request.sessions)
for session_index, session in enumerate(request.sessions, start=1):
prep = _prepare_session_pairs(
session,
request,
R_prior=r_prior,
prior_sigma_deg=prior_sigma_deg,
progress_callback=progress_callback,
session_index=session_index,
session_total=session_total,
)
if not prep.get("ok"):
return finalize_result(
return finish(
status=CalibrationStatus.BLOCKED,
message=f"blocked at stage {prep.get('stage')} ({prep.get('session_id')})",
details={"sessions": [prep]},
output_directory=request.output_directory,
)
prepared.append(prep)
all_pairs = _remap_pairs_for_joint(prepared)
pair_counts_per_session = {
p["session_id"]: int(p["pair_count"]) for p in prepared
}
_emit_progress(
progress_callback,
7,
"joint_handeye_start",
session_count=len(prepared),
merged_pair_count=len(all_pairs),
pair_counts_per_session=pair_counts_per_session,
)
handeye_started_at = perf_counter()
handeye = solve_rotation_handeye(
all_pairs,
R_prior=r_prior,
prior_sigma_deg=prior_sigma_deg,
)
if not handeye.ok:
return finalize_result(
_emit_progress(
progress_callback,
7,
"joint_handeye_complete",
pair_count=handeye.pair_count,
rms_deg=float(handeye.residual_rms_deg),
p95_deg=float(handeye.residual_p95_deg),
outlier_fraction_gt_5deg=float(handeye.outlier_fraction_gt_5deg),
ok=handeye.ok,
elapsed_s=perf_counter() - handeye_started_at,
)
if handeye.pair_count < 3:
return finish(
status=CalibrationStatus.BLOCKED,
message="blocked at stage rotation_handeye (joint)",
details={
@@ -384,33 +681,124 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
"joint_handeye": asdict(handeye),
"merged_pair_count": len(all_pairs),
},
output_directory=request.output_directory,
)
force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config)
gyro_bias = np.mean(np.stack([p["gyro_bias_rad_s"] for p in prepared], axis=0), axis=0)
gravity_init = np.mean(np.stack([p["gravity_init_m_s2"] for p in prepared], axis=0), axis=0)
g_n = float(np.linalg.norm(gravity_init))
if g_n > 1e-6:
gravity_init = gravity_init * (9.80665 / g_n)
gyro_bias_by_session = {
p["session_id"]: np.asarray(p["gyro_bias_rad_s"], dtype=float) for p in prepared
}
time_offset_by_session = {
p["session_id"]: float(p["time_offset_s"]) for p in prepared
}
preexcluded_session_ids = {
p["session_id"] for p in prepared if not p["handeye_local"]["ok"]
}
if len(preexcluded_session_ids) == len(prepared):
_emit_progress(
progress_callback,
8,
"phase_a_complete",
accepted=False,
reason="all_sessions_failed_local_handeye_gate",
excluded_sessions=sorted(preexcluded_session_ids),
)
return finish(
status=CalibrationStatus.BLOCKED,
message=(
"Phase-A blocked: all sessions failed the local "
"rotation residual gate"
),
details={
"sessions": [_public_session(p) for p in prepared],
"joint_handeye": asdict(handeye),
"merged_pair_count": len(all_pairs),
"excluded_sessions": sorted(
preexcluded_session_ids
),
},
)
_emit_progress(
progress_callback,
8,
"phase_a_start",
session_count=len(prepared),
merged_pair_count=len(all_pairs),
preexcluded_sessions=sorted(preexcluded_session_ids),
)
phase_a_started_at = perf_counter()
def on_phase_a_progress(
event: str,
fields: dict[str, Any],
) -> None:
_emit_progress(
progress_callback,
8,
event,
**fields,
)
joint = solve_joint_extrinsic(
all_pairs,
handeye.R_IMU_lidar,
force_rotation_only=force_rotation_only,
imu=None,
delta_t_s=0.0,
gyro_bias_rad_s=gyro_bias,
gravity_init_m_s2=gravity_init,
gyro_bias_rad_s_by_session=gyro_bias_by_session,
time_offset_s_by_session=time_offset_by_session,
preexcluded_session_ids=preexcluded_session_ids,
rotation_prior=r_prior,
rotation_prior_sigma_deg=(
15.0 if prior_sigma_deg is None else prior_sigma_deg
),
phase_a_progress_callback=on_phase_a_progress,
enable_phase_c=not force_rotation_only,
t_init_m=t_prior,
t_prior_m=t_prior,
t_prior_sigma_m=t_prior_sigma,
)
included_sessions = [
item.session_id for item in joint.phase_a_sessions if item.included_in_final
]
excluded_sessions = [
item.session_id for item in joint.phase_a_sessions if not item.included_in_final
]
_emit_progress(
progress_callback,
8,
"phase_a_complete",
accepted=joint.phase_a_accepted,
joint_rms_deg=float(joint.residual_rms_rot_deg),
rotation_observable=joint.observability.rotation_observable,
included_sessions=included_sessions,
excluded_sessions=excluded_sessions,
elapsed_s=perf_counter() - phase_a_started_at,
)
for item in joint.phase_a_sessions:
_emit_progress(
progress_callback,
8,
"phase_a_session",
session=item.session_id,
included=item.included_in_final,
accepted=item.accepted,
pair_count=item.pair_count,
rms_deg=float(item.residual_rms_deg),
p95_deg=float(item.residual_p95_deg),
bias_delta_norm_rad_s=float(
np.linalg.norm(item.gyro_bias_rad_s - item.gyro_bias0_rad_s)
),
gyro_bias_rad_s=np.asarray(item.gyro_bias_rad_s, dtype=float).round(8).tolist(),
)
phase_a_by_session = {
item.session_id: item for item in joint.phase_a_sessions
}
session_results = []
for prep in prepared:
phase_a = phase_a_by_session.get(prep["session_id"])
session_bias = joint.gyro_bias_rad_s_per_session.get(prep["session_id"])
session_results.append(
{
**_public_session(prep),
@@ -418,6 +806,8 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
"handeye": {
"residual_rms_deg": handeye.residual_rms_deg,
"residual_median_deg": handeye.residual_median_deg,
"residual_p95_deg": handeye.residual_p95_deg,
"outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
"pair_count": handeye.pair_count,
"ok": handeye.ok,
"notes": tuple(list(handeye.notes) + [f"joint over {len(request.sessions)} sessions"]),
@@ -430,9 +820,10 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
"observability": asdict(joint.observability),
"notes": joint.notes,
"T_IMU_lidar": joint.T_IMU_lidar.tolist(),
"phase_a": None if phase_a is None else asdict(phase_a),
"gyro_bias_rad_s": None
if joint.gyro_bias_rad_s is None
else np.asarray(joint.gyro_bias_rad_s, dtype=float).tolist(),
if session_bias is None
else np.asarray(session_bias, dtype=float).tolist(),
"accel_bias_m_s2": None
if joint.accel_bias_m_s2 is None
else np.asarray(joint.accel_bias_m_s2, dtype=float).tolist(),
@@ -441,14 +832,40 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
else np.asarray(joint.gravity_m_s2, dtype=float).tolist(),
},
"translation_accepted": joint.translation_accepted,
"rotation_ok": handeye.ok and joint.observability.rotation_observable,
"rotation_ok": (
phase_a is not None
and phase_a.included_in_final
and phase_a.accepted
and joint.phase_a_accepted
and joint.observability.rotation_observable
),
"rotation_prior_constrained": (
phase_a is not None
and phase_a.included_in_final
and phase_a.accepted
and joint.phase_a_accepted
and not joint.observability.rotation_observable
and r_prior is not None
),
}
)
T = np.asarray(joint.T_IMU_lidar, dtype=float)
# Report per-session δt list; keep first as scalar for backward-compatible field.
delta_t = float(prepared[0]["time_offset_s"])
if request.requested_mode == CalibrationMode.FULL_SE3:
if request.requested_mode == CalibrationMode.ROTATION_ONLY:
# A rotation-only result must never expose a seed/prior translation,
# including when the rotation itself is rejected by a later gate.
T = T.copy()
T[:3, 3] = 0.0
# Multi-session offsets stay in details; the legacy scalar is single-session only.
delta_t = float(prepared[0]["time_offset_s"]) if len(prepared) == 1 else None
joint_rotation_ok = joint.phase_a_accepted
if not joint_rotation_ok:
status = CalibrationStatus.BLOCKED
message = (
f"joint rotation rejected: RMS={joint.residual_rms_rot_deg:.3f} deg "
"or a retained session failed the Phase-A residual gates"
)
elif request.requested_mode == CalibrationMode.FULL_SE3:
if joint.translation_accepted:
status = CalibrationStatus.FULL_SE3_ACCEPTED
message = f"full SE3 accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
@@ -456,15 +873,31 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
status = CalibrationStatus.FULL_SE3_REJECTED
message = (
f"rotation accepted jointly ({len(prepared)} sessions); "
"translation rejected by observability/residual gates"
"translation deferred until Phase-B/C session-state redesign"
)
else:
elif joint.observability.rotation_observable:
status = CalibrationStatus.ROTATION_ONLY_ACCEPTED
message = f"rotation-only calibration accepted (joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
message = (
f"rotation-only calibration accepted "
f"(joint {len(prepared)} sessions, {len(all_pairs)} pairs)"
)
T = T.copy()
T[:3, 3] = 0.0
elif r_prior is not None:
status = CalibrationStatus.ROTATION_ONLY_PRIOR_CONSTRAINED
message = (
"rotation residuals passed, but motion does not independently observe all "
"rotation axes; result remains constrained by the installation prior"
)
T = T.copy()
T[:3, 3] = 0.0
else:
status = CalibrationStatus.BLOCKED
message = "rotation residuals passed but rotation observability failed without a prior"
T = T.copy()
T[:3, 3] = 0.0
return finalize_result(
return finish(
status=status,
message=message,
details={
@@ -475,12 +908,26 @@ def run_calibration(request: CalibrationRequest) -> CalibrationResult:
"pair_counts_per_session": {p["session_id"]: p["pair_count"] for p in prepared},
"time_offset_s_per_session": {p["session_id"]: p["time_offset_s"] for p in prepared},
"handeye_rms_deg": handeye.residual_rms_deg,
"handeye_p95_deg": handeye.residual_p95_deg,
"handeye_outlier_fraction_gt_5deg": handeye.outlier_fraction_gt_5deg,
"phase_a_accepted": joint.phase_a_accepted,
"phase_a_comparison": joint.phase_a_comparison,
"phase_a_sessions": [asdict(item) for item in joint.phase_a_sessions],
"gyro_bias_rad_s_per_session": {
sid: np.asarray(value, dtype=float).tolist()
for sid, value in joint.gyro_bias_rad_s_per_session.items()
},
"excluded_sessions": [
item.session_id for item in joint.phase_a_sessions if not item.included_in_final
],
"joint_rotation_rms_deg": joint.residual_rms_rot_deg,
"rotation_observable": joint.observability.rotation_observable,
"translation_accepted": joint.translation_accepted,
},
"joint_handeye": asdict(handeye),
},
T_IMU_lidar=T,
T_IMU_lidar=None if status == CalibrationStatus.BLOCKED else T,
time_offset_s=delta_t,
output_directory=request.output_directory,
motion_pairs_payload=build_motion_pairs_payload(prepared_sessions=prepared),
)
imu_lidar/rotation_handeye.py
+74 -10
View File
@@ -16,6 +16,8 @@ class RotationHandeyeResult:
R_IMU_lidar: np.ndarray
residual_rms_deg: float
residual_median_deg: float
residual_p95_deg: float
outlier_fraction_gt_5deg: float
pair_count: int
ok: bool
notes: tuple[str, ...] = ()
@@ -28,17 +30,21 @@ def _pair_weight(pair: MotionPair) -> float:
return weight
def _tsai_rotation_initial(pairs: list[MotionPair]) -> np.ndarray:
def _tsai_rotation_initial(
pairs: list[MotionPair],
pair_weights: np.ndarray | None = None,
) -> np.ndarray:
"""Closed-form rotation hand-eye initial guess (Tsai-style linear solve)."""
rows: list[np.ndarray] = []
rhs: list[np.ndarray] = []
for pair in pairs:
weights = np.ones(len(pairs)) if pair_weights is None else np.asarray(pair_weights, dtype=float)
for pair, pair_weight in zip(pairs, weights):
alpha = so3_log(pair.R_A)
beta = so3_log(pair.R_B)
if np.linalg.norm(alpha) < 1e-6 or np.linalg.norm(beta) < 1e-6:
continue
w = np.sqrt(_pair_weight(pair))
w = np.sqrt(float(pair_weight))
rows.append(w * skew(alpha + beta))
rhs.append(w * (beta - alpha))
if len(rows) < 2:
@@ -64,6 +70,44 @@ def _rms_deg(r_x: np.ndarray, pairs: list[MotionPair]) -> float:
return float(np.sqrt(np.mean(errs**2)))
def select_strong_rotation_pairs(
pairs: list[MotionPair] | tuple[MotionPair, ...],
*,
min_rotation_deg: float = 1.0,
) -> list[MotionPair]:
"""Return pairs that independently excite rotation on both sensor sides."""
threshold = float(min_rotation_deg)
return [
pair
for pair in pairs
if rotation_angle_deg(pair.R_A) > threshold
and rotation_angle_deg(pair.R_B) > threshold
]
def estimate_rotation_handeye_initial(
pairs: list[MotionPair] | tuple[MotionPair, ...],
*,
min_rotation_deg: float = 1.0,
) -> np.ndarray:
"""Return the fast data-only Tsai initialization without nonlinear refine."""
usable = select_strong_rotation_pairs(
pairs,
min_rotation_deg=min_rotation_deg,
)
if not usable:
return np.eye(3)
raw_weights = np.asarray(
[_pair_weight(pair) for pair in usable],
dtype=float,
)
median = max(float(np.median(raw_weights)), 1e-12)
weights = np.clip(raw_weights / median, 0.1, 10.0)
return _tsai_rotation_initial(usable, weights)
def solve_rotation_handeye(
pairs: list[MotionPair] | tuple[MotionPair, ...],
*,
@@ -76,19 +120,24 @@ def solve_rotation_handeye(
is weakly observable under near-planar motion.
"""
usable = [pair for pair in pairs if rotation_angle_deg(pair.R_A) > 1.0 and rotation_angle_deg(pair.R_B) > 1.0]
usable = select_strong_rotation_pairs(pairs)
notes: list[str] = []
if len(usable) < 3:
return RotationHandeyeResult(
R_IMU_lidar=np.eye(3),
residual_rms_deg=1e9,
residual_median_deg=1e9,
residual_p95_deg=1e9,
outlier_fraction_gt_5deg=1.0,
pair_count=len(usable),
ok=False,
notes=("need at least 3 motion pairs with meaningful rotation",),
)
r0 = _tsai_rotation_initial(usable)
raw_weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
median_raw_weight = max(float(np.median(raw_weights)), 1e-12)
weights = np.clip(raw_weights / median_raw_weight, 0.1, 10.0)
r0 = _tsai_rotation_initial(usable, weights)
r_prior = None
if R_prior is not None:
r_prior = orthonormalize_rotation(np.asarray(R_prior, dtype=float).reshape(3, 3))
@@ -104,10 +153,11 @@ def solve_rotation_handeye(
f"init from Tsai (rms={rms_tsai:.3f} deg; prior {rms_prior:.3f} deg kept as soft constraint)"
)
weights = np.asarray([_pair_weight(pair) for pair in usable], dtype=float)
notes.append(
f"weighted hand-eye: weight median={float(np.median(weights)):.3g}, "
f"min={float(np.min(weights)):.3g}, max={float(np.max(weights)):.3g}"
"weighted hand-eye: normalized/clipped IMU confidence "
f"raw_median={median_raw_weight:.3g}, "
f"normalized_min={float(np.min(weights)):.3g}, "
f"normalized_max={float(np.max(weights)):.3g}"
)
def pack(r: np.ndarray) -> np.ndarray:
@@ -139,14 +189,28 @@ def solve_rotation_handeye(
# Report unweighted RMS/median for interpretability.
rms = float(np.sqrt(np.mean(errs**2)))
med = float(np.median(errs))
p95 = float(np.percentile(errs, 95.0))
outlier_fraction = float(np.mean(errs > 5.0))
notes.append(f"optimized over {len(usable)} pairs")
ok = rms < 5.0 and len(usable) >= 3
notes.append(
f"rotation residual quality: rms={rms:.3f} deg, median={med:.3f} deg, "
f"p95={p95:.3f} deg, >5deg={100.0 * outlier_fraction:.2f}%"
)
ok = (
len(usable) >= 3
and rms < 1.5
and med < 0.5
and p95 < 1.5
and outlier_fraction <= 0.005
)
if not ok:
notes.append("rotation residual RMS too high or too few pairs")
notes.append("rotation residual distribution failed acceptance gates")
return RotationHandeyeResult(
R_IMU_lidar=r_x,
residual_rms_deg=rms,
residual_median_deg=med,
residual_p95_deg=p95,
outlier_fraction_gt_5deg=outlier_fraction,
pair_count=len(usable),
ok=ok,
notes=tuple(notes),
imu_lidar/文件职责说明.md
+1 -1
View File
@@ -57,7 +57,7 @@ python -m imu_lidar.cli run --vehicle-config ... --imu ... --lidar ... --output
- **运动对**始终计算完整预积分量（旋转、速度增量、位移增量及不确定度）。
- `--mode rotation_only`：只精修旋转与常值陀螺零偏，交付旋转与时间偏置。
- `--mode full_se3`：在可观时再估计重力、关键帧速度、时变零偏与平移；结果写入 `summary.json` 的 joint 字段。
- `--mode full_se3`：当前完成 Phase-A 后明确拒绝平移；待 Phase-B/C 会话状态重构完成后再恢复完整 SE(3) 交付。
---