欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页  >  IT编程

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