基于python的李代数实现方法

---

前言

有好久没更新博客，比较惭愧，这次索性给出一个深入一点的文章——在python中实现李代数。

一、李代数是什么？

简而言之，李代数可以用来描述物体在某个参考系下的状态（姿态或者位姿），可以用一个多维（三维或者六维）矢量来表示。说到李代数，就不得不提到李群，每个李群都有与之对应的李代数。懂的人都懂，我就不详细阐述了，对于做slam的人，这都是基础知识。
在此祭出《视觉slam十四讲》中的一个关系图：

在C++中有专用的sophus库，可以非常方便地调用李代数。在python中，该如何使用李代数呢？笔者在github上搜到了一个开源库，基于numpy定义了李代数，且实现了与李群的转换，使用非常方便，因此在此贡献出来。
https://github.com/franciscodominguezmateos/pySophus

二、源码解析

以旋转矩阵对应的李代数so3为例：

代码如下：

class so3(Algebra):
    """3D rotation Lie algebra"""
    G1 = np.matrix([[0, 0, 0],
                    [0, 0, -1],
                    [0, 1, 0]])
    G2 = np.matrix([[0, 0, 1],
                    [0, 0, 0],
                    [-1, 0, 0]])
    G3 = np.matrix([[0, -1, 0],
                    [1, 0, 0],
                    [0, 0, 0]])

    def __init__(self, **kwargs):
        """Algebra element constructor
        :param kwargs: must be `matrix = ndarray` with dimensions 3x3 or `vector = ndarray` with dimensions 1x3
        :type kwargs
        :raises TypeError, if the needed argument (matrix or vector) is not given
        """
        if "vector" in kwargs:
            self.w = kwargs["vector"]
        elif "matrix" in kwargs:
            m = kwargs["matrix"]
            self.w = np.array([0, 0, 0])
            self.w[0] = m[2, 1]
            self.w[1] = m[0, 2]
            self.w[2] = m[1, 0]
        else:
            raise TypeError("Argument must be matrix or vector")

    def __add__(self, other):
        """Returns the algebra element for the equivalent rotation as applying the given ones one after the other
        :param other: element to add this element to
        :type other: so3
        :return: equivalent algebra element
        :rtype: so3
        """
        R1 = self.exp()
        R2 = other.exp()
        R = R1 * R2
        return R.log()

    def magnitude(self):
        """Given the vector w=(wx,wy,wz) returns its norm, which is how many radians this rotation makes around the normalized vector
        :return: radians this rotation makes
        :rtype: float
        """
        return np.linalg.norm(self.w)

    def exp(self):
        """
        :return: group element associated with this algebra element
        :rtype: SO3
        """
        wx = self.matrix()
        theta = np.linalg.norm(self.w)
        cs = np.cos(theta)
        sn = np.sin(theta)
        I = np.eye(3)
        a = (sn / theta) if theta != 0 else 1
        b = ((1 - cs) / theta ** 2) if theta != 0 else 1 / 2.0
        R = I + a * wx + b * wx.dot(wx)
        return SO3(R)

    def vector(self):
        """
        :return: this algebra element represented as vector (1x3)
        :rtype: ndarray
        """
        return self.w

    def matrix(self):
        """
        :return: this algebra element represented as skew matrix (3x3)
        :rtype: ndarray
        """
        return so3.G1 * self.w[0] + so3.G2 * self.w[1] + so3.G3 * self.w[2]

1. __init__

定义了李代数so3的实现方式：vector或者matrix，分别表示可以通过一个三维向量或者其反对称矩阵来生成李代数。

2. __add__

两个so3相加，等于其对应的李群(exp)相乘，然后再变成李代数(log)。

3. magnitude

给出对应的旋转角度。

4. exp

将李代数变成对应的李群，即：旋转矩阵R。

5. vector

给出李代数的矢量形式。

6. matrix

给出李代数的反对称矩阵形式。

三、二次开发

该库并没有实现李代数或者李群与四元数的转换，在此笔者自己增加了该内容，如下所示：

    def quaternion(self):
        """
        :return:  this algebra element represented as quaternion (1x4)
        """
        theta = np.linalg.norm(self.w[:3])
        u = self.w[:3] / theta
        q = np.zeros(4)
        q[0] = np.cos(0.5 * theta)
        q[1] = u[0] * np.sin(0.5 * theta)
        q[2] = u[1] * np.sin(0.5 * theta)
        q[3] = u[2] * np.sin(0.5 * theta)
        return q

基于so3给出了其到四元数的转换。方法非常简单，利用旋转向量来定义四元数：
$$q=[cos\frac{\theta }{2},\vec{n}sin\frac{\theta }{2}]$$

总结

本文分享了一种在python中实现李代数的方法，源码简单明了，只依赖于numpy库，非常实用，也便于在次基础上做二次开发。