Sophus库：李群和李代数

1. 准备工作

1.1 Sophus库简介

Sophus库是基于Eigen库开发的，用于李群和李代数的运算。下面是一些常用的头文件：

//三维
se3.hpp
so3.hpp
sim3.hpp
//二维
se2.hpp
so2.hpp
sim2.hpp

1.2 Sophus库下载

git clone https://github.com/strasdat/Sophus.git
cd Sophus
mkdir build 
cd build
cmake ..
make
sudo make install

1.3 CMakeLists.txt

CMakeList添加下面的语句：

find_package(Sophus REQUIRED)
include_directories( ${Sophus_INCLUDE_DIRS} )

target_link_libraries(程序名 Sophus::Sophus)

2. SO3的构造

//默认，Identity
Sophus::SO3d SO3_I;
//旋转矩阵
Sophus::SO3d SO3_R(R);
//四元数
Sophus::SO3d SO3_q(q);
//其他SO3
Sophus::SO3d SO3_other(SO3_R);

3. SE3的构造

//默认，旋转矩阵Identity + 零平移
Sophus::SE3d SE3_I;
//旋转矩阵和平移向量
Sophus::SE3d SE3_Rt(R,t);
//四元数和平移向量
Sophus::SE3d SE3_qt(q,t);    
//其他SE3
Sophus::SE3d SE3_other(SE3_Rt);

4. 指数映射和对数映射

1、SO3和so3

//使用对数映射从SO3获得对应的so3
Vector3d so3 = SO3_R.log();
//使用指数映射从so3中获得对应的SO3
Vector3d update_so3(1e-4,0,0);
Sophus::SO3d updated_SO3 = Sophus::SO3d::exp(update_so3);

2、SE3和se3

//使用对数映射从SE3获得对应的se3
Vector6d se3 = SE3_Rt.log();
//使用指数映射从se3中获得对应的SE3
Vector6d update_se3; //更新量
update_se3.setZero();
update_se3(0,0) = 1e-4;
Sophus::SE3d updated_SE3 = Sophus::SE3d::exp(update_se3);

5. 增量扰动模型的更新

1、SO3

//增量扰动模型的更新, 添加一个小的so3的扰动，然后通过指数映射反映到SO3上
Vector3d update_so3(1e-4,0,0);
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
cout<<"SO3 updated = \n"<<SO3_updated.matrix()<<endl;

2、SE3

//最后，演示SE3的扰动模型的更新
Vector6d update_se3; //更新量
update_se3.setZero();
update_se3(0,0) = 1e-4;
Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
cout<<"SE3 updated = \n"<<SE3_updated.matrix()<<endl;

6. 向量和反对称矩阵

1、so3

//使用对数映射从李群中获得对应的李代数
Vector3d so3 = SO3_R.log();
cout<<"so3 = "<<so3.transpose()<<endl;
//hat 为向量到反对称矩阵, hat是SO3d的hat
cout<<"so3 hat=\n"<<Sophus::SO3d::hat(so3)<<endl;
//vee 为反对称矩阵到向量, vee是SO3d的vee
cout<<"so3 hat vee=\n"<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;

2、se3

//同样的，SE3有对应的hat和vee
cout<<"se3 hat = \n"<<Sophus::SE3d::hat(se3)<<endl;
cout<<"se3 hat vee = \n"<<Sophus::SE3d ::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;

总结：SO3，so3，SE3和se3初始化以及相互转换关系：

7. 完整代码

下面是李群和李代数的简单代码useSophus.cpp

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

using namespace std;
using namespace Eigen;

int main() {
    //沿着Z轴旋转90度的旋转矩阵
    Matrix3d R = AngleAxisd(M_PI/2,Vector3d(0,0,1)).toRotationMatrix();
    //或者四元数
    Quaterniond q(R);

    //用旋转矩阵或四元数定义李群SO3
    Sophus::SO3d SO3_R(R);
    Sophus::SO3d SO3_q(q);
    //两者是等价的
    cout<<"SO3 from matrix:\n"<<SO3_R.matrix()<<endl;
    cout<<"SO3 from quaternion\n"<<SO3_q.matrix()<<endl;
    cout<<"the are equal"<<endl;

    //使用对数映射从李群中获得对应的李代数
    Vector3d so3 = SO3_R.log();
    cout<<"so3 = "<<so3.transpose()<<endl;
    //hat 为向量到反对称矩阵, hat是SO3d的hat
    cout<<"so3 hat=\n"<<Sophus::SO3d::hat(so3)<<endl;
    //vee 为反对称矩阵到向量, vee是SO3d的vee
    cout<<"so3 hat vee=\n"<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;

    //增量扰动模型的更新, 添加一个小的so3的扰动，然后通过指数映射反映到SO3上
    Vector3d update_so3(1e-4,0,0);
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
    cout<<"SO3 updated = \n"<<SO3_updated.matrix()<<endl;

    cout<<"**************************"<<endl;

    //对SE3操作大同小异
    Vector3d t(1,0,0); //沿x轴平移1
    Sophus::SE3d SE3_Rt(R,t); // 从R,t构造SE3
    Sophus::SE3d SE3_qt(q,t); // 从q,t构造SE3
    cout<<"SE3 from R,t=\n"<<SE3_Rt.matrix()<<endl;
    cout<<"SE3 from q,t=\n"<<SE3_qt.matrix()<<endl;
    //李代数se3是一个六维向量，为了方便先typedef一下
    typedef Matrix<double,6,1> Vector6d;
    //定义SE3对应的李代数se3
    Vector6d se3 = SE3_Rt.log();
    cout<<"se3 = \n"<<se3.transpose()<<endl;
    //观察输出，会发现Sophus中，se3的平移在前，旋转在后
    //同样的，SE3有对应的hat和vee
    cout<<"se3 hat = \n"<<Sophus::SE3d::hat(se3)<<endl;
    cout<<"se3 hat vee = \n"<<Sophus::SE3d ::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;

    //最后，演示SE3的扰动模型的更新
    Vector6d update_se3; //更新量
    update_se3.setZero();
    update_se3(0,0) = 1e-4;
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
    cout<<"SE3 updated = \n"<<SE3_updated.matrix()<<endl;

    return 0;
}

下面是对应的CMakeLists.txt

cmake_minimum_required(VERSION 3.22)

project(ch4)

include_directories("/usr/include/eigen3")
find_package(Sophus REQUIRED)
include_directories( ${Sophus_INCLUDE_DIRS} )

add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus Sophus::Sophus)