1. 三维旋转矩阵构成的特殊正交群 $SO(3)$

    $$ S O ( 3 ) = \left\{ R \in \mathbb R ^ { n \times n } \mid R R ^ T = 1 , \operatorname { det } ( R ) = 1 \right\} $$

  2. 变换矩阵构成的特殊欧式群 $SE(3)$

    $$ S E ( 3) = \left\{ T = \left[ \begin{array} { c c } R & t \\ 0 ^ { T } & 1 \end{array} \right] \in \mathbb R ^ { 4 \times 4 } \mid R \in S O ( s ) , t \in \mathbb R ^ { 3 } \right\} $$

李群的定义

群

群 - 维基百科，自由的百科全书

群是一种集合加上一种运算的代数结构, 若把集合记为 $A$, 运算记为 $\cdot$ , 则这个群记为 $G = (A, \cdot)$ , 并且要求满足以下性质

  1. 封闭性: 对于所有 $G$ 中 $a, b$，运算 $a·b$ 的结果也在 $G$ 中。
  2. 结合律: 对于所有 $G$ 中的 $a, b$ 和 $c$，等式 $(a·b)·c = a· (b·c)$ 成立。
  3. 单位元: 存在 $G$ 中的一个元素 $e$ ，使得对于所有 $G$ 中的元素 $a$，总有等式 $e·a = a·e = a$ 成立。
  4. 逆元: 对于每个 $G$ 中的 $a$，存在 $G$ 中的一个元素 $b$ 使得总有 $a·b = b·a = e$，此处 $e$为单位元。

李群

粗略地说，李群是连续的群，也即其元素可由几个实参数描述。

李代数的定义

李代数由一个集合 $\mathbb V$, 一个数域 $\mathbb F$, 和一个二元运算符 $[,]$ 组成, 若其满足以下几条性质, 则称 $(\mathbb V, \mathbb F, [,])$ 为一个李代数

  1. 封闭性: $\forall X, Y \in \mathbb V, [X, Y] \in \mathbb V$
  2. 双线性: $\forall X, Y, Z \in \mathbb V, a, b \in \mathbb F, 有\\ \left[aX+bY, Z \right]=a[X,Z]+b[Y,Z], [Z,aX+bY]=a[Z,X]+b[Z,Y]$
  3. 自反性 $\forall X\in \mathbb V, [X,X]=0$
  4. 雅可比等价 $\forall X, Y, Z \in \mathbb V, [X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$

李代数 $\mathfrak{so}(3)$

$SO(3)$ 在 $\mathbb R ^3$ 上的向量, 记作 $\phi = [\phi_1, \phi_2, \phi_3]^T$.

$$ \Phi = \phi^\wedge = \left[\begin{array}{c} 0&-\phi_3&\phi_2 \\ \phi_3&0&-\phi_1 \\ -\phi_2&\phi_1&0\end{array} \right] $$

在此定义下, 两个向量 $\phi_1, \phi_2$ 的李括号运算定义为: $[\phi_1, \phi_2] = (\Phi_1\Phi_2-\Phi_2\Phi_1)^\vee$

李代数 $\mathfrak{se}(3)$

$\mathfrak{se}(3)$ 位于 $\mathbb R^ 6$ 空间中, $\rho$ 表示平移, $\phi$ 表示旋转

$$ \mathfrak{se}(3) = \left\{ \xi = \left[ \begin{array}{c}\rho\\\phi \end{array}\right]\in \mathbb R^6, \rho \in \mathbb R ^3, \phi \in \mathfrak{so}(3), \xi ^{\wedge} = \left[ \begin{array}{c} \phi^\wedge & \rho \\ 0^T & 0\end{array}\right] \in \mathbb R^{4 \times 4}\right\} $$

李代数求导与扰动模型

李代数中完成矩阵乘法

李代数的求导

Sophus 实践

Sophus 的安装

git clone <https://github.com/strasdat/Sophus.git>
cd Sophus
git checkout a621ff

// 编译
mkdir build
cd build
cmake ..
make

在Cmakelist.txt中添加

find_package(Sophus REQUIRED)

include_directories( ${Sophus_INCLUDE_DIRS})
add_executable(useSophus useSophus.cpp) 
target_link_libraries(useSophus ${Sophus_LIBRARIES})

//
// Created by fjczx on 2021/8/29.
//

#include <iostream>

#include "Eigen/Core"
#include "Eigen/Geometry"
#include "sophus/so3.h"
#include "sophus/se3.h"

using namespace std;

int main(){
    // 以旋转向量方式初始化旋转矩阵R
    Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();

    // 用旋转矩阵构建SO3
    Sophus::SO3 SO3_R(R);
    // 用向量直接构建SO3
    Sophus::SO3 SO3_v(0, 0, M_PI/2);
    // 从四元数构建SO3
    Eigen::Quaterniond q(R);
    Sophus::SO3 SO3_q(q);

    // 它们的输出都是一致的
    // 注意: 当输出 SO(3) 的时候, 将会以 so3 的形式输出
    cout << "SO(3) from matrix: \n" << SO3_R << endl;
    cout << "SO(3) from vector: \n" << SO3_v << endl;
    cout << "SO(3) from quaternion: \n" << SO3_q << endl;
    // 使用对数映射来获取它的李代数
    Eigen::Vector3d so3 = SO3_R.log();
    cout << "so3: \n" << so3.transpose() << endl;

    // 旋转向量所对应的反对称阵
    cout << "so3^{\\wedge}: \n " << Sophus::SO3::hat(so3) << endl;
    // 反对称矩阵所对应的特征向量
    cout << "so3^{\\wedge}^{\\vee}: \n" << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl;

    // 增量扰动模型
    Eigen::Vector3d update_so3(1e-4, 0, 0);
    Sophus::SO3 SO3_update = Sophus::SO3::exp(update_so3)*SO3_R;
    cout << "SO3_update: \n" << SO3_update << endl;

    /*
     * 如下是对SE3的操作
     */
    Eigen::Vector3d t(1, 0, 0);
    // 从旋转矩阵和平移来构建SE(3)
    Sophus::SE3 SE3_Rt(R, t);
    // 使用四元数和平移来构建SE(3)
    Sophus::SE3 SE3_qt(q, t);
    cout << "SE(3) from R,t: \n" << SE3_Rt << endl;
    cout << "SE(3) from q,t: \n" << SE3_qt << endl;

    // 由于李代数 se(3) 是一个六维向量, 所以先定义一个六维向量类型
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    // 输出李代数 se(3), 输出的时候平移部分在前面, 旋转部分在后面
    cout << "se3: \n" << se3.transpose() << endl;
    // 将李代数 se(3) 转化为响应的反对称矩阵
    cout << "se3^{\\wedge}: \n" << Sophus::SE3::hat(se3) << endl;
    cout << "se3^{\\wedge}^{\\vee}: \n" << Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose() << endl;

    // 定义更新量
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0, 0) = 1e-4;
    Sophus::SE3 SE3_update = Sophus::SE3::exp(update_se3)*SE3_Rt;
    cout << "SE3 updated: \n" << SE3_update << endl;

    return 0;

}