@@ -622,22 +622,23 @@ def Exp(cls, S, check=True, so3=True):
622622 else :
623623 return cls (base .trexp (S , check = check ), check = False )
624624
625- def angdist (self , other , metric = 3 ):
625+ def angdist (self , other , metric = 6 ):
626626 r"""
627- Angular distance metric between unit quaternions
627+ Angular distance metric between rotations
628628
629- :param other: second unit quaternion
630- :type other: UnitQuaternion instance
631- :param metric: metric, default is 3
629+ :param other: second rotation
630+ :type other: SO3 instance
631+ :param metric: metric, default is 6
632632 :type metric: int
633- :raises TypeError: if other is not a UnitQuaternion
633+ :raises TypeError: if other is not an SO3
634634 :return: angle in radians
635- :rtype: float
635+ :rtype: float or ndarray
636636
637- ``q1 .angdist(q2 )`` is the geodesic norm, or geodesic distance between two
638- unit quaternions. We can consider it as the angle between two quaternions .
637+ ``R1 .angdist(R2 )`` is the geodesic norm, or geodesic distance between two
638+ rotations .
639639
640- Several metrics are supported:
640+ Several metrics are supported, the first 5 are computed after conversion
641+ to unit quaternions.
641642
642643 ====== ===============================================================
643644 Metric Details
@@ -647,29 +648,46 @@ def angdist(self, other, metric=3):
647648 2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
648649 3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
649650 4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]`
651+ 5 :math:`\|I - \mat{R}_1 \mat{R}_2^T\| \in [0, 2]`
652+ 6 :math:`\|\log \mat{R}_1 \mat{R}_2^T\| \in [0, \pi]`
650653 ====== ===============================================================
651654
652655 Example:
653656
654657 .. runblock:: pycon
655658
656659 >>> from spatialmath import UnitQuaternion
657- >>> q1 = UnitQuaternion .Rx(0.3)
658- >>> q2 = UnitQuaternion .Ry(0.3)
659- >>> print(q1 .angdist(q1 ))
660- >>> print(q1 .angdist(q2 ))
660+ >>> R1 = SO3 .Rx(0.3)
661+ >>> R2 = SO3 .Ry(0.3)
662+ >>> print(R1 .angdist(R1 ))
663+ >>> print(R1 .angdist(R2 ))
661664
662665 .. note::
663666 - metrics 1, 2, 4 can throw ValueError "math domain error" due to
664667 numeric errors which push the argument of ``acos()`` marginally
665668 outside its domain [0, 1].
666669 - metrics 2 and 3 are equivalent, but 3 is more robust
667- - SMTB-MATLAB uses metric 3 for UnitQuaternion.angle()
668- - MATLAB's quaternion.dist() uses metric 4
670+
671+ :seealso: :func:`UnitQuaternion.angdist`
669672 """
670- from spatialmath .quaternion import UnitQuaternion
671673
672- return UnitQuaternion (self ).angdist (UnitQuaternion (other ), metric = metric )
674+ if metric < 5 :
675+ from spatialmath .quaternion import UnitQuaternion
676+
677+ return UnitQuaternion (self ).angdist (UnitQuaternion (other ), metric = metric )
678+
679+ elif metric == 5 :
680+ op = lambda R1 , R2 : np .linalg .norm (np .eye (3 ) - R1 @ R2 .T )
681+ elif metric == 6 :
682+ op = lambda R1 , R2 : base .norm (base .trlog (R1 @ R2 .T , twist = True ))
683+ else :
684+ raise ValueError ('unknown metric' )
685+
686+ ad = self ._op2 (other , op )
687+ if isinstance (ad , list ):
688+ return np .array (ad )
689+ else :
690+ return ad
673691
674692# ============================== SE3 =====================================#
675693
0 commit comments