完善Phase-A会话级联合优化并修正雷达相位中心高度先验
This commit is contained in:
lichun.qu
@@ -18,7 +18,8 @@ installation:
|
||||
HI13R4 + H32 DLogCapture. Body +X forward: LiDAR and IMU at positive X.
|
||||
CAD sheet may draw +X rearward; numbers below are body-frame.
|
||||
Z is height above ground = CAD height at axle + 0.294 m (axle AGL).
|
||||
LiDAR phase-center AGL is the measured 1.9165 m (not CAD dZ+63.5).
|
||||
LiDAR CAD dZ is 1637.499879 mm relative to the axle reference. Phase-center
|
||||
AGL adds rear-axle height 294 mm and the 63.5 mm phase-center offset.
|
||||
IMU axes: HI13R4 manual §2.4 RFU (X right, Y forward, Z up).
|
||||
LiDAR Cartesian in NPZ assumed body-aligned (X forward).
|
||||
|
||||
@@ -45,11 +46,12 @@ sensors:
|
||||
axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
|
||||
driver_axis_remapped: false
|
||||
mount_in_body:
|
||||
# X/Y：后轮轴中心 → 雷达；Z：相位中心真实离地 1.9165 m
|
||||
translation_m: [2.522276859, 0.000020526, 1.9165]
|
||||
# X/Y：后轮轴中心 → 雷达
|
||||
# Z离地 = CAD dZ 1.637499879 + 后轮轴离地 0.294 + 相位中心偏移 0.0635
|
||||
translation_m: [2.522276859, 0.000020526, 1.994999879]
|
||||
rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
|
||||
rotation_quaternion_xyzw: null
|
||||
source: "CAD X/Y in body (+X forward); Z = measured phase-center AGL 1.9165 m; attitude = body"
|
||||
source: "CAD X/Y in body (+X forward); Z AGL = CAD dZ 1.637499879 + axle AGL 0.294 + phase-center offset 0.0635; attitude = body"
|
||||
|
||||
rtk:
|
||||
frame_definition: ""
|
||||
@@ -67,17 +69,17 @@ time:
|
||||
# t_IMU_lidar = R_IMU_body * t_body, R_IMU_lidar = R_IMU_body * R_body_lidar
|
||||
derived_T_IMU_lidar_prior:
|
||||
R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
|
||||
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.730]
|
||||
t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.730]
|
||||
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
|
||||
t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879]
|
||||
notes: >
|
||||
Rotation prior ~90 deg yaw (body/lidar X-fwd vs IMU Y-fwd).
|
||||
Relative Z = 1.9165 - 1.1865 = 0.730 m (was 0.8085 m with old CAD+63.5mm Z).
|
||||
Relative Z = 1.994999879 - 1.1865 = 0.808499879 m.
|
||||
|
||||
initialization:
|
||||
translation_prior:
|
||||
enabled: true
|
||||
sigma_m: [0.05, 0.05, 0.05]
|
||||
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.730]
|
||||
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
|
||||
rotation_prior:
|
||||
enabled: true
|
||||
sigma_deg: 15.0
|
||||
|
||||
@@ -96,16 +96,16 @@ t = [0, 0, 0]
|
||||
| 传感器 | X / Y（后轮轴中心） | Z（离地） |
|
||||
|--------|---------------------|-----------|
|
||||
| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
|
||||
| 雷达 | 2.522 / 0.00002 m | 相位中心实测 **1.9165 m** |
|
||||
| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
|
||||
|
||||
- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。
|
||||
- 雷达相位中心以实测 1.9165 m 为准（不再用「CAD dZ + 63.5 mm」作为最终离地高）。
|
||||
- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。
|
||||
|
||||
### 3.3 导出外参先验
|
||||
|
||||
- `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。
|
||||
- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.730]` m**（相对 Z = 1.9165 − 1.1865）。
|
||||
- **旋转先验未因 Z 修正改变**；仅平移先验 Z 从旧值 0.8085 改为 0.730。
|
||||
- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
|
||||
- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
|
||||
|
||||
---
|
||||
|
||||
|
||||
+4
-4
@@ -96,16 +96,16 @@ t = [0, 0, 0]
|
||||
| 传感器 | X / Y（后轮轴中心） | Z（离地） |
|
||||
|--------|---------------------|-----------|
|
||||
| IMU | 2.574 / 0.0365 m | 0.8925 + 0.294 = **1.1865 m** |
|
||||
| 雷达 | 2.522 / 0.00002 m | 相位中心实测 **1.9165 m** |
|
||||
| 雷达 | 2.522 / 0.00002 m | 相位中心离地 **1.994999879 m** |
|
||||
|
||||
- 后轮轴中心离地：**294 mm**（Z 用离地高时加在 CAD 轴心高上）。
|
||||
- 雷达相位中心以实测 1.9165 m 为准（不再用「CAD dZ + 63.5 mm」作为最终离地高）。
|
||||
- 雷达 CAD `dZ=1.637499879 m`；相位中心离地还需加后轮轴离地 `0.294 m` 和相位中心偏移 `0.0635 m`，最终为 `1.994999879 m`。
|
||||
|
||||
### 3.3 导出外参先验
|
||||
|
||||
- `R_IMU_lidar` ≈ yaw 90°：`[[0,-1,0],[1,0,0],[0,0,1]]`（软约束 σ=15°）。
|
||||
- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.730]` m**（相对 Z = 1.9165 − 1.1865）。
|
||||
- **旋转先验未因 Z 修正改变**；仅平移先验 Z 从旧值 0.8085 改为 0.730。
|
||||
- `t_IMU_lidar` ≈ **`[0.0365, -0.0518, 0.8085]` m**（相对 Z = 1.994999879 − 1.1865 = 0.808499879 m）。
|
||||
- **旋转先验不因 Z 修正改变**；平移先验 Z 更新为 0.808499879 m。
|
||||
|
||||
---
|
||||
|
||||
|
||||
+202
-3
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
+451
-19
@@ -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),
|
||||
)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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))
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
+40
-30
@@ -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,
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
+485
-38
@@ -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),
|
||||
)
|
||||
|
||||
|
||||
@@ -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),
|
||||
|
||||
+1
-1
@@ -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) 交付。
|
||||
|
||||
---
|
||||
|
||||
|
||||
+8
-2
@@ -28,8 +28,14 @@ python -m pytest -q
|
||||
| `test_preintegration_bias_jacobian_matches_finite_difference` | 随机陀螺序列 | 旋转预积分 `J_bg` | 与有限差分一致（松阈值） |
|
||||
| `test_imu_preintegration_recovers_constant_accel_translation` | 常值加速度 | 完整预积分 Δv/Δp | 接近解析值 |
|
||||
| `test_imu_preintegration_bias_jacobian_finite_difference` | 随机 IMU | `J_bg`/`J_ba` 一阶修正 | 与重积分接近 |
|
||||
| `test_synthetic_pipeline_rotation_and_time_offset` | 端到端合成会话 | `rotation_only` 全流程 | `rotation_only_accepted`；δt 准；手眼 RMS < 5° |
|
||||
| `test_synthetic_pipeline_full_se3_smoke` | 同上 | `full_se3` 不崩溃 | 状态为 accepted / rejected / rotation_only 之一 |
|
||||
| `test_synthetic_pipeline_rejects_noisy_icp_but_keeps_time_audit` | synthetic end-to-end | strict rotation quality gate + time audit | noisy ICP is blocked; delta-t remains accurate |
|
||||
| `test_synthetic_pipeline_full_se3_smoke` | synthetic end-to-end | full-SE(3) smoke test | returns an explicit accepted/rejected/blocked status |
|
||||
| `test_planar_yaw_is_not_full_rotation_or_translation_observable` | pure-yaw motion pairs | degeneracy detection | full rotation/translation observability is rejected |
|
||||
| `test_multi_axis_motion_is_rotation_and_translation_observable` | multi-axis motion pairs | positive observability case | rotation and translation pass |
|
||||
| `test_translation_prior_is_reported_but_not_accepted_when_unobservable` | planar motion + CAD prior | prior semantics | prior is reported but not accepted as calibration |
|
||||
| `test_handeye_rejects_a_small_fraction_of_gross_rotation_outliers` | motion pairs with a gross outlier | residual-distribution gate | solve is rejected |
|
||||
| `test_motion_pairs_reject_low_fitness` | low-fitness registration | fitness gate | no motion pair is emitted |
|
||||
| `test_motion_pairs_reject_imu_and_lidar_discontinuities` | timestamp gaps | continuity gates | cross-gap pairs are rejected |
|
||||
|
||||
|
||||
合成数据由 `tools/generate_synthetic_session.py` 生成（墙面点云 + 已知外参 yaw 与 δt）。
|
||||
|
||||
@@ -29,14 +29,29 @@ def test_pair_roundtrip(tmp_path: Path) -> None:
|
||||
t_A_m=np.array([0.1, 0.0, 0.0]),
|
||||
t_B_m=np.array([0.1, 0.0, 0.0]),
|
||||
fitness=0.8,
|
||||
metadata={"weight": 12.0, "cov9": [[0.0] * 9] * 9, "backend": "test"},
|
||||
metadata={
|
||||
"weight": 12.0,
|
||||
"cov": (np.eye(3) * 1e-4).tolist(),
|
||||
"J_bg": (-np.eye(3)).tolist(),
|
||||
"cov9": [[0.0] * 9] * 9,
|
||||
"backend": "test",
|
||||
"gyro_bias0_rad_s": [0.01, -0.02, 0.03],
|
||||
"accel_bias0_m_s2": [0.1, 0.2, -0.1],
|
||||
"time_offset_s": 0.004,
|
||||
"keyframe_span": 3,
|
||||
"is_consecutive": False,
|
||||
},
|
||||
)
|
||||
encoded = pair_to_dict(pair)
|
||||
assert "cov9" not in encoded["metadata"]
|
||||
assert "cov" in encoded["metadata"]
|
||||
assert "J_bg" in encoded["metadata"]
|
||||
assert encoded["metadata"]["weight"] == 12.0
|
||||
restored = pair_from_dict(encoded)
|
||||
assert restored.i == 1 and restored.j == 4
|
||||
np.testing.assert_allclose(restored.t_A_m, [0.1, 0.0, 0.0])
|
||||
np.testing.assert_allclose(restored.metadata["gyro_bias0_rad_s"], [0.01, -0.02, 0.03])
|
||||
assert restored.metadata["keyframe_span"] == 3
|
||||
|
||||
payload = build_motion_pairs_payload(
|
||||
prepared_sessions=[
|
||||
@@ -50,6 +65,14 @@ def test_pair_roundtrip(tmp_path: Path) -> None:
|
||||
)
|
||||
path = save_motion_pairs(tmp_path / "motion_pairs.json", payload)
|
||||
loaded = load_motion_pairs(path)
|
||||
assert loaded["schema_version"] == 2
|
||||
pairs = pairs_for_session(loaded, "s0")
|
||||
assert len(pairs) == 1
|
||||
assert pairs[0].session_id == "s0"
|
||||
|
||||
payload["schema_version"] = 1
|
||||
legacy_path = save_motion_pairs(
|
||||
tmp_path / "motion_pairs_v1.json", payload
|
||||
)
|
||||
legacy = load_motion_pairs(legacy_path)
|
||||
assert legacy["schema_version"] == 1
|
||||
|
||||
@@ -0,0 +1,194 @@
|
||||
"""Tests for cached, session-balanced Phase-A comparison."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from imu_lidar.contracts import ImuSeries, MotionPair
|
||||
from imu_lidar.geometry import so3_exp, so3_log
|
||||
from imu_lidar.imu_preintegration import preintegrate_gyro
|
||||
from imu_lidar.phase_a import (
|
||||
rehydrate_phase_a_pairs,
|
||||
select_decorrelated_phase_a_pairs,
|
||||
solve_phase_a_comparison,
|
||||
)
|
||||
|
||||
|
||||
def _phase_a_pair(
|
||||
session_id: str,
|
||||
index: int,
|
||||
r_true: np.ndarray,
|
||||
vector_deg: tuple[float, float, float],
|
||||
bias0: np.ndarray,
|
||||
) -> MotionPair:
|
||||
r_b = so3_exp(np.deg2rad(np.asarray(vector_deg, dtype=float)))
|
||||
return MotionPair(
|
||||
session_id=session_id,
|
||||
i=index,
|
||||
j=index + 1,
|
||||
t_i_s=float(index),
|
||||
t_j_s=float(index + 1),
|
||||
R_A=r_true @ r_b @ r_true.T,
|
||||
R_B=r_b,
|
||||
t_A_m=np.zeros(3),
|
||||
t_B_m=np.zeros(3),
|
||||
metadata={
|
||||
"J_bg": (-np.eye(3)).tolist(),
|
||||
"cov": (np.eye(3) * 1e-5).tolist(),
|
||||
"gyro_bias0_rad_s": bias0.tolist(),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def test_phase_a_reports_three_variants_and_leave_one_session_out() -> None:
|
||||
r_true = so3_exp(np.deg2rad(np.array([3.0, -2.0, 25.0])))
|
||||
prior = so3_exp(np.deg2rad(np.array([0.0, 0.0, 0.2]))) @ r_true
|
||||
vectors = (
|
||||
(12.0, 0.0, 0.0),
|
||||
(0.0, 15.0, 0.0),
|
||||
(0.0, 0.0, 18.0),
|
||||
(10.0, 8.0, 0.0),
|
||||
(0.0, 11.0, 9.0),
|
||||
(7.0, 0.0, 13.0),
|
||||
(9.0, -5.0, 6.0),
|
||||
(-6.0, 8.0, 11.0),
|
||||
(5.0, 7.0, -9.0),
|
||||
)
|
||||
biases = {
|
||||
"s0": np.array([0.001, -0.0005, 0.0002]),
|
||||
"s1": np.array([-0.0004, 0.0008, -0.0001]),
|
||||
"s2": np.array([0.0002, 0.0001, -0.0006]),
|
||||
}
|
||||
pairs: list[MotionPair] = []
|
||||
index = 0
|
||||
for sid, count in (("s0", 18), ("s1", 9), ("s2", 6)):
|
||||
for local_index in range(count):
|
||||
pairs.append(
|
||||
_phase_a_pair(
|
||||
sid,
|
||||
index,
|
||||
r_true,
|
||||
vectors[local_index % len(vectors)],
|
||||
biases[sid],
|
||||
)
|
||||
)
|
||||
index += 1
|
||||
|
||||
result = solve_phase_a_comparison(
|
||||
pairs,
|
||||
gyro_bias_rad_s_by_session=biases,
|
||||
rotation_prior=prior,
|
||||
rotation_prior_sigma_deg=15.0,
|
||||
yaw_std_max_deg=1.0,
|
||||
leave_one_out_yaw_range_max_deg=1.0,
|
||||
data_prior_difference_max_deg=1.0,
|
||||
decorrelation_block_s=0.0,
|
||||
max_nfev=80,
|
||||
)
|
||||
|
||||
assert result.accepted
|
||||
assert result.strong_pair_counts_per_session == {
|
||||
"s0": 18,
|
||||
"s1": 9,
|
||||
"s2": 6,
|
||||
}
|
||||
assert len(result.leave_one_out) == 3
|
||||
assert result.marginal_observability.rank == 3
|
||||
assert result.leave_one_out_yaw_range_deg < 0.1
|
||||
for variant in (
|
||||
result.fixed_bg_data_only,
|
||||
result.session_bg_data_only,
|
||||
result.session_bg_with_rotation_prior,
|
||||
):
|
||||
error_deg = np.degrees(
|
||||
np.linalg.norm(
|
||||
so3_log(r_true.T @ variant.R_IMU_lidar)
|
||||
)
|
||||
)
|
||||
assert error_deg < 0.1
|
||||
|
||||
|
||||
def test_rehydrate_phase_a_pairs_recovers_jacobian_without_lidar() -> None:
|
||||
t_s = np.linspace(0.0, 1.0, 201)
|
||||
gyro = np.tile(np.array([0.12, -0.04, 0.2]), (t_s.size, 1))
|
||||
bias0 = np.array([0.01, -0.005, 0.002])
|
||||
imu = ImuSeries(
|
||||
t_s=t_s,
|
||||
gyro_rad_s=gyro,
|
||||
acc_m_s2=np.zeros((t_s.size, 3)),
|
||||
)
|
||||
preint = preintegrate_gyro(t_s, gyro, 0.1, 0.8, bias0)
|
||||
pair = MotionPair(
|
||||
session_id="s0",
|
||||
i=0,
|
||||
j=1,
|
||||
t_i_s=0.1,
|
||||
t_j_s=0.8,
|
||||
R_A=preint.delta_R,
|
||||
R_B=preint.delta_R,
|
||||
metadata={
|
||||
"t_i_imu_s": 0.1,
|
||||
"t_j_imu_s": 0.8,
|
||||
"gyro_bias0_rad_s": bias0.tolist(),
|
||||
"preint_sigma_rad": preint.sigma_rad,
|
||||
},
|
||||
)
|
||||
|
||||
enriched, report = rehydrate_phase_a_pairs(
|
||||
[pair],
|
||||
imu_by_session={"s0": imu},
|
||||
bias0_by_session={"s0": bias0},
|
||||
)
|
||||
|
||||
assert "J_bg" in enriched[0].metadata
|
||||
assert "cov" in enriched[0].metadata
|
||||
assert report["max_R_A_error_deg"] < 1e-8
|
||||
|
||||
|
||||
def test_phase_a_time_blocks_do_not_count_overlapping_pairs_as_independent() -> None:
|
||||
r_true = so3_exp(np.deg2rad(np.array([1.0, -2.0, 20.0])))
|
||||
bias = np.zeros(3)
|
||||
pairs = [
|
||||
_phase_a_pair("s0", index, r_true, (5.0 + index, 2.0, 1.0), bias)
|
||||
for index in range(9)
|
||||
]
|
||||
selected = select_decorrelated_phase_a_pairs(
|
||||
pairs,
|
||||
block_s=3.0,
|
||||
max_pairs_per_block=1,
|
||||
)
|
||||
assert len(selected) == 3
|
||||
assert all(pair in pairs for pair in selected)
|
||||
|
||||
|
||||
def test_phase_a_planar_motion_is_partial_and_keeps_weak_direction_from_prior() -> None:
|
||||
r_true = so3_exp(np.deg2rad(np.array([4.0, -3.0, 31.0])))
|
||||
prior = so3_exp(np.deg2rad(np.array([0.2, -0.1, 0.4]))) @ r_true
|
||||
biases = {"s0": np.zeros(3), "s1": np.zeros(3)}
|
||||
pairs: list[MotionPair] = []
|
||||
for session_index, sid in enumerate(biases):
|
||||
for index in range(12):
|
||||
pairs.append(
|
||||
_phase_a_pair(
|
||||
sid,
|
||||
session_index * 100 + index,
|
||||
r_true,
|
||||
(0.0, 0.0, 8.0 + index),
|
||||
biases[sid],
|
||||
)
|
||||
)
|
||||
result = solve_phase_a_comparison(
|
||||
pairs,
|
||||
gyro_bias_rad_s_by_session=biases,
|
||||
rotation_prior=prior,
|
||||
decorrelation_block_s=0.0,
|
||||
yaw_std_max_deg=0.5,
|
||||
run_leave_one_out=False,
|
||||
max_nfev=80,
|
||||
)
|
||||
assert not result.accepted
|
||||
assert result.partial_accepted
|
||||
assert result.solution_status == "phase_a_partial_accepted"
|
||||
assert result.marginal_observability.precision_rank == 2
|
||||
assert result.observable_subspace_with_prior is not None
|
||||
assert np.isinf(result.marginal_observability.direction_std_deg[0])
|
||||
@@ -0,0 +1,260 @@
|
||||
"""Regression tests for calibration quality, continuity, and observability gates."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from imu_lidar.contracts import ImuSeries, LidarFrame, MotionPair
|
||||
from imu_lidar.geometry import make_transform, so3_exp, so3_log
|
||||
from imu_lidar.joint_optimizer import solve_joint_extrinsic
|
||||
from imu_lidar.motion_pairs import build_motion_pairs
|
||||
from imu_lidar.observability import analyze_observability
|
||||
from imu_lidar.registration import RegistrationResult
|
||||
from imu_lidar.rotation_handeye import solve_rotation_handeye
|
||||
|
||||
|
||||
def _motion_pair(index: int, rotation_vector: np.ndarray) -> MotionPair:
|
||||
rotation = so3_exp(np.asarray(rotation_vector, dtype=float))
|
||||
return MotionPair(
|
||||
session_id="synthetic",
|
||||
i=index,
|
||||
j=index + 1,
|
||||
t_i_s=float(index),
|
||||
t_j_s=float(index + 1),
|
||||
R_A=rotation,
|
||||
R_B=rotation,
|
||||
t_A_m=np.zeros(3),
|
||||
t_B_m=np.array([0.1, -0.03, 0.0]),
|
||||
fitness=0.9,
|
||||
metadata={
|
||||
"J_bg": (-np.eye(3)).tolist(),
|
||||
"cov": (np.eye(3) * 1e-4).tolist(),
|
||||
"gyro_bias0_rad_s": [0.0, 0.0, 0.0],
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
def _frame(frame_id: str, mid_s: float) -> LidarFrame:
|
||||
return LidarFrame(
|
||||
frame_id=frame_id,
|
||||
t_start_s=mid_s - 0.01,
|
||||
t_end_s=mid_s + 0.01,
|
||||
points_xyz=np.zeros((64, 3)),
|
||||
)
|
||||
|
||||
|
||||
def _registration(*, fitness: float = 0.9) -> RegistrationResult:
|
||||
rotation = so3_exp(np.deg2rad(np.array([0.0, 0.0, 10.0])))
|
||||
return RegistrationResult(
|
||||
transform=make_transform(np.array([0.4, 0.0, 0.0]), rotation),
|
||||
fitness=fitness,
|
||||
rotation_deg=10.0,
|
||||
translation_m=0.4,
|
||||
backend="test",
|
||||
ok=True,
|
||||
)
|
||||
|
||||
|
||||
def test_planar_yaw_is_not_full_rotation_or_translation_observable():
|
||||
pairs = [
|
||||
_motion_pair(i, np.deg2rad(np.array([0.0, 0.0, angle_deg])))
|
||||
for i, angle_deg in enumerate((5.0, 8.0, 12.0, 17.0, 23.0, 31.0))
|
||||
]
|
||||
|
||||
report = analyze_observability(pairs, np.eye(3))
|
||||
|
||||
assert not report.rotation_observable
|
||||
assert not report.translation_observable
|
||||
|
||||
|
||||
def test_multi_axis_motion_is_rotation_and_translation_observable():
|
||||
vectors_deg = (
|
||||
(12.0, 0.0, 0.0),
|
||||
(0.0, 15.0, 0.0),
|
||||
(0.0, 0.0, 18.0),
|
||||
(10.0, 8.0, 0.0),
|
||||
(0.0, 11.0, 9.0),
|
||||
(7.0, 0.0, 13.0),
|
||||
)
|
||||
pairs = [
|
||||
_motion_pair(i, np.deg2rad(np.asarray(vector_deg)))
|
||||
for i, vector_deg in enumerate(vectors_deg)
|
||||
]
|
||||
|
||||
report = analyze_observability(pairs, np.eye(3))
|
||||
|
||||
assert report.rotation_observable
|
||||
assert report.translation_observable
|
||||
|
||||
|
||||
def test_translation_prior_is_reported_but_not_accepted_when_unobservable():
|
||||
pairs = [
|
||||
_motion_pair(i, np.deg2rad(np.array([0.0, 0.0, angle_deg])))
|
||||
for i, angle_deg in enumerate((5.0, 8.0, 12.0, 17.0, 23.0, 31.0))
|
||||
]
|
||||
prior = np.array([0.3, -0.2, 0.5])
|
||||
|
||||
result = solve_joint_extrinsic(
|
||||
pairs,
|
||||
np.eye(3),
|
||||
force_rotation_only=False,
|
||||
enable_phase_c=False,
|
||||
t_prior_m=prior,
|
||||
)
|
||||
|
||||
assert not result.translation_accepted
|
||||
np.testing.assert_allclose(result.T_IMU_lidar[:3, 3], prior)
|
||||
assert any("prior only" in note for note in result.notes)
|
||||
|
||||
|
||||
def test_handeye_rejects_a_small_fraction_of_gross_rotation_outliers():
|
||||
rng = np.random.default_rng(7)
|
||||
r_true = so3_exp(np.deg2rad(np.array([2.0, -3.0, 20.0])))
|
||||
pairs: list[MotionPair] = []
|
||||
for index in range(100):
|
||||
axis = rng.normal(size=3)
|
||||
axis /= np.linalg.norm(axis)
|
||||
r_b = so3_exp(axis * np.deg2rad(rng.uniform(8.0, 30.0)))
|
||||
r_a = r_true @ r_b @ r_true.T
|
||||
if index == 0:
|
||||
r_a = so3_exp(np.deg2rad(np.array([18.0, 0.0, 0.0]))) @ r_a
|
||||
pairs.append(
|
||||
MotionPair(
|
||||
session_id="outlier",
|
||||
i=index,
|
||||
j=index + 1,
|
||||
t_i_s=float(index),
|
||||
t_j_s=float(index + 1),
|
||||
R_A=r_a,
|
||||
R_B=r_b,
|
||||
)
|
||||
)
|
||||
|
||||
result = solve_rotation_handeye(pairs)
|
||||
|
||||
assert not result.ok
|
||||
assert result.outlier_fraction_gt_5deg > 0.005
|
||||
|
||||
|
||||
def test_motion_pairs_reject_low_fitness(monkeypatch):
|
||||
monkeypatch.setattr(
|
||||
"imu_lidar.motion_pairs.register_lidar_pair",
|
||||
lambda *_args, **_kwargs: _registration(fitness=0.3),
|
||||
)
|
||||
imu = ImuSeries(
|
||||
t_s=np.linspace(0.0, 1.2, 121),
|
||||
gyro_rad_s=np.zeros((121, 3)),
|
||||
acc_m_s2=np.zeros((121, 3)),
|
||||
)
|
||||
|
||||
result = build_motion_pairs(
|
||||
session_id="fitness",
|
||||
keyframes=[_frame("0", 0.1), _frame("1", 1.1)],
|
||||
keyframe_indices=[0, 1],
|
||||
imu=imu,
|
||||
delta_t_s=0.0,
|
||||
min_registration_fitness=0.5,
|
||||
)
|
||||
|
||||
assert not result.pairs
|
||||
assert any("fitness<0.50: 1" in note for note in result.notes)
|
||||
|
||||
|
||||
def test_motion_pairs_reject_imu_and_lidar_discontinuities(monkeypatch):
|
||||
monkeypatch.setattr(
|
||||
"imu_lidar.motion_pairs.register_lidar_pair",
|
||||
lambda *_args, **_kwargs: _registration(),
|
||||
)
|
||||
imu_with_gap = ImuSeries(
|
||||
t_s=np.array([0.0, 0.1, 0.2, 0.3, 0.4, 0.8, 0.9, 1.0, 1.1, 1.2]),
|
||||
gyro_rad_s=np.zeros((10, 3)),
|
||||
acc_m_s2=np.zeros((10, 3)),
|
||||
)
|
||||
imu_result = build_motion_pairs(
|
||||
session_id="imu-gap",
|
||||
keyframes=[_frame("0", 0.1), _frame("1", 1.1)],
|
||||
keyframe_indices=[0, 1],
|
||||
imu=imu_with_gap,
|
||||
delta_t_s=0.0,
|
||||
max_imu_gap_s=0.2,
|
||||
)
|
||||
|
||||
assert not imu_result.pairs
|
||||
assert any("IMU gap>0.200s: 1" in note for note in imu_result.notes)
|
||||
|
||||
continuous_imu = ImuSeries(
|
||||
t_s=np.linspace(0.0, 2.2, 221),
|
||||
gyro_rad_s=np.zeros((221, 3)),
|
||||
acc_m_s2=np.zeros((221, 3)),
|
||||
)
|
||||
lidar_result = build_motion_pairs(
|
||||
session_id="lidar-gap",
|
||||
keyframes=[_frame("0", 0.1), _frame("2", 2.1)],
|
||||
keyframe_indices=[0, 2],
|
||||
imu=continuous_imu,
|
||||
delta_t_s=0.0,
|
||||
all_frame_times_s=np.array([0.1, 0.2, 2.1]),
|
||||
max_lidar_gap_s=0.5,
|
||||
)
|
||||
|
||||
assert not lidar_result.pairs
|
||||
assert any("LiDAR gap>0.500s: 1" in note for note in lidar_result.notes)
|
||||
|
||||
def test_phase_a_keeps_session_bias_linearization_points_independent():
|
||||
r_true = so3_exp(np.deg2rad(np.array([2.0, -3.0, 20.0])))
|
||||
bias0_by_session = {
|
||||
"s0": np.array([0.010, -0.004, 0.002]),
|
||||
"s1": np.array([-0.006, 0.008, -0.003]),
|
||||
}
|
||||
vectors_deg = (
|
||||
(12.0, 0.0, 0.0),
|
||||
(0.0, 15.0, 0.0),
|
||||
(0.0, 0.0, 18.0),
|
||||
(10.0, 8.0, 0.0),
|
||||
(0.0, 11.0, 9.0),
|
||||
(7.0, 0.0, 13.0),
|
||||
)
|
||||
pairs: list[MotionPair] = []
|
||||
for session_index, (session_id, bias0) in enumerate(bias0_by_session.items()):
|
||||
for pair_index, vector_deg in enumerate(vectors_deg):
|
||||
r_b = so3_exp(np.deg2rad(np.asarray(vector_deg)))
|
||||
r_a = r_true @ r_b @ r_true.T
|
||||
index = session_index * 100 + pair_index
|
||||
pairs.append(
|
||||
MotionPair(
|
||||
session_id=session_id,
|
||||
i=index,
|
||||
j=index + 1,
|
||||
t_i_s=float(pair_index),
|
||||
t_j_s=float(pair_index + 1),
|
||||
R_A=r_a,
|
||||
R_B=r_b,
|
||||
t_A_m=np.zeros(3),
|
||||
t_B_m=np.zeros(3),
|
||||
metadata={
|
||||
"J_bg": np.eye(3).tolist(),
|
||||
"cov": (np.eye(3) * 1e-4).tolist(),
|
||||
"gyro_bias0_rad_s": bias0.tolist(),
|
||||
},
|
||||
)
|
||||
)
|
||||
|
||||
result = solve_joint_extrinsic(
|
||||
pairs,
|
||||
r_true,
|
||||
force_rotation_only=True,
|
||||
gyro_bias_rad_s_by_session=bias0_by_session,
|
||||
)
|
||||
|
||||
assert result.phase_a_accepted
|
||||
assert set(result.phase_a_comparison["variants"]) == {
|
||||
"A0_fixed_bg_data_only",
|
||||
"A1_session_bg_data_only",
|
||||
"A2_session_bg_with_rotation_prior",
|
||||
}
|
||||
assert set(result.gyro_bias_rad_s_per_session) == {"s0", "s1"}
|
||||
for session_id, bias0 in bias0_by_session.items():
|
||||
np.testing.assert_allclose(
|
||||
result.gyro_bias_rad_s_per_session[session_id], bias0, atol=1e-8
|
||||
)
|
||||
assert np.linalg.norm(so3_log(r_true.T @ result.T_IMU_lidar[:3, :3])) < 1e-8
|
||||
+29
-14
@@ -54,7 +54,7 @@ def _log(rotation: np.ndarray) -> np.ndarray:
|
||||
return so3_log(rotation)
|
||||
|
||||
|
||||
def test_synthetic_pipeline_rotation_and_time_offset(tmp_path: Path):
|
||||
def test_synthetic_pipeline_rejects_noisy_icp_but_keeps_time_audit(tmp_path: Path):
|
||||
meta = generate_synthetic_session(tmp_path, delta_t_s=0.17, yaw_extrinsic_deg=25.0)
|
||||
config = Path(__file__).resolve().parents[1] / "config" / "vehicle_installation.template.yaml"
|
||||
out = tmp_path / "out"
|
||||
@@ -74,17 +74,28 @@ def test_synthetic_pipeline_rotation_and_time_offset(tmp_path: Path):
|
||||
min_pair_rotation_deg=2.0,
|
||||
min_pair_translation_m=0.05,
|
||||
)
|
||||
result = run_calibration(request)
|
||||
assert result.status.value == "rotation_only_accepted"
|
||||
assert result.time_offset_s is not None
|
||||
assert abs(result.time_offset_s - meta["delta_t_s"]) < 0.05
|
||||
assert result.T_IMU_lidar is not None
|
||||
# End-to-end uses approximate ICP; allow moderate absolute error but require consistency.
|
||||
r_true = so3_exp(np.deg2rad(np.array([2.0, -1.5, meta["yaw_extrinsic_deg"]])))
|
||||
err_deg = np.degrees(np.linalg.norm(_log(r_true.T @ result.T_IMU_lidar[:3, :3])))
|
||||
assert err_deg < 15.0
|
||||
progress_events: list[dict] = []
|
||||
result = run_calibration(request, progress_callback=progress_events.append)
|
||||
# The lightweight synthetic point cloud uses approximate ICP and has a
|
||||
# roughly 3-degree P95 residual. The production gate must reject it rather
|
||||
# than expose a plausible-looking extrinsic.
|
||||
assert result.status.value == "blocked"
|
||||
assert result.T_IMU_lidar is None
|
||||
session0 = result.details["sessions"][0]
|
||||
assert session0["handeye"]["residual_rms_deg"] < 5.0
|
||||
assert abs(session0["time_offset_s"] - meta["delta_t_s"]) < 0.05
|
||||
assert result.details["joint_handeye"]["residual_p95_deg"] > 1.5
|
||||
assert not result.details["joint_handeye"]["ok"]
|
||||
assert progress_events[0]["event"] == "pipeline_start"
|
||||
assert any(
|
||||
event["stage"] == "motion_pairs" and event["event"] == "complete"
|
||||
for event in progress_events
|
||||
)
|
||||
assert any(
|
||||
event["stage"] == "joint_optimizer" and event["event"] == "phase_a_complete"
|
||||
for event in progress_events
|
||||
)
|
||||
assert progress_events[-1]["stage"] == "finalize"
|
||||
assert progress_events[-1]["event"] == "complete"
|
||||
|
||||
|
||||
def test_time_offset_on_synthetic(tmp_path: Path):
|
||||
@@ -176,13 +187,17 @@ def test_synthetic_pipeline_full_se3_smoke(tmp_path: Path):
|
||||
"full_se3_accepted",
|
||||
"full_se3_rejected_due_to_observability",
|
||||
"rotation_only_accepted",
|
||||
"blocked",
|
||||
}
|
||||
assert result.T_IMU_lidar is not None
|
||||
session0 = result.details["sessions"][0]
|
||||
assert "delta_v" in session0.get("pair_notes", []) or session0.get("pair_count", 0) >= 0
|
||||
# Phase-C fields appear when joint ran successfully on pairs.
|
||||
if session0.get("ok"):
|
||||
# Phase-C fields appear only when the strict rotation gate passed.
|
||||
if result.status.value != "blocked":
|
||||
assert result.T_IMU_lidar is not None
|
||||
assert "gyro_bias_rad_s" in session0["joint"]
|
||||
else:
|
||||
assert result.T_IMU_lidar is None
|
||||
assert not result.details["joint_handeye"]["ok"]
|
||||
|
||||
|
||||
def test_signed_time_offset_refine_improves_or_keeps(tmp_path: Path):
|
||||
|
||||
Reference in New Issue
Block a user