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__":