Python中李群SO(3)与李代数so(3)之间指数映射与实现源码

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 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)

---

如果觉得文章对您有帮助，可以点个赞，是对作者最大的鼓励。