Add mean() method for SE3 objects. (#167)

Author: petercorke

Diff for: 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 <https://users.cecs.anu.edu.au/~hartley/Papers/PDF/Hartley-Trumpf:Rotation-averaging:IJCV.pdf>`_, Algorithm 1, page 15.
             - `Karcher mean <https://en.wikipedia.org/wiki/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 <https://users.cecs.anu.edu.au/~hartley/Papers/PDF/Hartley-Trumpf:Rotation-averaging:IJCV.pdf>`_, Algorithm 1, page 15.
+            - `Karcher mean <https://en.wikipedia.org/wiki/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 for: 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__":