105 lines
3.6 KiB
Python
105 lines
3.6 KiB
Python
 
|
"""Normalized-Jacobian observability analysis for rotation / SE(3) gates."""
|
||
|
|
|
||
|
|
from __future__ import annotations
|
||
|
|
|
||
|
|
from dataclasses import dataclass
|
||
|
|
|
||
|
|
import numpy as np
|
||
|
|
|
||
|
|
from .contracts import MotionPair
|
||
|
|
from .geometry import skew, so3_log
|
||
|
|
|
||
|
|
|
||
|
|
@dataclass(frozen=True)
|
||
|
|
class ObservabilityReport:
|
||
|
|
rotation_observable: bool
|
||
|
|
translation_observable: bool
|
||
|
|
condition_rotation: float
|
||
|
|
condition_translation: float
|
||
|
|
notes: tuple[str, ...] = ()
|
||
|
|
|
||
|
|
|
||
|
|
def _rotation_jacobian(pairs: list[MotionPair], r_x: np.ndarray) -> np.ndarray:
|
||
|
|
rows = []
|
||
|
|
for pair in pairs:
|
||
|
|
# Residual r = log(R_x^T R_A R_x R_B^T); approximate J w.r.t. left perturbation of R_x.
|
||
|
|
# Use finite-difference columns for robustness in V1.
|
||
|
|
base = so3_log(r_x.T @ pair.R_A @ r_x @ pair.R_B.T)
|
||
|
|
cols = []
|
||
|
|
eps = 1e-5
|
||
|
|
for axis in range(3):
|
||
|
|
delta = np.zeros(3)
|
||
|
|
delta[axis] = eps
|
||
|
|
r_pert = r_x @ (np.eye(3) + skew(delta))
|
||
|
|
# Orthonormalize lightly
|
||
|
|
u, _, vt = np.linalg.svd(r_pert)
|
||
|
|
r_pert = u @ vt
|
||
|
|
pert = so3_log(r_pert.T @ pair.R_A @ r_pert @ pair.R_B.T)
|
||
|
|
cols.append((pert - base) / eps)
|
||
|
|
rows.append(np.column_stack(cols))
|
||
|
|
return np.vstack(rows) if rows else np.zeros((0, 3))
|
||
|
|
|
||
|
|
|
||
|
|
def analyze_observability(
|
||
|
|
pairs: list[MotionPair] | tuple[MotionPair, ...],
|
||
|
|
r_x: np.ndarray,
|
||
|
|
*,
|
||
|
|
condition_threshold: float = 100.0,
|
||
|
|
) -> ObservabilityReport:
|
||
|
|
"""Gate whether rotation-only or full SE(3) should be accepted."""
|
||
|
|
|
||
|
|
usable = list(pairs)
|
||
|
|
notes: list[str] = []
|
||
|
|
if len(usable) < 3:
|
||
|
|
return ObservabilityReport(False, False, 1e9, 1e9, ("insufficient pairs",))
|
||
|
|
|
||
|
|
j_r = _rotation_jacobian(usable, np.asarray(r_x, dtype=float))
|
||
|
|
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)
|
||
|
|
cond_r = float(singular[0] / max(singular[-1], 1e-12))
|
||
|
|
rotation_ok = cond_r < condition_threshold and singular[-1] > 1e-3
|
||
|
|
|
||
|
|
# 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)))
|
||
|
|
|
||
|
|
if not rotation_ok:
|
||
|
|
notes.append(f"rotation condition {cond_r:.1f} exceeds threshold {condition_threshold}")
|
||
|
|
else:
|
||
|
|
notes.append(f"rotation condition {cond_r:.1f}")
|
||
|
|
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"
|
||
|
|
)
|
||
|
|
return ObservabilityReport(
|
||
|
|
rotation_observable=rotation_ok,
|
||
|
|
translation_observable=translation_ok,
|
||
|
|
condition_rotation=cond_r,
|
||
|
|
condition_translation=cond_t,
|
||
|
|
notes=tuple(notes),
|
||
|
|
)
|