115 lines
3.8 KiB
Python
115 lines
3.8 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",))
|
|
|
|
singular = np.linalg.svd(j_r, compute_uv=False)
|
|
cond_r = float(singular[0] / max(singular[-1], 1e-12))
|
|
rotation_information = float(singular[-1] / np.sqrt(max(len(usable), 1)))
|
|
rotation_ok = (
|
|
cond_r < condition_threshold
|
|
and rotation_information > 1e-3
|
|
and singular[-1] > 1e-6
|
|
)
|
|
|
|
# Translation lever arm is observable through stacked (R_A - I). Pure
|
|
# planar yaw leaves its vertical column in the nullspace and must fail.
|
|
translation_rows = [
|
|
np.asarray(pair.R_A, dtype=float).reshape(3, 3) - np.eye(3)
|
|
for pair in usable
|
|
if pair.t_B_m is not None
|
|
]
|
|
if translation_rows:
|
|
j_t = np.vstack(translation_rows)
|
|
singular_t = np.linalg.svd(j_t, compute_uv=False)
|
|
cond_t = float(singular_t[0] / max(singular_t[-1], 1e-12))
|
|
translation_information = float(
|
|
singular_t[-1] / np.sqrt(max(len(translation_rows), 1))
|
|
)
|
|
else:
|
|
cond_t = 1e9
|
|
translation_information = 0.0
|
|
translation_ok = (
|
|
len(translation_rows) >= 5
|
|
and cond_t < condition_threshold
|
|
and translation_information > 0.02
|
|
)
|
|
|
|
if not rotation_ok:
|
|
notes.append(
|
|
f"rotation not observable: condition={cond_r:.1f}, "
|
|
f"min_information={rotation_information:.3e}"
|
|
)
|
|
else:
|
|
notes.append(
|
|
f"rotation observable: condition={cond_r:.1f}, "
|
|
f"min_information={rotation_information:.3e}"
|
|
)
|
|
if not translation_ok:
|
|
notes.append(
|
|
f"translation not observable: condition={cond_t:.1f}, "
|
|
f"min_information={translation_information:.3e}; "
|
|
"full SE3 will be rejected"
|
|
)
|
|
return ObservabilityReport(
|
|
rotation_observable=rotation_ok,
|
|
translation_observable=translation_ok,
|
|
condition_rotation=cond_r,
|
|
condition_translation=cond_t,
|
|
notes=tuple(notes),
|
|
)
|