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

imu_lidar/cli.py

@@ -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: