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__": pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy