218 lines
7.2 KiB
Python
218 lines
7.2 KiB
Python
"""SO(3) rotation hand-eye solver for ``R_A R_X = R_X R_B``."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
|
|
import numpy as np
|
|
from scipy.optimize import least_squares
|
|
|
|
from .contracts import MotionPair
|
|
from .geometry import orthonormalize_rotation, rotation_angle_deg, skew, so3_exp, so3_log
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
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, ...] = ()
|
|
|
|
|
|
def _pair_weight(pair: MotionPair) -> float:
|
|
weight = float(pair.metadata.get("weight", 1.0))
|
|
if not np.isfinite(weight) or weight <= 0:
|
|
return 1.0
|
|
return weight
|
|
|
|
|
|
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] = []
|
|
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(float(pair_weight))
|
|
rows.append(w * skew(alpha + beta))
|
|
rhs.append(w * (beta - alpha))
|
|
if len(rows) < 2:
|
|
return np.eye(3)
|
|
a = np.vstack(rows)
|
|
b = np.concatenate(rhs)
|
|
try:
|
|
rotvec, *_ = np.linalg.lstsq(a, b, rcond=None)
|
|
except np.linalg.LinAlgError:
|
|
return np.eye(3)
|
|
return orthonormalize_rotation(so3_exp(rotvec))
|
|
|
|
|
|
def _pair_residual_deg(r_x: np.ndarray, pair: MotionPair) -> float:
|
|
err = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
|
return float(np.degrees(np.linalg.norm(err)))
|
|
|
|
|
|
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 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, ...],
|
|
*,
|
|
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 = 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",),
|
|
)
|
|
|
|
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))
|
|
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)"
|
|
)
|
|
|
|
notes.append(
|
|
"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:
|
|
return so3_log(r)
|
|
|
|
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)
|
|
r_x = unpack(opt.x)
|
|
errs = np.asarray([_pair_residual_deg(r_x, pair) for pair in usable], dtype=float)
|
|
# 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")
|
|
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 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),
|
|
)
|