Skip to content

Commit 2e15eaf

Browse files
committed
added angdist metric
1 parent 29300b1 commit 2e15eaf

1 file changed

Lines changed: 36 additions & 18 deletions

File tree

‎spatialmath/pose3d.py‎

Lines changed: 36 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -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

Comments
 (0)