perf(base): speed up isskewa, cache identity in trexp/trlog/rodrigues (#216)

## Summary

`trexp`/`trlog` are much slower than `rodrigues` (~20us and ~17us vs
~7us), but unlike the previous three perf PRs (#213, #214, #215) they
don't have one dominant wasteful generic-NumPy call to fix - they're
deep call chains (`trexp` -> `isskewa` -> `vexa` -> `iszerovec` ->
`unittwist_norm` -> `rodrigues` -> `skew` -> `rt2tr` -> `ishom` ->
`isR`) where cost is spread thin across many small layers, each
independently re-validating/re-extracting. This PR pulls out the two
safe, verified wins found in that chain; it does **not** close the full
gap to `rodrigues` - see "further speedup opportunities" below.

- **`isskewa`** (the validity check `trexp` runs on its se(3) input) had
the exact same bug `ishom` had before #213: `np.linalg.norm()` on a
small fixed-size matrix, plus an `all(S[-1,:] == 0)`
array-allocation-and-compare for checking the bottom row is zero. Fixed
the same way (explicit sum-of-squares residual + scalar comparisons).
~1.9x faster standalone.
- **`np.eye(3)`** was rebuilt from scratch on every call in `rodrigues`
(1x), `trexp`'s `V`-matrix construction (1x), and `trlog` (2x) - despite
only ever being used as a read-only operand in an
addition/multiplication that produces a new array. Replaced with a
module-level constant `_EYE3` at those call sites only. **Not** used at
the four zero-motion/zero-rotation early-return `np.eye(N)` calls in
`rodrigues`/`trexp` (e.g. `if iszerovec(w): return np.eye(3)`) - those
hand the array object directly to the caller, and aliasing them to a
shared constant would risk a real bug if a caller ever mutated the
returned matrix in place. Left as fresh arrays.
- **`isskew`** (the plain, non-augmented so(n) check) was also tried
with the same explicit-arithmetic treatment as `isskewa`, but did
**not** show a reliable win under careful (min-of-repeats) benchmarking
- the only cost it avoids is `np.linalg.norm` on an already-cheap `S +
S.T`, not enough margin to reliably beat by hand-unrolling the indexing.
Reverted to the original implementation rather than keep an unproven
"fix" - same judgment call as an earlier fully-scalar `isR` variant that
was tried and discarded for the same reason.

Net effect, measured old-vs-new in the same process, min of 9 repeats
each (single-run `timeit` numbers were misleading here - see commit
message): **isskewa ~1.9x, rodrigues ~1.13x, trexp ~1.05x, trlog
~1.10x**. Real, but modest compared to #213-#215.

## Further speedup opportunities (not attempted here)

A bigger win on `trexp`/`trlog` would mean collapsing the redundant
`getvector`/re-validation layers in the hot path into a more direct,
fused implementation - e.g. `trexp(se3_matrix)` currently re-extracts
and re-validates the same data through `isskewa` -> `vexa` ->
`iszerovec` -> `unittwist_norm` independently, each with its own
`getvector` call. That's a larger, riskier change: this code has
clearly-deliberate handling for numerically delicate edge cases
(near-zero rotation, near-pi singularity, caller-supplied `theta`), so a
fused rewrite needs much more careful edge-case test coverage than the
drop-in fixes here. Not scoped or attempted in this PR.

Two smaller, unrelated candidates surfaced but out of scope for this PR:
- `np.linalg.norm` is used directly (instead of the project's own
`norm()`, which is faster for small vectors per its own docstring) in
several other places - `iszerovec`, `isunitvec`, `qnorm`/`qunit` in
`quaternions.py`, and a few spots in
`geom2d.py`/`geom3d.py`/`pose3d.py`. Worth noting: `iszerovec` was
checked directly as part of this investigation, and swapping it to the
project's `norm()` did **not** show a reliable win either (consistent
with the project's own timing script, which shows `base.norm(R6)`
measuring *slower* than `np.linalg.norm(R6)` for a 6-vector) - so this
isn't a guaranteed win either, just an inconsistency worth someone
measuring properly before touching.
- `np.eye(N)` is rebuilt from scratch in ~20 other places across the
codebase (`pose2d.py`, `pose3d.py`, `transforms2d.py`,
`transformsNd.py`, and other unrelated functions in `transforms3d.py`).
None are in the `trexp`/`trlog`/`rodrigues` path touched here; a broader
sweep would need the same operand-vs-return-value aliasing care taken in
this PR, function by function.

## Test plan
- [x] `pytest tests/` - 343 passed
- [x] `isskewa` verified against the prior implementation over 3000
randomized trials (skew/non-skew, near-boundary perturbations)
- [x] `rodrigues`, `trexp`, `trlog` (both matrix and twist-vector return
forms) verified bit-identical (max diff 0.0) against reference copies of
the prior implementations over 1000-2000 randomized trials each

🤖 Generated with [Claude Code](https://claude.com/claude-code)

Co-authored-by: Claude Sonnet 5 <noreply@anthropic.com>

Author: petercorke

spatialmath/base/transforms3d.py

@@ -61,6 +61,11 @@
 
 _eps = np.finfo(np.float64).eps
 
+# read-only constant: safe to use directly as an operand in expressions that
+# produce a new array (e.g. `_EYE3 + ...`), never in a context that could
+# mutate it in place or return it directly to a caller
+_EYE3 = np.eye(3)
+
 # ---------------------------------------------------------------------------------------#
 
 
@@ -1370,7 +1375,7 @@ def trlog(
         else:
             # general case
             Ginv = (
-                np.eye(3)
+                _EYE3
                 - S / 2
                 + (1 / theta - 1 / math.tan(theta / 2) / 2) / theta * S @ S
             )
@@ -1389,8 +1394,7 @@ def trlog(
             diagonal = R.diagonal()
             k = diagonal.argmax()
             mx = diagonal[k]
-            I = np.eye(3)
-            col = R[:, k] + I[:, k]
+            col = R[:, k] + _EYE3[:, k]
             w = col / np.sqrt(2 * (1 + mx))
             theta = math.pi
             if twist:
@@ -1529,7 +1533,7 @@ def trexp(S, theta=None, check=True):
 
         skw = skew(w)
         V = (
-            np.eye(3) * theta
+            _EYE3 * theta
             + (1.0 - math.cos(theta)) * skw
             + (theta - math.sin(theta)) * skw @ skw
         )
@@ -2809,7 +2813,7 @@ def rodrigues(w: ArrayLike3, theta: Optional[float] = None) -> SO3Array:
 
     skw = skew(cast(ArrayLike3, w))
     return (
-        np.eye(skw.shape[0])
+        _EYE3
         + math.sin(theta) * skw
         + (1.0 - math.cos(theta)) * skw @ skw
     )

spatialmath/base/transformsNd.py

@@ -432,6 +432,11 @@ def isskew(S: NDArray, tol: float = 20) -> bool:  # -> TypeGuard[sonArray]:
 
     :seealso: isskewa
     """
+    # NB: unlike isR/isskewa, an explicit-arithmetic fast path here did not
+    # show a reliable win under careful (min-of-repeats) benchmarking - the
+    # only overhead being avoided is np.linalg.norm on an already-cheap
+    # `S + S.T`, not enough to reliably beat the scalar-indexing cost of
+    # unrolling it by hand. Left as the original implementation.
     return bool(np.linalg.norm(S + S.T) < tol * _eps)
 
 
@@ -460,9 +465,42 @@ def isskewa(S: NDArray, tol: float = 20) -> bool:  # -> TypeGuard[senArray]:
 
     :seealso: isskew
     """
-    return bool(np.linalg.norm(S[0:-1, 0:-1] + S[0:-1, 0:-1].T) < tol * _eps) and all(
-        S[-1, :] == 0
-    )
+    n = S.shape[0]
+    if n == 4:
+        # explicit sum-of-squares + scalar bottom-row check avoids the
+        # generic-dispatch overhead of np.linalg.norm and the array
+        # allocation + all() of the bottom-row comparison, same as isR/ishom
+        r00 = S[0, 0] + S[0, 0]
+        r01 = S[0, 1] + S[1, 0]
+        r02 = S[0, 2] + S[2, 0]
+        r11 = S[1, 1] + S[1, 1]
+        r12 = S[1, 2] + S[2, 1]
+        r22 = S[2, 2] + S[2, 2]
+        resid = r00 * r00 + r11 * r11 + r22 * r22 + 2.0 * (
+            r01 * r01 + r02 * r02 + r12 * r12
+        )
+        return bool(
+            resid < (tol * _eps) ** 2
+            and S[3, 0] == 0
+            and S[3, 1] == 0
+            and S[3, 2] == 0
+            and S[3, 3] == 0
+        )
+    elif n == 3:
+        r00 = S[0, 0] + S[0, 0]
+        r01 = S[0, 1] + S[1, 0]
+        r11 = S[1, 1] + S[1, 1]
+        resid = r00 * r00 + r11 * r11 + 2.0 * r01 * r01
+        return bool(
+            resid < (tol * _eps) ** 2
+            and S[2, 0] == 0
+            and S[2, 1] == 0
+            and S[2, 2] == 0
+        )
+    else:
+        return bool(
+            np.linalg.norm(S[0:-1, 0:-1] + S[0:-1, 0:-1].T) < tol * _eps
+        ) and all(S[-1, :] == 0)
 
 
 def iseye(S: NDArray, tol: float = 20) -> bool: