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

imu_lidar/rotation_handeye.py

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