942 lines
34 KiB
Python
942 lines
34 KiB
Python
"""Executable LiDAR–IMU calibration pipeline (V1)."""
|
||
|
||
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
|
||
|
||
from .contracts import (
|
||
CalibrationMode,
|
||
CalibrationRequest,
|
||
CalibrationResult,
|
||
CalibrationStatus,
|
||
MotionPair,
|
||
SessionInput,
|
||
)
|
||
from .finalize import finalize_result
|
||
from .imu_audit import audit_imu
|
||
from .imu_io import load_imu_samples
|
||
from .joint_optimizer import solve_joint_extrinsic
|
||
from .keyframes import build_keyframes
|
||
from .lidar_deskew import deskew_lidar_frames
|
||
from .lidar_io import load_lidar_frames
|
||
from .motion_pairs import build_motion_pairs
|
||
from .motion_pairs_io import build_motion_pairs_payload
|
||
from .rotation_handeye import solve_rotation_handeye
|
||
from .time_offset import TimeOffsetResult, estimate_time_offset, refine_time_offset_signed
|
||
from .timestamp_audit import audit_timestamps
|
||
from .vehicle_config import load_vehicle_config, prior_enabled
|
||
|
||
# Remap keyframe indices so multi-session Phase-C graphs do not collide.
|
||
_SESSION_INDEX_OFFSET = 1_000_000
|
||
|
||
|
||
def _merge_time_offset(previous: TimeOffsetResult, refined: TimeOffsetResult) -> TimeOffsetResult:
|
||
return TimeOffsetResult(
|
||
delta_t_s=refined.delta_t_s,
|
||
correlation_peak=refined.correlation_peak,
|
||
search_s=previous.search_s,
|
||
notes=tuple(list(previous.notes) + list(refined.notes)),
|
||
ok=True,
|
||
)
|
||
|
||
|
||
@dataclass(frozen=True)
|
||
class PipelineStage:
|
||
name: str
|
||
responsibility: str
|
||
|
||
|
||
STAGES = (
|
||
PipelineStage("vehicle_config", "加载并校验当前车辆安装配置"),
|
||
PipelineStage("timestamp_audit", "审查 IMU 与 LiDAR 时间域"),
|
||
PipelineStage("imu_audit", "审查单位、轴向启发与静止零偏"),
|
||
PipelineStage("time_offset", "各会话独立粗估/精修 δt"),
|
||
PipelineStage("lidar_motion", "各会话关键帧、可选去畸变与 LiDAR 相对运动"),
|
||
PipelineStage("motion_pairs", "各会话构造运动对，再合并"),
|
||
PipelineStage("rotation_handeye", "用全部会话运动对联合求解旋转外参"),
|
||
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."""
|
||
|
||
return STAGES
|
||
|
||
|
||
def _build_pairs_and_handeye(
|
||
*,
|
||
session_id: str,
|
||
working_frames,
|
||
imu,
|
||
delta_t_s: float,
|
||
gyro_bias_rad_s: np.ndarray,
|
||
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),
|
||
keyframe_indices=keyframes.indices,
|
||
imu=imu,
|
||
delta_t_s=delta_t_s,
|
||
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,
|
||
R_prior=R_prior,
|
||
prior_sigma_deg=prior_sigma_deg,
|
||
)
|
||
return keyframes, pair_set, handeye
|
||
|
||
|
||
def _translation_prior_from_config(
|
||
vehicle_config: dict[str, Any] | None,
|
||
) -> tuple[np.ndarray | None, np.ndarray | float | None]:
|
||
if vehicle_config is None or not prior_enabled(vehicle_config, "translation_prior"):
|
||
return None, None
|
||
init_cfg = vehicle_config.get("initialization") or {}
|
||
tp = init_cfg.get("translation_prior") or {}
|
||
if tp.get("t_IMU_lidar_m") is None:
|
||
return None, None
|
||
return np.asarray(tp["t_IMU_lidar_m"], dtype=float).reshape(3), tp.get("sigma_m", [0.05, 0.05, 0.05])
|
||
|
||
|
||
def _rotation_prior_from_config(
|
||
vehicle_config: dict[str, Any] | None,
|
||
) -> tuple[np.ndarray | None, float | None]:
|
||
if vehicle_config is None or not prior_enabled(vehicle_config, "rotation_prior"):
|
||
return None, None
|
||
init_cfg = vehicle_config.get("initialization") or {}
|
||
rp = init_cfg.get("rotation_prior") or {}
|
||
if rp.get("R_IMU_lidar") is None:
|
||
return None, None
|
||
return np.asarray(rp["R_IMU_lidar"], dtype=float).reshape(3, 3), float(rp.get("sigma_deg", 15.0))
|
||
|
||
|
||
def _prepare_session_pairs(
|
||
session: SessionInput,
|
||
request: CalibrationRequest,
|
||
*,
|
||
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)}
|
||
|
||
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(fixed_time_offset_s),
|
||
correlation_peak=1.0,
|
||
search_s=0.0,
|
||
notes=(
|
||
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,
|
||
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||
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)
|
||
handeye = None
|
||
pair_set = None
|
||
keyframes = None
|
||
pairs_notes: list[str] = []
|
||
pair_count = 0
|
||
|
||
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,
|
||
delta_t_s=offset.delta_t_s,
|
||
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,
|
||
imu=imu,
|
||
delta_t_s=offset.delta_t_s,
|
||
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||
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",
|
||
"session_id": session.session_id,
|
||
"iteration": iteration,
|
||
"time_offset": asdict(offset),
|
||
"imu_audit": asdict(imu_report),
|
||
"timestamp_audit": asdict(ts),
|
||
"keyframes": 0 if keyframes is None else len(keyframes.indices),
|
||
"pair_notes": pairs_notes,
|
||
"handeye": asdict(handeye),
|
||
}
|
||
r_x = handeye.R_IMU_lidar
|
||
|
||
if not request.enable_signed_time_refine:
|
||
continue
|
||
|
||
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,
|
||
delta_t_s=offset.delta_t_s,
|
||
R_IMU_lidar=r_x,
|
||
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||
search_s=min(0.12, max(0.04, 0.25 * request.time_offset_search_s)),
|
||
max_shift_s=request.max_signed_refine_shift_s,
|
||
)
|
||
# Also bound total walk away from the original coarse estimate.
|
||
if abs(refined.delta_t_s - coarse_delta_t) > request.max_signed_refine_shift_s:
|
||
refined = TimeOffsetResult(
|
||
delta_t_s=float(offset.delta_t_s),
|
||
correlation_peak=refined.correlation_peak,
|
||
search_s=refined.search_s,
|
||
notes=tuple(
|
||
list(refined.notes)
|
||
+ [
|
||
f"signed refine clamped: |δt-coarse| would exceed "
|
||
f"{request.max_signed_refine_shift_s:.3f}s"
|
||
]
|
||
),
|
||
ok=True,
|
||
)
|
||
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,
|
||
imu=imu,
|
||
delta_t_s=offset.delta_t_s,
|
||
gyro_bias_rad_s=imu_report.gyro_bias_rad_s,
|
||
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",
|
||
"session_id": session.session_id,
|
||
"iteration": iteration,
|
||
"time_offset": asdict(offset),
|
||
"imu_audit": asdict(imu_report),
|
||
"timestamp_audit": asdict(ts),
|
||
"keyframes": 0 if keyframes is None else len(keyframes.indices),
|
||
"pair_notes": pairs_notes,
|
||
"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))
|
||
if acc_n > 1e-6:
|
||
gravity_init = -acc_mean * (9.80665 / acc_n)
|
||
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,
|
||
"pairs": tuple(pair_set.pairs),
|
||
"gyro_bias_rad_s": np.asarray(imu_report.gyro_bias_rad_s, dtype=float).reshape(3),
|
||
"gravity_init_m_s2": gravity_init,
|
||
"timestamp_audit": asdict(ts),
|
||
"imu_audit": {
|
||
**asdict(imu_report),
|
||
"gyro_bias_rad_s": imu_report.gyro_bias_rad_s.tolist(),
|
||
"static_acc_mean_m_s2": imu_report.static_acc_mean_m_s2.tolist(),
|
||
},
|
||
"time_offset": asdict(offset),
|
||
"time_offset_s": float(offset.delta_t_s),
|
||
"keyframes": len(keyframes.indices),
|
||
"pair_count": pair_count,
|
||
"pair_notes": pairs_notes,
|
||
"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,
|
||
"R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
|
||
},
|
||
}
|
||
|
||
|
||
def _remap_pairs_for_joint(prepared: list[dict[str, Any]]) -> list[MotionPair]:
|
||
merged: list[MotionPair] = []
|
||
for index, prep in enumerate(prepared):
|
||
id_offset = (index + 1) * _SESSION_INDEX_OFFSET
|
||
for pair in prep["pairs"]:
|
||
merged.append(
|
||
replace(
|
||
pair,
|
||
i=int(pair.i) + id_offset,
|
||
j=int(pair.j) + id_offset,
|
||
)
|
||
)
|
||
return merged
|
||
|
||
|
||
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 finish(
|
||
status=CalibrationStatus.BLOCKED,
|
||
message="no sessions provided",
|
||
details={},
|
||
)
|
||
|
||
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
|
||
_emit_progress(
|
||
progress_callback,
|
||
1,
|
||
"blocked",
|
||
reason="vehicle_config",
|
||
error=str(exc),
|
||
)
|
||
return finish(
|
||
status=CalibrationStatus.BLOCKED,
|
||
message=f"vehicle config failed: {exc}",
|
||
details={},
|
||
)
|
||
_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]] = []
|
||
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 finish(
|
||
status=CalibrationStatus.BLOCKED,
|
||
message=f"blocked at stage {prep.get('stage')} ({prep.get('session_id')})",
|
||
details={"sessions": [prep]},
|
||
)
|
||
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,
|
||
)
|
||
_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={
|
||
"sessions": [_public_session(p) for p in prepared],
|
||
"joint_handeye": asdict(handeye),
|
||
"merged_pair_count": len(all_pairs),
|
||
},
|
||
)
|
||
|
||
force_rotation_only = request.requested_mode == CalibrationMode.ROTATION_ONLY
|
||
t_prior, t_prior_sigma = _translation_prior_from_config(vehicle_config)
|
||
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,
|
||
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),
|
||
"vehicle_config_loaded": vehicle_config is not None,
|
||
"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"]),
|
||
"R_IMU_lidar": handeye.R_IMU_lidar.tolist(),
|
||
},
|
||
"joint": {
|
||
"translation_accepted": joint.translation_accepted,
|
||
"residual_rms_rot_deg": joint.residual_rms_rot_deg,
|
||
"residual_rms_trans_m": joint.residual_rms_trans_m,
|
||
"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 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(),
|
||
"gravity_m_s2": None
|
||
if joint.gravity_m_s2 is None
|
||
else np.asarray(joint.gravity_m_s2, dtype=float).tolist(),
|
||
},
|
||
"translation_accepted": joint.translation_accepted,
|
||
"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)
|
||
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)"
|
||
else:
|
||
status = CalibrationStatus.FULL_SE3_REJECTED
|
||
message = (
|
||
f"rotation accepted jointly ({len(prepared)} sessions); "
|
||
"translation deferred until Phase-B/C session-state redesign"
|
||
)
|
||
elif joint.observability.rotation_observable:
|
||
status = CalibrationStatus.ROTATION_ONLY_ACCEPTED
|
||
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 finish(
|
||
status=status,
|
||
message=message,
|
||
details={
|
||
"sessions": session_results,
|
||
"joint": {
|
||
"session_count": len(prepared),
|
||
"merged_pair_count": len(all_pairs),
|
||
"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=None if status == CalibrationStatus.BLOCKED else T,
|
||
time_offset_s=delta_t,
|
||
motion_pairs_payload=build_motion_pairs_payload(prepared_sessions=prepared),
|
||
)
|
||
|
||
|
||
def _public_session(session_result: dict[str, Any]) -> dict[str, Any]:
|
||
payload = dict(session_result)
|
||
payload.pop("T_IMU_lidar", None)
|
||
payload.pop("pairs", None)
|
||
payload.pop("gyro_bias_rad_s", None)
|
||
payload.pop("gravity_init_m_s2", None)
|
||
return payload
|