Skip to content

Commit 26568b0

Browse files
committed
add angular distance metrics
1 parent 20ab96d commit 26568b0

2 files changed

Lines changed: 70 additions & 10 deletions

File tree

‎spatialmath/pose3d.py‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -619,6 +619,55 @@ def Exp(cls, S, check=True, so3=True):
619619
else:
620620
return cls(base.trexp(S, check=check), check=False)
621621

622+
def angdist(self, other, metric=3):
623+
r"""
624+
Angular distance metric between unit quaternions
625+
626+
:param other: second unit quaternion
627+
:type other: UnitQuaternion instance
628+
:param metric: metric, default is 3
629+
:type metric: int
630+
:raises TypeError: if other is not a UnitQuaternion
631+
:return: angle in radians
632+
:rtype: float
633+
634+
``q1.angdist(q2)`` is the geodesic norm, or geodesic distance between two
635+
unit quaternions. We can consider it as the angle between two quaternions.
636+
637+
Several metrics are supported:
638+
639+
====== ===============================================================
640+
Metric Details
641+
====== ===============================================================
642+
0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]`
643+
1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
644+
2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
645+
3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
646+
4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]`
647+
====== ===============================================================
648+
649+
Example:
650+
651+
.. runblock:: pycon
652+
653+
>>> from spatialmath import UnitQuaternion
654+
>>> q1 = UnitQuaternion.Rx(0.3)
655+
>>> q2 = UnitQuaternion.Ry(0.3)
656+
>>> print(q1.angdist(q1))
657+
>>> print(q1.angdist(q2))
658+
659+
.. note::
660+
- metrics 1, 2, 4 can throw ValueError "math domain error" due to
661+
numeric errors which push the argument of ``acos()`` marginally
662+
outside its domain [0, 1].
663+
- metrics 2 and 3 are equivalent, but 3 is more robust
664+
- SMTB-MATLAB uses metric 3 for UnitQuaternion.angle()
665+
- MATLAB's quaternion.dist() uses metric 4
666+
"""
667+
from spatialmath.quaternion import UnitQuaternion
668+
669+
return UnitQuaternion(self).angdist(UnitQuaternion(other), metric=metric)
670+
622671
# ============================== SE3 =====================================#
623672

624673

‎spatialmath/quaternion.py‎

Lines changed: 21 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -2127,20 +2127,20 @@ def angvec(self, unit='rad'):
21272127
# """
21282128
# return Quaternion(s=0, v=math.acos(self.s) * base.unitvec(self.v))
21292129

2130-
def angdist(self, other, metric=0):
2130+
def angdist(self, other, metric=3):
21312131
r"""
21322132
Angular distance metric between unit quaternions
21332133
21342134
:param other: second unit quaternion
21352135
:type other: UnitQuaternion instance
2136-
:param metric: metric, default is 0
2136+
:param metric: metric, default is 3
21372137
:type metric: int
21382138
:raises TypeError: if other is not a UnitQuaternion
21392139
:return: angle in radians
21402140
:rtype: float
21412141
2142-
``q1.angle(q2)`` is the geodesic norm, or geodesic distance between two
2143-
unit quaternions and has units of angle. We can consider it as the angle between two quaternions.
2142+
``q1.angdist(q2)`` is the geodesic norm, or geodesic distance between two
2143+
unit quaternions. We can consider it as the angle between two quaternions.
21442144
21452145
Several metrics are supported:
21462146
@@ -2150,10 +2150,13 @@ def angdist(self, other, metric=0):
21502150
0 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]`
21512151
1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
21522152
2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
2153-
3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
2153+
3 :math:`2 \tan^{-1} \| \q_1 \pm \q_2\| / \|\q_1 \mp \q_2\| \in [0, \pi/2]`
21542154
4 :math:`\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]`
21552155
====== ===============================================================
21562156
2157+
Metric 3 computes the sum and difference of the quaternions and uses
2158+
the largest value in the denominator.
2159+
21572160
Example:
21582161
21592162
.. runblock:: pycon
@@ -2170,19 +2173,27 @@ def angdist(self, other, metric=0):
21702173
outside its domain [0, 1].
21712174
- metrics 2 and 3 are equivalent, but 3 is more robust
21722175
- SMTB-MATLAB uses metric 3 for UnitQuaternion.angle()
2173-
- MATLAB's quaternion.dist() uses metric 2
2176+
- MATLAB's quaternion.dist() uses metric 4
21742177
"""
21752178
if not isinstance(other, UnitQuaternion):
21762179
raise TypeError('bad operand')
21772180

2181+
def metric3(p, q):
2182+
x = base.norm(p - q)
2183+
y = base.norm(p + q)
2184+
if x >= y:
2185+
return 2 * math.atan(y / x)
2186+
else:
2187+
return 2 * math.atan(x / y)
2188+
21782189
if metric == 0:
2179-
measure = lambda p, q: 1 - np.dot(p, q)
2190+
measure = lambda p, q: 1 - abs(np.dot(p, q))
21802191
elif metric == 1:
2181-
measure = lambda p, q: math.acos(np.dot(p, q))
2192+
measure = lambda p, q: math.acos(abs(np.dot(p, q)))
21822193
elif metric == 2:
2183-
measure = lambda p, q: math.acos(np.dot(p, q))
2194+
measure = lambda p, q: math.acos(abs(np.dot(p, q)))
21842195
elif metric == 3:
2185-
measure = lambda p, q: 2 * math.atan2(base.norm(p - q), base.norm(p + q))
2196+
measure = metric3
21862197
elif metric == 4:
21872198
measure = lambda p, q: math.acos(2 * np.dot(p, q) ** 2 - 1)
21882199

0 commit comments

Comments
 (0)