FazBrowse GitHub Viewer | Trending |
URL:
| Home
Tools: [Download Repo ZIP]   [Original HTTPS Page]

perf(base): speed up trnorm and tr2adjoint by petercorke · Pull Request #214 · rai-opensource/spatialmath-python · GitHub

/ spatialmath-python Public

perf(base): speed up trnorm and tr2adjoint #214

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
petercorke wants to merge 1 commit into
base: master
Choose a base branch
Could not load branches
Branch not found: {{ refName }}

Are you sure you want to change the base?

Some commits from the old base branch may be removed from the timeline, and old review comments may become outdated.
from
Conversation 1 Commits 1 Checks 13 Files changed
Open

perf(base): speed up trnorm and tr2adjoint #214

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension .py  (1) All 1 file type selected
Viewed files
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Unified
Split
Hide whitespace
Diff view
Unified
Split
Hide whitespace
52 changes: 36 additions & 16 deletions spatialmath/base/transforms3d.py
Show comments
View file
Open in desktop
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters. Learn more about bidirectional Unicode characters
Show hidden characters
Original file line number Diff line number Diff line change
Expand Up @@ -1591,17 +1591,36 @@ def trnorm(T: SE3Array) -> SE3Array:
if not ishom(T) and not isrot(T):
raise ValueError("expecting SO(3) or SE(3)")

o = T[:3, 1]
a = T[:3, 2]
# explicit scalar arithmetic avoids the generic-dispatch overhead of
# np.cross/unitvec/np.stack (built for arbitrary shapes/broadcasting),
# which dominates cost for a fixed 3x3 rotation submatrix
o0, o1, o2 = T[0, 1], T[1, 1], T[2, 1]
a0, a1, a2 = T[0, 2], T[1, 2], T[2, 2]

n = np.cross(o, a) # N = O x A
o = np.cross(a, n) # (a)];
R = np.stack((unitvec(n), unitvec(o), unitvec(a)), axis=1)
# n = o x a
n0 = o1 * a2 - o2 * a1
n1 = o2 * a0 - o0 * a2
n2 = o0 * a1 - o1 * a0

if ishom(T):
return rt2tr(cast(SO3Array, R), T[:3, 3])
else:
return R
# o = a x n, to re-orthogonalize
o0, o1, o2 = a1 * n2 - a2 * n1, a2 * n0 - a0 * n2, a0 * n1 - a1 * n0

n_norm = (n0 * n0 + n1 * n1 + n2 * n2) ** 0.5
o_norm = (o0 * o0 + o1 * o1 + o2 * o2) ** 0.5
a_norm = (a0 * a0 + a1 * a1 + a2 * a2) ** 0.5

is_hom = ishom(T)
R = np.empty((4, 4) if is_hom else (3, 3))
R[0, 0], R[1, 0], R[2, 0] = n0 / n_norm, n1 / n_norm, n2 / n_norm
R[0, 1], R[1, 1], R[2, 1] = o0 / o_norm, o1 / o_norm, o2 / o_norm
R[0, 2], R[1, 2], R[2, 2] = a0 / a_norm, a1 / a_norm, a2 / a_norm

if is_hom:
R[3, :3] = 0.0
R[3, 3] = 1.0
R[:3, 3] = T[:3, 3]

return cast(SE3Array, R)


@overload
Expand Down Expand Up @@ -2718,20 +2737,21 @@ def tr2adjoint(T):
:SymPy: supported
"""

Z = np.zeros((3, 3), dtype=T.dtype)
if T.shape == (3, 3):
# SO(3) adjoint
R = T
return R
elif T.shape == (4, 4):
# SE(3) adjoint
(R, t) = tr2rt(T)
# fmt: off
return np.block([
[R, skew(t) @ R],
[Z, R]
])
# fmt: on
# direct block assignment avoids np.block's generic nested-list
# assembly overhead, which dominates cost for this fixed 6x6-from-
# four-3x3 layout; dtype preserved for SymPy support
A = np.zeros((6, 6), dtype=T.dtype)
A[:3, :3] = R
A[:3, 3:] = skew(t) @ R
A[3:, 3:] = R
return A
else:
raise ValueError("bad argument")

Expand Down
Loading
Back | FazBrowse Home | New Git URL