346 lines
12 KiB
Python
346 lines
12 KiB
Python
"""Command-line entry point for LiDAR–IMU calibration."""
|
||
|
||
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)
|
||
|
||
plan = subcommands.add_parser("plan", help="显示标定阶段，不读取数据")
|
||
plan.add_argument("--vehicle-config", help="车辆配置路径（仅展示，plan 不读取）")
|
||
plan.add_argument(
|
||
"--mode",
|
||
choices=[mode.value for mode in CalibrationMode],
|
||
default=CalibrationMode.ROTATION_ONLY.value,
|
||
)
|
||
|
||
run = subcommands.add_parser(
|
||
"run",
|
||
help="执行 V1 标定流水线（可重复 --imu/--lidar/--session-id 做多会话联合）",
|
||
)
|
||
run.add_argument(
|
||
"--session-id",
|
||
action="append",
|
||
default=None,
|
||
help="会话 ID（可重复；与 --imu/--lidar 一一对应）",
|
||
)
|
||
run.add_argument(
|
||
"--imu",
|
||
action="append",
|
||
required=True,
|
||
help="IMU CSV/NPZ 路径（可重复）",
|
||
)
|
||
run.add_argument(
|
||
"--lidar",
|
||
action="append",
|
||
required=True,
|
||
help="LiDAR 会话目录（可重复）",
|
||
)
|
||
run.add_argument("--vehicle-config", required=True, help="车辆配置 YAML")
|
||
run.add_argument("--output", required=True, help="输出目录")
|
||
run.add_argument(
|
||
"--mode",
|
||
choices=[mode.value for mode in CalibrationMode],
|
||
default=CalibrationMode.ROTATION_ONLY.value,
|
||
)
|
||
run.add_argument("--max-iterations", type=int, default=2)
|
||
run.add_argument("--time-offset-search-s", type=float, default=1.0)
|
||
run.add_argument(
|
||
"--fixed-time-offset-s",
|
||
type=float,
|
||
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",
|
||
help="Disable signed 3-axis δt refine after hand-eye (recommended for host-bridged data)",
|
||
)
|
||
run.add_argument(
|
||
"--max-signed-refine-shift-s",
|
||
type=float,
|
||
default=0.05,
|
||
help="Max |Δδt| accepted by signed refine from the coarse estimate",
|
||
)
|
||
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
|
||
|
||
|
||
def _build_sessions(args: argparse.Namespace) -> tuple[SessionInput, ...]:
|
||
imus = [Path(p) for p in args.imu]
|
||
lidars = [Path(p) for p in args.lidar]
|
||
if len(imus) != len(lidars):
|
||
raise SystemExit(f"--imu count ({len(imus)}) must match --lidar count ({len(lidars)})")
|
||
if args.session_id is None:
|
||
session_ids = [f"session{i}" for i in range(len(imus))]
|
||
else:
|
||
session_ids = list(args.session_id)
|
||
if len(session_ids) != len(imus):
|
||
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,
|
||
fixed_time_offset_s=offset,
|
||
)
|
||
for sid, imu, lidar, offset in zip(session_ids, imus, lidars, session_offsets)
|
||
)
|
||
|
||
|
||
def main(argv: list[str] | None = None) -> int:
|
||
parser = build_parser()
|
||
args = parser.parse_args(argv)
|
||
|
||
if args.command == "plan":
|
||
request = CalibrationRequest(
|
||
vehicle_config=Path(args.vehicle_config) if args.vehicle_config else None,
|
||
requested_mode=CalibrationMode(args.mode),
|
||
)
|
||
print("LiDAR–IMU calibration stages:")
|
||
print(f"requested mode: {request.requested_mode.value}")
|
||
for index, stage in enumerate(describe_pipeline(request), start=1):
|
||
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(
|
||
vehicle_config=Path(args.vehicle_config),
|
||
sessions=sessions,
|
||
requested_mode=CalibrationMode(args.mode),
|
||
output_directory=Path(args.output),
|
||
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, progress_callback=_print_progress)
|
||
print(f"status: {result.status.value}")
|
||
print(f"message: {result.message}")
|
||
if result.time_offset_s is not None:
|
||
print(f"time_offset_s (first session; t_imu = t_lidar + dt): {result.time_offset_s:.6f}")
|
||
joint = (result.details or {}).get("joint") or {}
|
||
if joint:
|
||
print(f"merged_pair_count: {joint.get('merged_pair_count')}")
|
||
print(f"pair_counts_per_session: {joint.get('pair_counts_per_session')}")
|
||
if result.T_IMU_lidar is not None:
|
||
print("T_IMU_lidar:")
|
||
print(result.T_IMU_lidar)
|
||
print(f"report directory: {args.output}")
|
||
return 0 if result.status != CalibrationStatus.BLOCKED else 2
|
||
|
||
parser.error(f"unknown command {args.command}")
|
||
return 2
|
||
|
||
|
||
if __name__ == "__main__":
|
||
raise SystemExit(main())
|