@@ -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
567635def slerp (q0 , q1 , s , shortest = False ):
0 commit comments