第四讲李群和李代数2

第一部分：SO(3)和so(3)

1、SO(3)对应的李代数 $\large so(3)$

SO(3)对应的李代数是定义在 $\large \mathbb{R}^{3}$ 上向量，我们记作 $\large \phi$ 。根据上一篇博客中的推导，可以知道，每个 $\large \phi$ 都可以生成一个反对称矩阵：

$\large \Phi =\phi ^{\Lambda }=\begin{bmatrix} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{bmatrix} \epsilon \mathbb{R}^{3*3}$

在此定义下，两个向量 $\large \phi_{1},\phi_{2}$ 的李括号（运算）为：

$\large [\phi_{1},\phi_{2}]=[\Phi_{1}\Phi_{2}-\Phi_{2}\Phi_{1}]^{\vee }$

由于 $\large \phi$ 和反对称矩阵关系很紧密，在不引起歧义的情况下，可以说 $\large so(3)$ 的元素是三维向量或者三维反对称矩阵，即：

$\large so(3)= \begin{Bmatrix} \phi\epsilon \mathbb{R}^{3},\Phi =\phi^{\Lambda }\epsilon \mathbb{R}^{3*3} \end{Bmatrix}$

到此，我们已经清楚了SO(3)对应的李代数 $\large so(3)$ 的内容，它们是一个由三维向量组成的集合，其中每个向量都可以对应到三维反对称矩阵，可以表达旋转矩阵的导数。它和SO(3)的关系由指数映射给定： $\large R=exp(\phi^{\Lambda })$

2、SO(3)上的指数映射

由上面的内容可以知道，SO(3)中的每个元素都有对应的李代数，它们之间的关系由指数映射给定： $\large R=exp(\phi^{\Lambda })$ 。那么它具体应该怎么去计算呢？

（1）再进入真正的计算公式的推导之前，首先来看一下如何利用Sophus库来进行李群和李代数的运算，因为这比较简单。

Eigen提供了几何模块，但是没有提供对李代数的支持。Sophus是一个较好的李代数库，它是Strasdat维护的，基于Eigen开发的，能够较好的支持SO(3)、SE(3)，此外还能够支持SO(2)、SE(2)。

关于Sophus库的配置在百度上可以找到很多教程，这里不记录了。下面是一个调用Sophus库进行李代数运算的例子。

#include <iostream>
//Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
//Sophus
#include "sophus/so3.h"

using namespace std;

int main(int argc, char **argv)
{
    //定义一个沿Z轴转90度的旋转
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();

    //可以直接从旋转矩阵构造SO（3）
    Sophus::SO3 SO3_R(R);

    //也可以直接从旋转向量构造SO（3）
    Sophus::SO3 SO3_V(0, 0, M_PI/2);

    //也可以从四元数构造SO（3）
    Eigen::Quaterniond q(R);
    Sophus::SO3 SO3_Q(q);

    //输出SO（3）的时候，是以so(3)的形式显示的
    cout << "SO(3) from rotation_matrix : " << SO3_R << endl;
    cout << "SO(3) from rotation_vector : " << SO3_V << endl;
    cout << "SO(3) from quaternion : " << SO3_Q << endl;

    //SO（3）---> so(3)   对数映射
    Eigen::Vector3d so3 = SO3_R.log();
    cout << "SO3_R.log() = " << so3 << endl;

    //hat ---> 从三维向量到三维反对称矩阵的运算
    cout << "so3 hat = " << Sophus::SO3::hat(so3) << endl;

    //vee ---> 从三维反对称矩阵到三维向量
    cout << "so3 hat vee = "<< Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl;

    return 0;
}

下面是CMakeLists.txt的内容：

cmake_minimum_required(VERSION 3.5)

project(use_sophus)

find_package(Sophus REQUIRED)

include_directories(
    ${Sophus_INCLUDE_DIRS}
)

add_executable(useSophus 
    useSophus.cpp
)

target_link_libraries(useSophus 
    ${Sophus_LIBRARIES}
)

（2）数学上的计算推导

通过上面的推导得到了计算 $\large R=exp(\phi^{\Lambda })$ 的公式：

$\exp(\phi ^{\Lambda }) = \exp(\theta a^{\Lambda }) =\cos\theta I+(1-\cos\theta )aa^{T}+\sin\theta a^{\Lambda }$

