8000 Replace r2q, using Cayley's method, 1.8x faster · krenshaw2018/spatialmath-python@29300b1 · GitHub
[go: up one dir, main page]
Skip to content

Commit 29300b1

Browse files
petercorkepetercorke
committed
Replace r2q, using Cayley's method, 1.8x faster
1 parent e2555b1 commit 29300b1

File tree

1 file changed

+106
-38
lines changed

1 file changed

+106
-38
lines changed

‎spatialmath/base/quaternions.py

Lines changed: 106 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -504,7 +504,7 @@ def r2q(R, check=False, tol=100):
504504
:type check: bool
505505
:param tol: tolerance in units of eps
506506
:type tol: float
507-
:return: unit-quaternion
507+
:return: unit-quaternion as Euler parameters
508508
:rtype: ndarray(4)
509509
:raises ValueError: for non SO(3) argument
510510
@@ -519,49 +519,117 @@ def r2q(R, check=False, tol=100):
519519
520520
.. warning:: There is no check that the passed matrix is a valid rotation matrix.
521521
522-
.. note:: Scalar part is always positive.
522+
.. note::
523+
- Scalar part is always positive
524+
- implements Cayley's method
525+
526+
:reference:
527+
- Sarabandi, S., and Thomas, F. (March 1, 2019).
528+
"A Survey on the Computation of Quaternions From Rotation Matrices."
529+
ASME. J. Mechanisms Robotics. April 2019; 11(2): 021006.
530+
`doi.org/10.1115/1.4041889 <https://doi.org/10.1115/1.4041889>`_
523531
524532
:seealso: :func:`q2r`
525533
"""
526534
if not base.isrot(R, check=check, tol=tol):
527535
raise ValueError("Argument must be a valid SO(3) matrix")
528536

529-
qs = math.sqrt(max(0, np.trace(R) + 1)) / 2.0 # scalar part
530-
kx = R[2, 1] - R[1, 2] # Oz - Ay
531-
ky = R[0, 2] - R[2, 0] # Ax - Nz
532-
kz = R[1, 0] - R[0, 1] # Ny - Ox
533-
534-
if (R[0, 0] >= R[1, 1]) and (R[0, 0] >= R[2, 2]):
535-
kx1 = R[0, 0] - R[1, 1] - R[2, 2] + 1 # Nx - Oy - Az + 1
536-
ky1 = R[1, 0] + R[0, 1] # Ny + Ox
537-
kz1 = R[2, 0] + R[0, 2] # Nz + Ax
538-
add = (kx >= 0)
539-
elif R[1, 1] >= R[2, 2]:
540-
kx1 = R[1, 0] + R[0, 1] # Ny + Ox
541-
ky1 = R[1, 1] - R[0, 0] - R[2, 2] + 1 # Oy - Nx - Az + 1
542-
kz1 = R[2, 1] + R[1, 2] # Oz + Ay
543-
add = (ky >= 0)
544-
else:
545-
kx1 = R[2, 0] + R[0, 2] # Nz + Ax
546-
ky1 = R[2, 1] + R[1, 2] # Oz + Ay
547-
kz1 = R[2, 2] - R[0, 0] - R[1, 1] + 1 # Az - Nx - Oy + 1
548-
add = (kz >= 0)
549-
550-
if add:
551-
kx = kx + kx1
552-
ky = ky + ky1
553-
kz = kz + kz1
554-
else:
555-
kx = kx - kx1
556-
ky = ky - ky1
557-
kz = kz - kz1
558-
559-
kv = np.r_[kx, ky, kz]
560-
nm = np.linalg.norm(kv)
561-
if abs(nm) < tol * _eps:
562-
return eye()
563-
else:
564-
return np.r_[qs, (math.sqrt(1.0 - qs ** 2) / nm) * kv]
537+
t12p = (R[0,1] + R[1,0]) ** 2
538+
t13p = (R[0,2] + R[2,0]) ** 2
539+
t23p = (R[1,2] + R[2,1]) ** 2
540+
541+
t12m = (R[0,1] - R[1,0]) ** 2
542+
t13m = (R[0,2] - R[2,0]) ** 2
543+
t23m = (R[1,2] - R[2,1]) ** 2
544+
545+
d1 = ( R[0,0] + R[1,1] + R[2,2] + 1) ** 2
546+
d2 = ( R[0,0] - R[1,1] - R[2,2] + 1) ** 2
547+
d3 = (-R[0,0] + R[1,1] - R[2,2] + 1) ** 2
548+
d4 = (-R[0,0] - R[1,1] + R[2,2] + 1) ** 2
549+
550+
e0 = math.sqrt( d1 + t23m + t13m + t12m) / 4.0
551+
e1 = math.sqrt(t23m + d2 + t12p + t13p) / 4.0
552+
e2 = math.sqrt(t13m + t12p + d3 + t23p) / 4.0
553+
e3 = math.sqrt(t12m + t13p + t23p + d4) / 4.0
554+
555+
# transfer sign from rotation element differences
556+
if R[2,1] < R[1,2]:
557+
e1 = -e1
558+
if R[0,2] < R[2,0]:
559+
e2 = -e2
560+
if R[1,0] < R[0,1]:
561+
e3 = -e3
562+
563+
return np.r_[e0, e1, e2, e3]
564+
565+
# def r2q_old(R, check=False, tol=100):
566+
# """
567+
# Convert SO(3) rotation matrix to unit-quaternion
568+
569+
# :arg R: SO(3) rotation matrix
570+
# :type R: ndarray(3,3)
571+
# :param check: check validity of rotation matrix, default False
572+
# :type check: bool
573+
# :param tol: tolerance in units of eps
574+
# :type tol: float
575+
# :return: unit-quaternion
576+
# :rtype: ndarray(4)
577+
# :raises ValueError: for non SO(3) argument
578+
579+
# Returns a unit-quaternion corresponding to the input SO(3) rotation matrix.
580+
581+
# .. runblock:: pycon
582+
583+
# >>> from spatialmath.base import r2q, qprint, rotx
584+
# >>> R = rotx(90, 'deg') # rotation of 90deg about x-axis
585+
# >>> print(R)
586+
# >>> qprint(r2q(R))
587+
588+
# .. warning:: There is no check that the passed matrix is a valid rotation matrix.
589+
590+
# .. note:: Scalar part is always positive.
591+
592+
# :seealso: :func:`q2r`
593+
# """
594+
# if not base.isrot(R, check=check, tol=tol):
595+
# raise ValueError("Argument must be a valid SO(3) matrix")
596+
597+
# qs = math.sqrt(max(0, np.trace(R) + 1)) / 2.0 # scalar part
598+
# kx = R[2, 1] - R[1, 2] # Oz - Ay
599+
# ky = R[0, 2] - R[2, 0] # Ax - Nz
600+
# kz = R[1, 0] - R[0, 1] # Ny - Ox
601+
602+
# if (R[0, 0] >= R[1, 1]) and (R[0, 0] >= R[2, 2]):
603+
# kx1 = R[0, 0] - R[1, 1] - R[2, 2] + 1 # Nx - Oy - Az + 1
604+
# ky1 = R[1, 0] + R[0, 1] # Ny + Ox
605+
# kz1 = R[2, 0] + R[0, 2] # Nz + Ax
606+
# add = (kx >= 0)
607+
# elif R[1, 1] >= R[2, 2]:
608+
# kx1 = R[1, 0] + R[0, 1] # Ny + Ox
609+
# ky1 = R[1, 1] - R[0, 0] - R[2, 2] + 1 # Oy - Nx - Az + 1
610+
# kz1 = R[2, 1] + R[1, 2] # Oz + Ay
611+
# add = (ky >= 0)
612+
# else:
613+
# kx1 = R[2, 0] + R[0, 2] # Nz + Ax
614+
# ky1 = R[2, 1] + R[1, 2] # Oz + Ay
615+
# kz1 = R[2, 2] - R[0, 0] - R[1, 1] + 1 # Az - Nx - Oy + 1
616+
# add = (kz >= 0)
617+
618+
# if add:
619+
# kx = kx + kx1
620+
# ky = ky + ky1
621+
# kz = kz + kz1
622+
# else:
623+
# kx = kx - kx1
624+
# ky = ky - ky1
625+
# kz = kz - kz1
626+
627+
# kv = np.r_[kx, ky, kz]
628+
# nm = np.linalg.norm(kv)
629+
# if abs(nm) < tol * _eps:
630+
# return eye()
631+
# else:
632+
# return np.r_[qs, (math.sqrt(1.0 - qs ** 2) / nm) * kv]
565633

566634

567635
def slerp(q0, q1, s, shortest=False):

0 commit comments

Comments
 (0)
0