Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

Appearance settings

Commit 99fd55b

Browse filesBrowse files
authored
Add mean() method for SE3 objects. (#167)
1 parent 550d6fb commit 99fd55b
Copy full SHA for 99fd55b

File tree

Expand file treeCollapse file tree

2 files changed

+52
-2
lines changed
Filter options
Expand file treeCollapse file tree

2 files changed

+52
-2
lines changed

‎spatialmath/pose3d.py

Copy file name to clipboardExpand all lines: spatialmath/pose3d.py
+26-2Lines changed: 26 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -983,18 +983,20 @@ def angdist(self, other: SO3, metric: int = 6) -> Union[float, ndarray]:
983983
return ad
984984

985985
def mean(self, tol: float = 20) -> SO3:
986-
"""Mean of a set of rotations
986+
"""Mean of a set of SO(3) values
987987
988988
:param tol: iteration tolerance in units of eps, defaults to 20
989989
:type tol: float, optional
990990
:return: the mean rotation
991991
:rtype: :class:`SO3` instance.
992992
993-
Computes the Karcher mean of the set of rotations within the SO(3) instance.
993+
Computes the Karcher mean of the set of SO(3) rotations within the :class:`SO3` instance.
994994
995995
:references:
996996
- `**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.
997997
- `Karcher mean <https://en.wikipedia.org/wiki/Karcher_mean>`_
998+
999+
:seealso: :class:`SE3.mean`
9981000
"""
9991001

10001002
eta = tol * np.finfo(float).eps
@@ -2194,6 +2196,28 @@ def angdist(self, other: SE3, metric: int = 6) -> float:
21942196
else:
21952197
return ad
21962198

2199+
def mean(self, tol: float = 20) -> SE3:
2200+
"""Mean of a set of SE(3) values
2201+
2202+
:param tol: iteration tolerance in units of eps, defaults to 20
2203+
:type tol: float, optional
2204+
:return: the mean SE(3) pose
2205+
:rtype: :class:`SE3` instance.
2206+
2207+
Computes the mean of all the SE(3) values within the :class:`SE3` instance. Rotations are
2208+
averaged using the Karcher mean, and translations are averaged using the
2209+
arithmetic mean.
2210+
2211+
:references:
2212+
- `**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.
2213+
- `Karcher mean <https://en.wikipedia.org/wiki/Karcher_mean>`_
2214+
2215+
:seealso: :meth:`SO3.mean`
2216+
"""
2217+
R_mean = SO3(self).mean(tol)
2218+
t_mean = self.t.mean(axis=0)
2219+
return SE3.Rt(R_mean, t_mean)
2220+
21972221
# @classmethod
21982222
# def SO3(cls, R, t=None, check=True):
21992223
# if isinstance(R, SO3):

‎tests/test_pose3d.py

Copy file name to clipboardExpand all lines: tests/test_pose3d.py
+26Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1429,6 +1429,32 @@ def test_interp(self):
14291429
test_angle = path_se3[i].angdist(path_se3[i + 1])
14301430
assert abs(test_angle - angle) < 1e-6
14311431

1432+
def test_mean(self):
1433+
rpy = np.ones((100, 1)) @ np.c_[0.1, 0.2, 0.3]
1434+
T = SE3.RPY(rpy)
1435+
self.assertEqual(len(T), 100)
1436+
m = T.mean()
1437+
self.assertIsInstance(m, SE3)
1438+
array_compare(m, T[0])
1439+
1440+
# range of angles, mean should be the middle one, index=25
1441+
T = SE3.Rz(np.linspace(start=0.3, stop=0.7, num=51))
1442+
m = T.mean()
1443+
self.assertIsInstance(m, SE3)
1444+
array_compare(m, T[25])
1445+
1446+
# now add noise
1447+
rng = np.random.default_rng(0) # reproducible random numbers
1448+
rpy += rng.normal(scale=0.00001, size=(100, 3))
1449+
T = SE3.RPY(rpy)
1450+
m = T.mean()
1451+
array_compare(m, SE3.RPY(0.1, 0.2, 0.3))
1452+
1453+
T = SE3.Tz(np.linspace(start=-2, stop=1, num=51))
1454+
m = T.mean()
1455+
self.assertIsInstance(m, SE3)
1456+
array_compare(m, T[25])
1457+
14321458

14331459
# ---------------------------------------------------------------------------------------#
14341460
if __name__ == "__main__":

0 commit comments

Comments
0 (0)
Morty Proxy This is a proxified and sanitized view of the page, visit original site.