完善Phase-A会话级联合优化并修正雷达相位中心高度先验
This commit is contained in:
lichun.qu
+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