Skip to content

Commit a5011df

Browse files
committed
Add angdist method for angular distance metric
angle method is gone
1 parent 607b587 commit a5011df

1 file changed

Lines changed: 35 additions & 6 deletions

File tree

‎spatialmath/quaternion.py‎

Lines changed: 35 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -2018,34 +2018,63 @@ def angvec(self, unit='rad'):
20182018
# """
20192019
# return Quaternion(s=0, v=math.acos(self.s) * base.unitvec(self.v))
20202020

2021-
def angle(self, other):
2021+
def angdist(self, other, metric=0):
20222022
"""
2023-
Angle between unit quaternions
2023+
Angular distance metric between unit quaternions
20242024
20252025
:param other: second unit quaternion
20262026
:type other: UnitQuaternion instance
2027+
:param metric: metric, default is 0
2028+
:type metric: int
20272029
:raises TypeError: if other is not a UnitQuaternion
20282030
:return: angle in radians
20292031
:rtype: float
20302032
20312033
``q1.angle(q2)`` is the geodesic norm, or geodesic distance between two
20322034
unit quaternions and has units of angle. We can consider it as the angle between two quaternions.
20332035
2036+
Several metrics are supported:
2037+
2038+
====== ===============================================================
2039+
Metric Details
2040+
====== ===============================================================
2041+
0 :math:`1 - | \q_1 \sbullet \q_2 | \in [0, 1]`
2042+
1 :math:`\cos^{-1} | \q_1 \sbullet \q_2 | \in [0, \pi/2]`
2043+
2 :math:`2 \cos^{-1} | \q_1 \sbullet \q_2 | \in [0, \pi]`
2044+
3 :math:`2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]`
2045+
4 :math:`\cos^{-1} \left( 2 (\q_1 \sbullet \q_2)^2 - 1\right) \in [0, 1]`
2046+
====== ===============================================================
2047+
20342048
Example:
20352049
20362050
.. runblock:: pycon
20372051
20382052
>>> from spatialmath import UnitQuaternion
20392053
>>> q1 = UnitQuaternion.Rx(0.3)
2040-
>>> q2 = UnitQuaternion.Rx(0.3)
2041-
>>> print(q1.angle(q1))
2042-
>>> print(q1.angle(q2))
2054+
>>> q2 = UnitQuaternion.Ry(0.3)
2055+
>>> print(q1.angdist(q1))
2056+
>>> print(q1.angdist(q2))
20432057
2058+
.. note::
2059+
- metrics 2 and 3 are equivalent
2060+
- SMTB-MATLAB uses metric 3 for UnitQuaternion.angle()
2061+
- MATLAB's quaternion.dist() uses metric 2
20442062
"""
20452063
if not isinstance(other, UnitQuaternion):
20462064
raise TypeError('bad operand')
20472065

2048-
return self.binop(other, lambda p, q: math.acos(2 * np.dot(p, q) ** 2 - 1))
2066+
if metric == 0:
2067+
measure = lambda p, q: 1 - np.dot(p, q)
2068+
elif metric == 1:
2069+
measure = lambda p, q: math.acos(np.dot(p, q))
2070+
elif metric == 2:
2071+
measure = lambda p, q: 2 * math.acos(np.dot(p, q))
2072+
elif metric == 3:
2073+
measure = lambda p, q: 2 * math.atan2(base.norm(p - q), base.norm(p + q))
2074+
elif metric == 4:
2075+
measure = lambda p, q: math.acos(2 * np.dot(p, q) ** 2 - 1)
2076+
2077+
return self.binop(other, measure)
20492078

20502079
def SO3(self):
20512080
"""

0 commit comments

Comments
 (0)