From b63567d7563944b37f0bf04c7e280e770f6d1463 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 06:58:41 +1000 Subject: [PATCH 01/16] add rotational mean method to SO3 class --- spatialmath/pose3d.py | 23 +++++++++++++++++++++++ tests/test_pose3d.py | 13 +++++++++++++ 2 files changed, 36 insertions(+) diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index b4301d93..11da8517 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -931,6 +931,29 @@ def angdist(self, other: SO3, metric: int = 6) -> Union[float, ndarray]: else: return ad + def mean(self, tol: float = 20) -> SO3: + """Mean of a set of rotations + + :param tol: iteration tolerance in units of eps, defaults to 20 + :type tol: float, optional + :return: the mean rotation + :rtype: :class:`SO3` instance. + + Computes the Karcher mean of the set of rotations within the SO(3) instance. + + :references: + - `**Hartley, Trumpf** - "Rotation Averaging" - IJCV 2011 `_ + - `Karcher mean Date: Tue, 21 Jan 2025 07:01:09 +1000 Subject: [PATCH 02/16] add mean() method to UnitQuaternion class. Need to make log() method handle a multi-value quaternion. --- spatialmath/quaternion.py | 30 ++++++++++++++++++++++++++++-- tests/test_quaternion.py | 13 +++++++++++++ 2 files changed, 41 insertions(+), 2 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 51561036..14c20501 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -395,8 +395,11 @@ def log(self) -> Quaternion: :seealso: :meth:`Quaternion.exp` :meth:`Quaternion.log` :meth:`UnitQuaternion.angvec` """ norm = self.norm() - s = math.log(norm) - v = math.acos(np.clip(self.s / norm, -1, 1)) * smb.unitvec(self.v) + s = np.log(norm) + v = [ + math.acos(np.clip(A[0] / n, -1, 1)) * smb.unitvec(A[1:4]) + for A, n in zip(self._A, norm) + ] return Quaternion(s=s, v=v) def exp(self, tol: float = 20) -> Quaternion: @@ -1569,6 +1572,29 @@ def dotb(self, omega: ArrayLike3) -> R4: """ return smb.qdotb(self._A, omega) + def mean(self, tol: float = 20) -> SO3: + """Mean of a set of rotations + + :param tol: iteration tolerance in units of eps, defaults to 20 + :type tol: float, optional + :return: the mean rotation + :rtype: :class:`UnitQuaternion` instance. + + Computes the Karcher mean of the set of rotations within the unit quaternion instance. + + :references: + - `**Hartley, Trumpf** - "Rotation Averaging" - IJCV 2011 `_ + - `Karcher mean UnitQuaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 73c1b090..27d5ff5a 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -1018,6 +1018,19 @@ def test_miscellany(self): nt.assert_equal(q.inner(q), q.norm() ** 2) nt.assert_equal(q.inner(u), np.dot(q.vec, u.vec)) + def test_mean(self): + rpy = np.ones((100, 1)) @ np.c_[0.1, 0.2, 0.3] + q = UnitQuaternion.RPY(rpy) + self.assertEqual(len(q), 100) + m = q.mean() + self.assertIsInstance(m, UnitQuaternion) + array_compare(m, q[0]) + + # now add noise + rpy += 0.1 * np.random.rand(100, 3) + m = q.mean() + array_compare(m, q.RPY(0.1, 0.2, 0.3)) + # ---------------------------------------------------------------------------------------# if __name__ == "__main__": From 4de94b05efdd11868ef656ee6acd97f2228acd86 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 07:03:16 +1000 Subject: [PATCH 03/16] Make RPY() and Eul() methods handle matrix inputs to create multi-valued quaternion object. --- spatialmath/quaternion.py | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 14c20501..6a049240 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -1263,7 +1263,7 @@ def Eul(cls, *angles: List[float], unit: Optional[str] = "rad") -> UnitQuaternio Construct a new unit quaternion from Euler angles :param 𝚪: 3-vector of Euler angles - :type 𝚪: array_like + :type 𝚪: 3 floats, array_like(3) or ndarray(N,3) :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: unit-quaternion @@ -1289,12 +1289,15 @@ def Eul(cls, *angles: List[float], unit: Optional[str] = "rad") -> UnitQuaternio if len(angles) == 1: angles = angles[0] - return cls(smb.r2q(smb.eul2r(angles, unit=unit)), check=False) + if smb.isvector(angles, 3): + return cls(smb.r2q(smb.eul2r(angles, unit=unit)), check=False) + else: + return cls([smb.eul2r(a, unit=unit) for a in angles], check=False) @classmethod def RPY( cls, - *angles: List[float], + *angles, order: Optional[str] = "zyx", unit: Optional[str] = "rad", ) -> UnitQuaternion: @@ -1302,7 +1305,7 @@ def RPY( Construct a new unit quaternion from roll-pitch-yaw angles :param 𝚪: 3-vector of roll-pitch-yaw angles - :type 𝚪: array_like + :type 𝚪: 3 floats, array_like(3) or ndarray(N,3) :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz' @@ -1344,7 +1347,13 @@ def RPY( if len(angles) == 1: angles = angles[0] - return cls(smb.r2q(smb.rpy2r(angles, unit=unit, order=order)), check=False) + if smb.isvector(angles, 3): + return cls(smb.r2q(smb.rpy2r(angles, unit=unit, order=order)), check=False) + else: + return cls( + [smb.r2q(smb.rpy2r(a, unit=unit, order=order)) for a in angles], + check=False, + ) @classmethod def OA(cls, o: ArrayLike3, a: ArrayLike3) -> UnitQuaternion: From fba2742787e77bb21d09a4142691416d36df0100 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 20:37:02 +1000 Subject: [PATCH 04/16] working vectorized versions of RPY() and Eul() constructors, with tests. --- spatialmath/quaternion.py | 2 +- tests/test_quaternion.py | 65 ++++++++++++++++++++++++++++++++++++--- 2 files changed, 61 insertions(+), 6 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 6a049240..24e73e4e 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -1292,7 +1292,7 @@ def Eul(cls, *angles: List[float], unit: Optional[str] = "rad") -> UnitQuaternio if smb.isvector(angles, 3): return cls(smb.r2q(smb.eul2r(angles, unit=unit)), check=False) else: - return cls([smb.eul2r(a, unit=unit) for a in angles], check=False) + return cls([smb.r2q(smb.eul2r(a, unit=unit)) for a in angles], check=False) @classmethod def RPY( diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 27d5ff5a..e6de78f9 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -257,18 +257,55 @@ def test_staticconstructors(self): UnitQuaternion.Rz(theta, "deg").R, rotz(theta, "deg") ) + def test_constructor_RPY(self): # 3 angle + q = UnitQuaternion.RPY([0.1, 0.2, 0.3]) + self.assertIsInstance(q, UnitQuaternion) + self.assertEqual(len(q), 1) + nt.assert_array_almost_equal(q.R, rpy2r(0.1, 0.2, 0.3)) + q = UnitQuaternion.RPY(0.1, 0.2, 0.3) + self.assertIsInstance(q, UnitQuaternion) + self.assertEqual(len(q), 1) + nt.assert_array_almost_equal(q.R, rpy2r(0.1, 0.2, 0.3)) + q = UnitQuaternion.RPY(np.r_[0.1, 0.2, 0.3]) + self.assertIsInstance(q, UnitQuaternion) + self.assertEqual(len(q), 1) + nt.assert_array_almost_equal(q.R, rpy2r(0.1, 0.2, 0.3)) + + nt.assert_array_almost_equal( + UnitQuaternion.RPY([10, 20, 30], unit="deg").R, + rpy2r(10, 20, 30, unit="deg"), + ) nt.assert_array_almost_equal( - UnitQuaternion.RPY([0.1, 0.2, 0.3]).R, rpy2r(0.1, 0.2, 0.3) + UnitQuaternion.RPY([0.1, 0.2, 0.3], order="xyz").R, + rpy2r(0.1, 0.2, 0.3, order="xyz"), ) - nt.assert_array_almost_equal( - UnitQuaternion.Eul([0.1, 0.2, 0.3]).R, eul2r(0.1, 0.2, 0.3) + angles = np.array( + [[0.1, 0.2, 0.3], [0.2, 0.3, 0.4], [0.3, 0.4, 0.5], [0.4, 0.5, 0.6]] ) + q = UnitQuaternion.RPY(angles) + self.assertIsInstance(q, UnitQuaternion) + self.assertEqual(len(q), 4) + for i in range(4): + nt.assert_array_almost_equal(q[i].R, rpy2r(angles[i, :])) + q = UnitQuaternion.RPY(angles, order="xyz") + self.assertIsInstance(q, UnitQuaternion) + self.assertEqual(len(q), 4) + for i in range(4): + nt.assert_array_almost_equal(q[i].R, rpy2r(angles[i, :], order="xyz")) + + angles *= 10 + q = UnitQuaternion.RPY(angles, unit="deg") + self.assertIsInstance(q, UnitQuaternion) + self.assertEqual(len(q), 4) + for i in range(4): + nt.assert_array_almost_equal(q[i].R, rpy2r(angles[i, :], unit="deg")) + + def test_constructor_Eul(self): nt.assert_array_almost_equal( - UnitQuaternion.RPY([10, 20, 30], unit="deg").R, - rpy2r(10, 20, 30, unit="deg"), + UnitQuaternion.Eul([0.1, 0.2, 0.3]).R, eul2r(0.1, 0.2, 0.3) ) nt.assert_array_almost_equal( @@ -276,6 +313,23 @@ def test_staticconstructors(self): eul2r(10, 20, 30, unit="deg"), ) + angles = np.array( + [[0.1, 0.2, 0.3], [0.2, 0.3, 0.4], [0.3, 0.4, 0.5], [0.4, 0.5, 0.6]] + ) + q = UnitQuaternion.Eul(angles) + self.assertIsInstance(q, UnitQuaternion) + self.assertEqual(len(q), 4) + for i in range(4): + nt.assert_array_almost_equal(q[i].R, eul2r(angles[i, :])) + + angles *= 10 + q = UnitQuaternion.Eul(angles, unit="deg") + self.assertIsInstance(q, UnitQuaternion) + self.assertEqual(len(q), 4) + for i in range(4): + nt.assert_array_almost_equal(q[i].R, eul2r(angles[i, :], unit="deg")) + + def test_constructor_AngVec(self): # (theta, v) th = 0.2 v = unitvec([1, 2, 3]) @@ -286,6 +340,7 @@ def test_staticconstructors(self): ) nt.assert_array_almost_equal(UnitQuaternion.AngVec(th, -v).R, angvec2r(th, -v)) + def test_constructor_EulerVec(self): # (theta, v) th = 0.2 v = unitvec([1, 2, 3]) From 629bb0584a5afe1b7a28501aa26a148233d1bd68 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 20:39:28 +1000 Subject: [PATCH 05/16] allow quaternion constructor to accept s, v for multivalue case, s is a vector, v is an array. --- spatialmath/quaternion.py | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 24e73e4e..fa2c724e 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -45,10 +45,10 @@ def __init__(self, s: Any = None, v=None, check: Optional[bool] = True): r""" Construct a new quaternion - :param s: scalar - :type s: float - :param v: vector - :type v: 3-element array_like + :param s: scalar part + :type s: float or ndarray(N) + :param v: vector part + :type v: ndarray(3), ndarray(Nx3) - ``Quaternion()`` constructs a zero quaternion - ``Quaternion(s, v)`` construct a new quaternion from the scalar ``s`` @@ -78,7 +78,7 @@ def __init__(self, s: Any = None, v=None, check: Optional[bool] = True): super().__init__() if s is None and smb.isvector(v, 4): - v,s = (s,v) + v, s = (s, v) if v is None: # single argument @@ -92,6 +92,11 @@ def __init__(self, s: Any = None, v=None, check: Optional[bool] = True): # Quaternion(s, v) self.data = [np.r_[s, smb.getvector(v)]] + elif ( + smb.isvector(s) and smb.ismatrix(v, (None, 3)) and s.shape[0] == v.shape[0] + ): + # Quaternion(s, v) where s and v are arrays + self.data = [np.r_[_s, _v] for _s, _v in zip(s, v)] else: raise ValueError("bad argument to Quaternion constructor") From be59431641ac256b7a56bf39a3eb5534b26656d1 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 20:41:35 +1000 Subject: [PATCH 06/16] working version of log() that handles multi-value case and the edge case when vector part is zero. --- spatialmath/quaternion.py | 21 ++++++++++++++++----- tests/test_quaternion.py | 14 ++++++++++++++ 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index fa2c724e..3143fa7c 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -401,11 +401,22 @@ def log(self) -> Quaternion: """ norm = self.norm() s = np.log(norm) - v = [ - math.acos(np.clip(A[0] / n, -1, 1)) * smb.unitvec(A[1:4]) - for A, n in zip(self._A, norm) - ] - return Quaternion(s=s, v=v) + if len(self) == 1: + if smb.iszerovec(self._A[1:4]): + v = np.zeros((3,)) + else: + v = math.acos(np.clip(self._A[0] / norm, -1, 1)) * smb.unitvec( + self._A[1:4] + ) + return Quaternion(s=s, v=v) + else: + v = [ + np.zeros((3,)) + if smb.iszerovec(A[1:4]) + else math.acos(np.clip(A[0] / n, -1, 1)) * smb.unitvec(A[1:4]) + for A, n in zip(self._A, norm) + ] + return Quaternion(s=s, v=np.array(v)) def exp(self, tol: float = 20) -> Quaternion: r""" diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index e6de78f9..68082b7f 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -885,6 +885,20 @@ def test_log(self): nt.assert_array_almost_equal(q1.log().exp(), q1) nt.assert_array_almost_equal(q2.log().exp(), q2) + q = Quaternion([q1, q2, q1, q2]) + qlog = q.log() + nt.assert_array_almost_equal(qlog[0].exp(), q1) + nt.assert_array_almost_equal(qlog[1].exp(), q2) + nt.assert_array_almost_equal(qlog[2].exp(), q1) + nt.assert_array_almost_equal(qlog[3].exp(), q2) + + q = UnitQuaternion() # identity + qlog = q.log() + nt.assert_array_almost_equal(qlog.vec, np.zeros(4)) + qq = qlog.exp() + self.assertIsInstance(qq, UnitQuaternion) + nt.assert_array_almost_equal(qq.vec, np.r_[1, 0, 0, 0]) + def test_concat(self): u = Quaternion() uu = Quaternion([u, u, u, u]) From 4752a59840497d5e0302b087e04cd4cff7c93f1c Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 20:42:14 +1000 Subject: [PATCH 07/16] exp() to handle the edge case where result is a unit quaternion. --- spatialmath/quaternion.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 3143fa7c..2d59f35c 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -456,7 +456,11 @@ def exp(self, tol: float = 20) -> Quaternion: exp_s = math.exp(self.s) norm_v = smb.norm(self.v) s = exp_s * math.cos(norm_v) - v = exp_s * self.v / norm_v * math.sin(norm_v) + if smb.iszerovec(self.v, tol * _eps): + # result will be a unit quaternion + v = np.zeros((3,)) + else: + v = exp_s * self.v / norm_v * math.sin(norm_v) if abs(self.s) < tol * _eps: # result will be a unit quaternion return UnitQuaternion(s=s, v=v) From 80e09280e806bd264a0cbb7c9ab2bd9fca917800 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 20:43:06 +1000 Subject: [PATCH 08/16] working version of mean() for unit quaternion --- spatialmath/quaternion.py | 2 +- tests/test_quaternion.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 2d59f35c..9e496c9a 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -1619,7 +1619,7 @@ def mean(self, tol: float = 20) -> SO3: eta = tol * np.finfo(float).eps q_mean = self[0] # initial guess while True: - r = np.dstack((self.inv() * self).log()) / len(self) + r = (self.inv() * self).log().vec.mean(axis=0) if np.linalg.norm(r) < eta: return q_mean q_mean = q_mean @ self.Exp(r) # update estimate and normalize diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 68082b7f..d76273d9 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -1093,12 +1093,12 @@ def test_mean(self): self.assertEqual(len(q), 100) m = q.mean() self.assertIsInstance(m, UnitQuaternion) - array_compare(m, q[0]) + nt.assert_array_almost_equal(m.vec, q[0].vec) # now add noise rpy += 0.1 * np.random.rand(100, 3) m = q.mean() - array_compare(m, q.RPY(0.1, 0.2, 0.3)) + nt.assert_array_almost_equal(m.vec, q.RPY(0.1, 0.2, 0.3).vec) # ---------------------------------------------------------------------------------------# From e00aa2d805a6a07d815eee643526a678e8ff6047 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 20:43:46 +1000 Subject: [PATCH 09/16] fix docstring indentation --- spatialmath/pose3d.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index 11da8517..fd2f72bb 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -843,22 +843,22 @@ def Exp( def UnitQuaternion(self) -> UnitQuaternion: """ - SO3 as a unit quaternion instance + SO3 as a unit quaternion instance - :return: a unit quaternion representation - :rtype: UnitQuaternion instance + :return: a unit quaternion representation + :rtype: UnitQuaternion instance - ``R.UnitQuaternion()`` is an ``UnitQuaternion`` instance representing the same rotation - as the SO3 rotation ``R``. + ``R.UnitQuaternion()`` is an ``UnitQuaternion`` instance representing the same rotation + as the SO3 rotation ``R``. - Example: + Example: - .. runblock:: pycon + .. runblock:: pycon - >>> from spatialmath import SO3 - >>> SO3.Rz(0.3).UnitQuaternion() + >>> from spatialmath import SO3 + >>> SO3.Rz(0.3).UnitQuaternion() - """ + """ # Function level import to avoid circular dependencies from spatialmath import UnitQuaternion From c0712340646ce41eaedce6aef48cec2ed7005f73 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 20:44:27 +1000 Subject: [PATCH 10/16] working version of mean() for SO3 class. --- spatialmath/pose3d.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index fd2f72bb..629605f9 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -949,7 +949,7 @@ def mean(self, tol: float = 20) -> SO3: eta = tol * np.finfo(float).eps R_mean = self[0] # initial guess while True: - r = np.dstack((self.inv() * self).log()) / len(self) + r = np.dstack((self.inv() * self).log()).mean(axis=2) if np.linalg.norm(r) < eta: return R_mean R_mean = R_mean @ self.Exp(r) # update estimate and normalize From b6c6557c586ccaa1b574fd75fd578d3c7bc14e2c Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Tue, 21 Jan 2025 20:55:12 +1000 Subject: [PATCH 11/16] tests for SO3 log/exp edge cases --- tests/test_pose3d.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 3c0e0291..443c9731 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -717,6 +717,16 @@ def test_functions_lie(self): nt.assert_equal(R, SO3.EulerVec(R.eulervec())) np.testing.assert_equal((R.inv() * R).eulervec(), np.zeros(3)) + R = SO3() # identity matrix case + + # Check log and exponential map + nt.assert_equal(R, SO3.Exp(R.log())) + np.testing.assert_equal((R.inv() * R).log(), np.zeros([3, 3])) + + # Check euler vector map + nt.assert_equal(R, SO3.EulerVec(R.eulervec())) + np.testing.assert_equal((R.inv() * R).eulervec(), np.zeros(3)) + def test_mean(self): rpy = np.ones((100, 1)) @ np.c_[0.1, 0.2, 0.3] R = SO3.RPY(rpy) From 7c1c1f6b1cbb29efc71ab22a74b6490664b442dc Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Thu, 23 Jan 2025 14:49:16 +1000 Subject: [PATCH 12/16] doco error --- spatialmath/pose3d.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index 629605f9..afc83bff 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -942,8 +942,8 @@ def mean(self, tol: float = 20) -> SO3: Computes the Karcher mean of the set of rotations within the SO(3) instance. :references: - - `**Hartley, Trumpf** - "Rotation Averaging" - IJCV 2011 `_ - - `Karcher mean `_, Algorithm 1, page 15. + - `Karcher mean `_ """ eta = tol * np.finfo(float).eps From 05f565573272dd9825f2c1784a05c40665e9d880 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 26 Jan 2025 16:25:06 +1100 Subject: [PATCH 13/16] For now, remove the unit quaternion method. Need to find a reference for a quaternion implementation, rather than converting to/from SO(3) matrices. --- spatialmath/quaternion.py | 31 +++++++++++++------------------ tests/test_quaternion.py | 32 ++++++++++++++++++++------------ 2 files changed, 33 insertions(+), 30 deletions(-) diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index 9e496c9a..3633646b 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -1601,28 +1601,23 @@ def dotb(self, omega: ArrayLike3) -> R4: """ return smb.qdotb(self._A, omega) - def mean(self, tol: float = 20) -> SO3: - """Mean of a set of rotations + # def mean(self, tol: float = 20) -> SO3: + # """Mean of a set of rotations - :param tol: iteration tolerance in units of eps, defaults to 20 - :type tol: float, optional - :return: the mean rotation - :rtype: :class:`UnitQuaternion` instance. + # :param tol: iteration tolerance in units of eps, defaults to 20 + # :type tol: float, optional + # :return: the mean rotation + # :rtype: :class:`UnitQuaternion` instance. - Computes the Karcher mean of the set of rotations within the unit quaternion instance. + # Computes the Karcher mean of the set of rotations within the unit quaternion instance. - :references: - - `**Hartley, Trumpf** - "Rotation Averaging" - IJCV 2011 `_ - - `Karcher mean `_ + # - `Karcher mean Date: Sun, 26 Jan 2025 16:26:08 +1100 Subject: [PATCH 14/16] make random testing reproducible. add a test over a range of rotations. --- tests/test_pose3d.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 443c9731..581b8306 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -735,8 +735,15 @@ def test_mean(self): self.assertIsInstance(m, SO3) array_compare(m, R[0]) + # range of angles, mean should be the middle one, index=25 + R = SO3.Rz(np.linspace(start=0.3, stop=0.7, num=51)) + m = R.mean() + self.assertIsInstance(m, SO3) + array_compare(m, R[25]) + # now add noise - rpy += 0.1 * np.random.rand(100, 3) + rng = np.random.default_rng(0) # reproducible random numbers + rpy += rng.normal(scale=0.1, size=(100, 3)) m = R.mean() array_compare(m, SO3.RPY(0.1, 0.2, 0.3)) From d8249b88f33dd0f1d055e6d529f77897a4df7fea Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 26 Jan 2025 16:27:33 +1100 Subject: [PATCH 15/16] fix bug in implementation --- spatialmath/pose3d.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index afc83bff..4e558512 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -949,7 +949,7 @@ def mean(self, tol: float = 20) -> SO3: eta = tol * np.finfo(float).eps R_mean = self[0] # initial guess while True: - r = np.dstack((self.inv() * self).log()).mean(axis=2) + r = np.dstack((R_mean.inv() * self).log()).mean(axis=2) if np.linalg.norm(r) < eta: return R_mean R_mean = R_mean @ self.Exp(r) # update estimate and normalize From 04cbe7fda7f2929a329e7890f33ed24e8ff5d70c Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Sun, 26 Jan 2025 16:34:43 +1100 Subject: [PATCH 16/16] reduce standard deviation of noise to make the test pass --- tests/test_pose3d.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 581b8306..ca03b70e 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -743,7 +743,8 @@ def test_mean(self): # now add noise rng = np.random.default_rng(0) # reproducible random numbers - rpy += rng.normal(scale=0.1, size=(100, 3)) + rpy += rng.normal(scale=0.00001, size=(100, 3)) + R = SO3.RPY(rpy) m = R.mean() array_compare(m, SO3.RPY(0.1, 0.2, 0.3))