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/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),
)