1. 李群李代数简介

2. Sophus库简介

Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。但是Eigen库提供了集合模块，但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库，它很好的支持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间。

using Transformation = Matrix;
using Point = Vector3;
using HomogeneousPoint = Vector4;
using Line = ParametrizedLine3;
using Hyperplane = Hyperplane3;
using Tangent = Vector;
using Adjoint = Matrix;

此外，为了方便说明SE(4)和se(4)，Sophus库还typedef了Vector4d、Matrix4d、Vector6d和Matrix6d等，即：

Sophus::Vector4d
Sophus::Matrix4d
Sophus::Vector6d
Sophus::Matrix6d

3. 安装方式

git clone https://github.com/strasdat/Sophus.git
cd Sophus
mkdir build 
cd build
cmake ..
make -j

4. CMake编写

CMAKE_MINIMUM_REQUIRED( VERSION 2.8)
PROJECT(useSophus)
FIND_PACKAGE(Sophus REQUIRED)
INCLUDE_DIRECTORIES(${Sophus_INCLUDE_DIRS})
add_executable(useSophus useSophus.cpp)
TARGET_LINK_LIBRARIES(useSophus ${Sophus_LIBRARIES})

5. 头文件

#include  //导入eigen库的核心组件
#include  //导入eigen库的几何组件
 
#include "sophus/so2.hpp" //导入sophus库的so2头文件
#include "sophus/se2.hpp" //导入sophus库的se2头文件
#include "sophus/sim2.hpp" //导入sophus库的sim2头文件
 
#include "sophus/so3.hpp" //导入sophus库的so3头文件
#include "sophus/se3.hpp" //导入sophus库的se3头文件
#include "sophus/sim3.hpp" //导入sophus库的sim3头文件

6. SO3，so3，SE3和se3初始化以及相互转换关系

7. 李代数so3

Sophus::Vector3d //因为so(3)仅仅只是一个普通的3维向量

8. 李群SO3的构造函数

SO3();
SO3(const SO3& other);
explicit SO3(const Matrix3d& _R); // 使用旋转矩阵初始化
explicit SO3(const Quaterniond& unit_quaternion); // 使用四元数初始化
SO3(double rot_x, double rot_y, double rot_z); // 使用轴角初始化

9. SO3与so3的相互转换

Eigen::Vector3d so3_V1 = SO3_V1.log();//so(3) // 在Sophus(Eigen)中用vector3d表示,使用对数映射获得李群对应的李代数
 
Sophus::SO3 SO3_V2 = Sophus::SO3::exp(so3_V1); //使用指数映射将李代数转化为李群
 
Eigen::Matrix3d M_so3_V1 = Sophus::SO3::hat(so3_V1);//hat为向量到其对应的反对称矩阵
 
Eigen::Vector3d V_M = Sophus::SO3::vee(M_so3_V1);//vee为反对称矩阵对应的向量

10. SO3增量扰动模型

Eigen::Vector3d update_so3(1e-4,0,0);//假设更新量为这么多
Eigen::Matrix3d update_matrix=Sophus::SO3::exp(update_so3).matrix();//将李群转换为旋转矩阵
cout<<"SO3 update Matrix=\n"<
 
Sophus::SO3 SO3_updated=Sophus::SO3::exp(update_so3)*SO3_R;
cout<<"SO3 updated = \n"<
 
Eigen::Matrix3d SO3_updated_matrix=SO3_updated.matrix();//将李群转换为旋转矩阵
cout<<"SO3 updated Matrix = \n"<

11. 李代数se3 

Sophus::Vector6d //因为se(3)仅仅只是一个普通的6维向量

12. 李群SE3的构造函数

SE3();
SE3(const SO3& so3, const Vector3d& translation);
SE3(const Matrix3d& rotation_matrix, const Vector3d& translation);
SE3(const Quaterniond& unit_quaternion, const Vector3d& translation_);
SE3(const SE3& other);

13. SE3与se3的相互转换

Sophus::Vector6d se3_Rt=SE_Rt.log(); //se(3)在Sophus中用Vector6d表示,使用对数映射获得李群对应的李代数
 
Sophus::SE3 SE3_Rt2=Sophus::SE3::exp(se3_Rt); //使用指数映射将李代数转化为李群
 
Sophus::Matrix4d M_se3_Rt=Sophus::SE3::hat(se3_Rt);
 
Sophus::Vector6d V_M_se3=Sophus::SE3::vee(M_se3_Rt);

14. SE3增量扰动模型

Sophus::Vector6d update_se3=Sophus::Vector6d::Zero();
update_se3(0)=1e-4d;
 
cout<<"update_se3\n"<transpose()<
 
Eigen::Matrix4d update_matrix2=Sophus::SE3::exp(update_se3).matrix();//将李群转换为旋转矩阵
cout<<"update matrix=\n"<
 
Sophus::SE3 SE3_updated=Sophus::SE3::exp(update_se3)*SE3_Rt2;
cout<<"SE3 updated=\n"<
 
Eigen::Matrix4d SE3_updated_matrix=SE3_updated.matrix();//将李群转换为旋转矩阵
cout<<"SE3 updated Matrix=\n"<

15. SO3, SE3和se3的输出说明

尽管SO3对应于矩阵群，但是SO3在使用cout时是以so3形式输出的，输出的是一个3维向量；

SE3在使用cout输出时输出的是一个6维向量，其中前3维为对应的so3的值，后3维为实际的平移向量t；

se3在使用cout输出时输出的也是一个6维向量，但是其前3维为平移值ρρ（注意此时的ρρ与SE3输出的t是不同的，t=Jρρ,其中J是雅克比矩阵），后3维为其对应的so3.

16. 插值

#include "sophus/interpolate.hpp"
 
Sophus::SE3f c = Sophus::interpolate(a, b, 0.5);

参考文献

Sophus库的安装和使用教程（转载）_山城盛夏的博客-程序员ITS401_sophus库 - 程序员ITS401

Sophus源码逐行解读 ( 结合视觉SLAM十四讲的概念 ) - 掘金