支持主机桥接后固定δt与旋转先验，并落盘运动对供可视化直读。

Co-authored-by: Cursor <cursoragent@cursor.com>

imu_lidar/rotation_handeye.py

@@ -57,8 +57,24 @@ def _pair_residual_deg(r_x: np.ndarray, pair: MotionPair) -> float:
     return float(np.degrees(np.linalg.norm(err)))
 
 
-def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) -> RotationHandeyeResult:
-    """Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement."""
+def _rms_deg(r_x: np.ndarray, pairs: list[MotionPair]) -> float:
+    if not pairs:
+        return 1e9
+    errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in pairs], dtype=float)
+    return float(np.sqrt(np.mean(errs**2)))
+
+
+def solve_rotation_handeye(
+    pairs: list[MotionPair] | tuple[MotionPair, ...],
+    *,
+    R_prior: np.ndarray | None = None,
+    prior_sigma_deg: float | None = None,
+) -> RotationHandeyeResult:
+    """Solve ``R_A R_X = R_X R_B`` with weighted robust nonlinear refinement.
+
+    Optional CAD / installation ``R_prior`` soft-constrains the extrinsic yaw that
+    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]
     notes: list[str] = []
@@ -73,6 +89,21 @@ def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) ->
         )
 
     r0 = _tsai_rotation_initial(usable)
+    r_prior = None
+    if R_prior is not None:
+        r_prior = orthonormalize_rotation(np.asarray(R_prior, dtype=float).reshape(3, 3))
+        rms_tsai = _rms_deg(r0, usable)
+        rms_prior = _rms_deg(r_prior, usable)
+        if rms_prior <= rms_tsai * 1.25:
+            r0 = r_prior
+            notes.append(
+                f"init from rotation prior (rms={rms_prior:.3f} deg vs Tsai {rms_tsai:.3f} deg)"
+            )
+        else:
+            notes.append(
+                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}, "
@@ -85,12 +116,21 @@ def solve_rotation_handeye(pairs: list[MotionPair] | tuple[MotionPair, ...]) ->
     def unpack(vec: np.ndarray) -> np.ndarray:
         return orthonormalize_rotation(so3_exp(vec))
 
+    sigma = 15.0 if prior_sigma_deg is None else float(prior_sigma_deg)
+    prior_w = 0.0
+    if r_prior is not None and sigma > 1e-6:
+        # Scale prior to a few strong pairs so it regularizes yaw without dominating.
+        prior_w = float(np.sqrt(np.median(weights)) / np.deg2rad(sigma))
+        notes.append(f"rotation prior soft constraint sigma={sigma:.1f} deg, weight={prior_w:.3g}")
+
     def residual(vec: np.ndarray) -> np.ndarray:
         r_x = unpack(vec)
         residuals = []
         for pair, weight in zip(usable, weights):
             err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
             residuals.append(np.sqrt(weight) * err)
+        if r_prior is not None and prior_w > 0:
+            residuals.append(prior_w * so3_log(r_prior.T @ r_x))
         return np.concatenate(residuals)
 
     opt = least_squares(residual, pack(r0), loss="huber", f_scale=np.deg2rad(1.0), max_nfev=200)