8000 computation speedups · StephLin/spatialmath-python@e6ea17a · GitHub
[go: up one dir, main page]
Skip to content {"props":{"docsUrl":"https://docs.github.com/get-started/accessibility/keyboard-shortcuts"}}

Commit e6ea17a

Browse files
petercorkepetercorke
committed
computation speedups
math.* is faster than numpy.*
1 parent fa13dbe commit e6ea17a

File tree

3 files changed

+33
-6
lines changed

3 files changed

+33
-6
lines changed

‎spatialmath/base/transforms3d.py

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1078,7 +1078,7 @@ def trlog(T, check=True, twist=False):
10781078
S = trlog(R, check=False) # recurse
10791079
w = base.vex(S)
10801080
theta = base.norm(w)
1081-
Ginv = np.eye(3) - S / 2 + (1 / theta - 1 / np.tan(theta / 2) / 2) / theta * S @ S
1081+
Ginv = np.eye(3) - S / 2 + (1 / theta - 1 / math.tan(theta / 2) / 2) / theta * S @ S
10821082
v = Ginv @ t
10831083
if twist:
10841084
return np.r_[v, w]
@@ -1110,8 +1110,8 @@ def trlog(T, check=True, twist=False):
11101110
return base.skew(w * theta)
11111111
else:
11121112
# general case
1113-
theta = np.arccos((np.trace(R) - 1) / 2)
1114-
skw = (R - R.T) / 2 / np.sin(theta)
1113+
theta = math.acos((np.trace(R) - 1) / 2)
1114+
skw = (R - R.T) / 2 / math.sin(theta)
11151115
if twist:
11161116
return base.vex(skw * theta)
11171117
else:

‎spatialmath/base/transformsNd.py

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -617,13 +617,11 @@ def rodrigues(w, theta=None):
617617
else:
618618
return np.eye(3)
619619
if theta is None:
620-
theta = base.norm(w)
621-
w = base.unitvec(w)
620+
w, theta = base.unitvec_norm(w)
622621

623622
skw = skew(w)
624623
return np.eye(skw.shape[0]) + math.sin(theta) * skw + (1.0 - math.cos(theta)) * skw @ skw
625624

626-
627625
def h2e(v):
628626
"""
629627
Convert from homogeneous to Euclidean form

‎spatialmath/base/vectors.py

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,35 @@ def unitvec(v):
6868
return None
6969

7070

71+
def unitvec_norm(v):
72+
"""
73+
Create a unit vector
74+
75+
:param v: any vector
76+
:type v: array_like(n)
77+
:return: a unit-vector parallel to ``v`` and the norm
78+
:rtype: (ndarray(n), float)
79+
:raises ValueError: for zero length vector
80+
81+
``unitvec(v)`` is a vector parallel to `v` of unit length.
82+
83+
.. runblock:: pycon
84+
85+
>>> from spatialmath.base import *
86+
>>> unitvec([3, 4])
87+
88+
:seealso: :func:`~numpy.linalg.norm`
89+
90+
"""
91+
92+
v = getvector(v)
93+
n = np.linalg.norm(v)
94+
95+
if n > 100 * _eps: # if greater than eps
96+
return (v / n, n)
97+
else:
98+
return None
99+
71100
def norm(v):
72101
"""
73102
Norm of vector

0 commit comments

Comments
 (0)
0