From 764e7f5be876848a5da977b3e13b7fe19164622d Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 21 Aug 2026 11:57:09 +1000 Subject: [PATCH] perf(base): speed up isskewa and cache identity matrix in trexp/trlog/rodrigues trexp/trlog don't have one dominant wasteful generic call like the isR/trnorm/tr2adjoint/qqmul/qvmul fixes did (#213/#214/#215) - 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. Two targeted, verified wins pulled out of that chain: - isskewa (validity check trexp runs on se(3) input) had the same bug ishom had pre-#213: np.linalg.norm on a small fixed matrix, plus an all(S[-1,:] == 0) array-allocation-and-compare for the bottom row. Fixed the same way, ~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, used only at call sites where it's provably never mutated or returned directly (an aliasing hazard if it were) - the four zero-motion/zero-rotation early-return `np.eye(N)` calls in rodrigues/trexp are untouched, left as fresh arrays, since they hand the object directly to the caller. 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 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. Reverted to the original implementation rather than keep an unproven "fix". Net effect, measured old-vs-new in the same process (min of 9 repeats each, to suppress system jitter after an earlier round of misleading single-run numbers): isskewa ~1.9x, rodrigues ~1.13x, trexp ~1.05x, trlog ~1.10x. Real but modest - see the PR description's "further speedup opportunities" section for what a bigger win here would actually require. Co-Authored-By: Claude Sonnet 5 --- spatialmath/base/transforms3d.py | 14 ++++++---- spatialmath/base/transformsNd.py | 44 +++++++++++++++++++++++++++++--- 2 files changed, 50 insertions(+), 8 deletions(-) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index 1bc90600..f020364e 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -52,6 +52,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) + # ---------------------------------------------------------------------------------------# @@ -1355,7 +1360,7 @@ def trlog( else: # general case Ginv = ( - np.eye(3) + _EYE3 - S / 2 + (1 / theta - 1 / math.tan(theta / 2) / 2) / theta * S @ S ) @@ -1374,8 +1379,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: @@ -1514,7 +1518,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 ) @@ -2774,7 +2778,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 ) diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index 611c89a3..3fc9b036 100644 --- a/spatialmath/base/transformsNd.py +++ b/spatialmath/base/transformsNd.py @@ -408,6 +408,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) @@ -436,9 +441,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: