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