Python中李群SO(3)与李代数so(3)之间指数映射与实现源码
程序员文章站
2024-01-26 11:13:04
Python中李群SO(3)与李代数so(3)之间指数映射与实现源码调用scipy.linalg.expm()对于李群SE(3)、SO(3),和与其对应的李代数se(3),so(3)。指数映射是十分重要的。在Python中我们可以调用:scipy.linalg.expm()来将李代数$\xi $对应的反对称矩阵${\hat \xi }$映射到其对应的旋转矩阵$R$。源码示例import scipy.linalg.expm as expmdef SkewFun(a): """ g...
Python中李群SO(3)
与李代数so(3)
之间指数映射与实现源码
调用scipy.linalg.expm()
对于李群SE(3)
、SO(3)
,和与其对应的李代数se(3)
,so(3)
。指数映射是十分重要的。
在Python中我们可以调用:scipy.linalg.expm()
来将李代数
ξ
\xi
ξ对应的反对称矩阵
ξ
^
{\hat \xi }
ξ^映射到其对应的旋转矩阵
R
R
R。
源码示例
import scipy.linalg.expm as expm
def SkewFun(a):
"""
got the corresponded antiSymmetric Matrix of the Lie algebra
:param a: Lie algebra
:return: antiSymmetric Matrix
"""
if len(a) == 3:
A = np.array([[0, -a[2], a[1]],
[a[2], 0, -a[0]],
[-a[1], a[0], 0]
])
return A
if len(a) == 2:
A = np.array([a[1], -a[0]])
return A
exit(-1)
def so3ToSO3(xi):
return expm(SkewFun(xi)
如果觉得文章对您有帮助,可以点个赞,是对作者最大的鼓励。
本文地址:https://blog.csdn.net/weixin_43977640/article/details/109922132