说明：实际上，这个公式和第3讲中讲到的“罗德里格斯公式 --- 描述旋转矩阵和旋转向量的转换过程”形式相同，这表明，so(3)实际上就是由所谓的旋转向量组成的空间，而指数映射公式即罗德里格斯公式。

3、从SO(3) ---> so(3) ：对数映射

$\phi =(\ln R)^{\vee }=[\sum_{n=0}^{\infty }\frac{(-1)^{n}}{n+1}(R-I)^{n+1}]^{\vee }$

第二部分：SE(3)和se(3)

1、SE(3)对应的李代数 $\large se(3)$

与上面相似的是SE(3)，也有对应的李代数 $\large se(3)$ 。与上面最大的不同是 $\large {\color{Red} se(3)}$ 位于 $\large {\color{Red} \mathbb{R}^6}$ 空间中， $\large {\color{Red} so(3)}$ 位于 $\large {\color{Red} \mathbb{R}^3}$ 空间中。

下面直接给出 $\large se(3)$ 的内容，如下：

对上式的说明：

（1） $\large \vec{\xi }$ 是一个6维向量

（2） $\large \vec{\xi }$ 向量的前3维 $\large \rho$ 表示平移（注意和变换矩阵中平移不同，后面会解释）

（3） $\large \vec{\xi }$ 向量的后3维 $\large \phi$ 表示旋转，实际上是 $\large so(3)$ 中的元素。

（4）在这里 $\large \Lambda$ 符号的含义不同了，在 $\large so(3)$ 中是将3维向量映射成为3*3反对称矩阵，在这里是将一个6维向量映射称为一个4*4的矩阵，并且不再表示反对称了。

在 $\large se(3)$ 中，也有类似的李括号运算：

$\large [\xi _{1},\xi _{2}]=(\xi _1^{\Lambda} \xi _2^{\Lambda }- \xi _2^{\Lambda }\xi _1^{\Lambda})^{\vee }$

2、SE(3)上的指数映射

（1）同样的，首先来看一下如何调用Sophus库来进行SE(3)和se(3)的相互运算。

#include <iostream>
//Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
//sophus
#include "sophus/se3.h"

using namespace std;

int main(int argc, char **argv)
{
    //可以从旋转矩阵，平移向量构造SE3
    Eigen::Matrix3d rotation_matrix = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();   
    Eigen::Vector3d t(1, 0, 0); 
    Sophus::SE3 SE3_RT(rotation_matrix, t);
    //也可以从四元数，平移向量构造SE3
    Eigen::Quaterniond q(rotation_matrix);
    Sophus::SE3 SE3_QT(q, t);

    cout << "SE3_RT = " << SE3_RT << endl;
    cout << "SE3_QT" << SE3_QT << endl;

    //SE3 --- se(3) : 对数映射
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    Vector6d se3 = Sophus::SE3::log(SE3_RT);
    cout << "se3 = " << se3.transpose() << endl;

    //同样的，在SE3中也有 hat 和 vee 这两个运符
    cout << "se3 hat = " << "\n" << Sophus::SE3::hat(se3) << endl;
    cout << "SE3 vee = " << "\n" << Sophus::SE3::vee(Sophus::SE3::hat(se3)) << endl;

    return 0;
}

（2）数学公式推导

这里直接给出se(3)上的指数映射公式：

$exp(\xi ^{\Lambda })=\begin{bmatrix} \sum_{n=0}^{\infty }\frac{1}{n!} (\phi ^{\Lambda })^{n}&\sum_{n=0}^{\infty }\frac{1}{(n+1)!} (\phi ^{\Lambda })^{n} \textbf{p} \\ 0^{T} & 1 \end{bmatrix} \doteq \begin{bmatrix} R & \textbf{J}\textbf{P} \\ 0^{T} & 1 \end{bmatrix} =T$

下面说一下关于 $\textbf{J}$ 的整理：

我们可以这样理解：平移部分经过指数映射之后，还发生了一次以 $\textbf{J}$ 为系数矩阵的线性变换。

（3）对数映射

这里没有给出直接的公式，但是给出了对数映射的计算思路。

关于对数和指数映射就先到这里了。下一篇会开始李代数求导和扰动模型的学习。