From 550d6fb52c4e9daf51c801424de3a6c630022d34 Mon Sep 17 00:00:00 2001 From: Thomas Weng <157421342+tweng-bdai@users.noreply.github.com> Date: Wed, 19 Feb 2025 16:56:00 -0500 Subject: [PATCH 1/2] #165 use qunit in trinterp (#166) Co-authored-by: Mark Yeatman --- spatialmath/base/transforms3d.py | 6 ++--- tests/test_pose3d.py | 40 ++++++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+), 3 deletions(-) diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py index bc2ceb05..08d3be26 100644 --- a/spatialmath/base/transforms3d.py +++ b/spatialmath/base/transforms3d.py @@ -43,7 +43,7 @@ tr2rt, Ab2M, ) -from spatialmath.base.quaternions import r2q, q2r, qeye, qslerp +from spatialmath.base.quaternions import r2q, q2r, qeye, qslerp, qunit from spatialmath.base.graphics import plotvol3, axes_logic from spatialmath.base.animate import Animate import spatialmath.base.symbolic as sym @@ -1675,7 +1675,7 @@ def trinterp(start, end, s, shortest=True): q1 = r2q(end) qr = qslerp(q0, q1, s, shortest=shortest) - return q2r(qr) + return q2r(qunit(qr)) elif ismatrix(end, (4, 4)): # SE(3) case @@ -1697,7 +1697,7 @@ def trinterp(start, end, s, shortest=True): qr = qslerp(q0, q1, s, shortest=shortest) pr = p0 * (1 - s) + s * p1 - return rt2tr(q2r(qr), pr) + return rt2tr(q2r(qunit(qr)), pr) else: return ValueError("Argument must be SO(3) or SE(3)") diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index 70b33ce0..fc9daf93 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -1389,6 +1389,46 @@ def test_rtvec(self): nt.assert_equal(rvec, [0, 1, 0]) nt.assert_equal(tvec, [2, 3, 4]) + def test_interp(self): + # This data is taken from https://github.com/bdaiinstitute/spatialmath-python/issues/165 + se3_1 = SE3() + se3_1.t = np.array( + [0.5705748101710814, 0.29623210833184527, 0.10764106509086407] + ) + se3_1.R = np.array( + [ + [0.2852875203191073, 0.9581330588259315, -0.024332536551692617], + [0.9582072394229962, -0.28568756930438033, -0.014882844564011068], + [-0.021211248608609852, -0.019069722856395098, -0.9995931315303468], + ] + ) + assert SE3.isvalid(se3_1.A) + + se3_2 = SE3() + se3_2.t = np.array( + [0.5150284150005691, 0.25796537207802533, 0.1558725490743694] + ) + se3_2.R = np.array( + [ + [0.42058255728234184, 0.9064420651629983, -0.038380919906699236], + [0.9070822373513454, -0.4209501599465646, -0.0016665901233428627], + [-0.01766712176680449, -0.0341137119645545, -0.9992617912561634], + ] + ) + assert SE3.isvalid(se3_2.A) + + path_se3 = se3_1.interp(end=se3_2, s=15, shortest=False) + + angle = None + for i in range(len(path_se3) - 1): + assert SE3.isvalid(path_se3[i].A) + + if angle is None: + angle = path_se3[i].angdist(path_se3[i + 1]) + else: + test_angle = path_se3[i].angdist(path_se3[i + 1]) + assert abs(test_angle - angle) < 1e-6 + # ---------------------------------------------------------------------------------------# if __name__ == "__main__": From 99fd55b8fefa3113fc7a6ea99e5b0232adb541a6 Mon Sep 17 00:00:00 2001 From: Peter Corke Date: Fri, 7 Mar 2025 22:37:37 +1000 Subject: [PATCH 2/2] Add `mean()` method for `SE3` objects. (#167) --- spatialmath/pose3d.py | 28 ++++++++++++++++++++++++++-- tests/test_pose3d.py | 26 ++++++++++++++++++++++++++ 2 files changed, 52 insertions(+), 2 deletions(-) diff --git a/spatialmath/pose3d.py b/spatialmath/pose3d.py index 9324eda1..b8d8d5de 100644 --- a/spatialmath/pose3d.py +++ b/spatialmath/pose3d.py @@ -983,18 +983,20 @@ def angdist(self, other: SO3, metric: int = 6) -> Union[float, ndarray]: return ad def mean(self, tol: float = 20) -> SO3: - """Mean of a set of rotations + """Mean of a set of SO(3) values :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. + Computes the Karcher mean of the set of SO(3) rotations within the :class:`SO3` instance. :references: - `**Hartley, Trumpf** - "Rotation Averaging" - IJCV 2011 `_, Algorithm 1, page 15. - `Karcher mean `_ + + :seealso: :class:`SE3.mean` """ eta = tol * np.finfo(float).eps @@ -2194,6 +2196,28 @@ def angdist(self, other: SE3, metric: int = 6) -> float: else: return ad + def mean(self, tol: float = 20) -> SE3: + """Mean of a set of SE(3) values + + :param tol: iteration tolerance in units of eps, defaults to 20 + :type tol: float, optional + :return: the mean SE(3) pose + :rtype: :class:`SE3` instance. + + Computes the mean of all the SE(3) values within the :class:`SE3` instance. Rotations are + averaged using the Karcher mean, and translations are averaged using the + arithmetic mean. + + :references: + - `**Hartley, Trumpf** - "Rotation Averaging" - IJCV 2011 `_, Algorithm 1, page 15. + - `Karcher mean `_ + + :seealso: :meth:`SO3.mean` + """ + R_mean = SO3(self).mean(tol) + t_mean = self.t.mean(axis=0) + return SE3.Rt(R_mean, t_mean) + # @classmethod # def SO3(cls, R, t=None, check=True): # if isinstance(R, SO3): diff --git a/tests/test_pose3d.py b/tests/test_pose3d.py index fc9daf93..35233dd2 100755 --- a/tests/test_pose3d.py +++ b/tests/test_pose3d.py @@ -1429,6 +1429,32 @@ def test_interp(self): test_angle = path_se3[i].angdist(path_se3[i + 1]) assert abs(test_angle - angle) < 1e-6 + def test_mean(self): + rpy = np.ones((100, 1)) @ np.c_[0.1, 0.2, 0.3] + T = SE3.RPY(rpy) + self.assertEqual(len(T), 100) + m = T.mean() + self.assertIsInstance(m, SE3) + array_compare(m, T[0]) + + # range of angles, mean should be the middle one, index=25 + T = SE3.Rz(np.linspace(start=0.3, stop=0.7, num=51)) + m = T.mean() + self.assertIsInstance(m, SE3) + array_compare(m, T[25]) + + # now add noise + rng = np.random.default_rng(0) # reproducible random numbers + rpy += rng.normal(scale=0.00001, size=(100, 3)) + T = SE3.RPY(rpy) + m = T.mean() + array_compare(m, SE3.RPY(0.1, 0.2, 0.3)) + + T = SE3.Tz(np.linspace(start=-2, stop=1, num=51)) + m = T.mean() + self.assertIsInstance(m, SE3) + array_compare(m, T[25]) + # ---------------------------------------------------------------------------------------# if __name__ == "__main__":