Sophus库编程基础Sophus编程开发的基本概念介绍. 包括如何定义SO3/SE3/so3/se3的李群李代数, 包

作者：边城量子 ( shihezichen@live.cn )

简介

本文主要介绍Sophus库的基本函数和调用代码。

Eigen 库是C++线性代数库,提供了快速的矩阵线性代数运作,以及解方程等功能
Sophus 库是李群/李代数库, 基于 Eigen 库基础上开发的, 它可以很好的支持 SO(3)/SE(3)/so(3)/se(3)

说明：

可通过本文和Sophus库源码讲解两篇帖子，深入理解Sophus库所代表的数学概念。
Sophus库的库函数的使用，表面上就是一些既有类和函数的调用，和调用其他类库并无不同，因此本质在于理解其背后所代表的数学概念。
在讲解Sophus库的使用之前，先回顾以下所涉及到的基本数学概念。

第一部分： Sophus库涉及到的基本概念回顾

1. 李群、李代数基本概念

李群 $\mathcal L$ 是一个群同时也是一个光滑的(可微的)的流形, 定义其上的群乘法和求逆运算均可微;
李代数 $\mathcal A$ 是 $\mathcal L$ 的在单位元 $\mathcal 1$ 处的正切空间;
最常见的李群就是 $SO(n)$ 和 $SE(n)$
李群/李代数/正切向量的映射关系: 其中 $\mathbb R^n$ 代表n维向量空间
- $\ \wedge: \mathbb R^n \longrightarrow \mathcal A$ : 把正切向量映射到李代数 $\mathcal A$ 中的元素
- $\exp (\phi^\wedge): \mathbb R^n \longrightarrow \mathcal L$ : 把正切向量 $\phi$ 映射到李代数元素, 然后再映射到李群中的元素
- $\vee: \mathcal A \longrightarrow \mathbb R^n$ : $\ ^\wedge$ 的逆运算, 把李代数中的元素映射到正切向量
- $\ln(R)^\vee: \mathcal L \longrightarrow \mathbb R^n$ : $\exp$ 的逆运算, 把李群中的元素映射到李代数中的元素, 然后再映射到正切向量

2. Sophus库的数据结构表示

在Sophus库中是如何表示如上的基本概念的呢？

旋转矩阵用来描述三维空间刚体运动
李群 $SO(3)$ : 3x3旋转矩阵
李代数 $so(3)$ : 3x1三维向量
李群 $SE(3)$ : 4x4变换矩阵
李代数 $se(3)$ : 6x1六维向量, 平移在前, 旋转在后

3. 参考阅读

A micro Lie theory for state estimation robotics, JohnSola ：前半部分详细介绍了李群、李代数基本概念，以及伴随，同时对李群微分四种形式均做了定义；

第二部分： Sophus库函数使用场景总览

具体Sophus库的使用例子，在 视觉SLAM十四讲 中已经有一个现成的例子，此处不再做代码搬运。下面对Sophus库的使用场景做一个汇总总结，并附上代码片段。

场景总览

ID	场景	方法
1	根据轴角定义旋转矩阵 R	Eigen旋转角.toRotationMatrix()
2	旋转矩阵 R -> SO3	Sophus::SO3d SO3_rotation_matrix(rotation_matrix)
3	对数映射 SO3 -> so3 : log()	Eigen::Vector3d so3 = SO3_rotation_matrix.log()
4	指数映射 so3 -> SO3 : exp()	Sophus::SO3d SO3_rotation_matrix1 = Sophus::SO3::exp(so3)
5	李代数so3 -> 反对称矩阵: hat()	Sophus::SO3d::hat(so3)
6	反对称矩阵 -> 李代数so3: vee()	Sophus::SO3d::vee(Sophus::SO3d::hat(so3))
7	旋转矩阵R+平移t -> SE3	Sophus::SE3d SE3_rotation_translation(rotation_matrix, t)
8	对数映射 SE3 -> se3 : log()	Vector6d se3 = SE3_rotation_translation.log()
9	指数映射 se3 -> SE3 : exp()	Sophus::SE3d SO3_rotation_matrix2 = Sophus::SE3::exp(se3)
10	李代数se3 -> 反对称矩阵 : hat()	Sophus::SE3d::hat(se3)
11	反对称矩阵 -> 李代数se3 ： vee()	Sophus::SE3d::vee(Sophus::SE3d::hat(se3))

第三部分： Sophus库函数使用代码片段

1. 根据轴角定义旋转矩阵 :

用沿Z轴转90度的Eigen旋转向量构造Eigen的旋转矩阵

// 旋转向量: 第一个参数为旋转角度，第二个参数哪个为1就绕哪轴旋转
Eigen::Matrix3d rotation_matrix = Eigen::AngleAxisd( 
                    M_PI / 4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();
cout << "用沿Z轴转90度的Eigen旋转向量构造Eigen旋转矩阵:\n" << rotation_matrix << endl;

实际上Sophus库也定义了自己的 SO3d 数据结构(也是基于Eigen)，位于自己的命名空间.

const double Pi = Sophus::Constants<double>::pi();
Sophus::SO3d R3 = Sophus::SO3d::rotZ( Pi / 4);
cout << "用沿Z轴转90度的SO3d构造Eigen旋转矩阵:\n" << R3.matrix() << endl;

相比之下 Eigen 更强大一些。

2. 旋转矩阵 R -> SO3 :

由Eigen旋转矩阵到李群旋转矩阵 $SO3$ :

用Eigen的旋转矩阵, 构造Sophus李群旋转矩阵 $SO3$
$SO3$ 旋转矩阵仍然可以和 Eigen 的Vector向量相乘
$SO3$ 旋转矩阵可以得到一个单位四元数

Sophus::SO3d SO3_rotation_matrix(rotation_matrix);
cout << "用Eigen的旋转矩阵构造Sophus李群旋转矩阵SO3:\n" 
     << SO3_rotation_matrix.matrix() << endl;
Eigen::Vector3d x; 
x << 0.0, 0.0, 1.0;
cout << "SO3*x\n" << SO3_rotation_matrix * x << endl;
cout << "uniq auaternion:\n" 
     << SO3_rotation_matrix.unit_quaternion().coeffs() << endl;

3. 对数映射 SO3 -> so3 : log()

由李群 $SO3$ 到李代数 $so3$ :

通过对数映射, 求李群 $SO3$ 的李代数 $so3$

Eigen::Vector3d so3 = SO3_rotation_matrix.log();
cout << "对数映射求李群SO3的李代数so3 = " << so3.transpose() << endl;

4. 指数映射 so3 -> SO3 : exp()

由李代数 $so(3)$ 到李群 $SO3$

通过指数映射, 求李代数 $so3$ 的李群旋转矩阵 $SO3$

Sophus::SO3d SO3_rotation_matrix1 = Sophus::SO3d::exp(so3);
cout << "指数映射求李代数so3的李群SO3 = \n" << SO3_rotation_matrix1.matrix() << endl;

5. 李代数so3 -> 反对称矩阵: hat()

由李代数 $so3$ 向量的hat, 得到对应的旋转反对称矩阵

通过 $so3$ 的hat()运算, 求向量的反对称矩阵

cout << "hat 求向量到反对称矩阵 =\n" << Sophus::SO3d::hat(so3) << endl;

6. 反对称矩阵 -> 李代数so3: vee()

通过 $so3$ 的vee()运算, 求反对称矩阵的向量

cout << "vee 求反对称矩阵到向量 = " << Sophus::SO3d::vee(Sophus::SO3::hat(so3)).transpose() << endl;

7. 旋转矩阵R+平移t -> SE3:

通过旋转矩阵R和平移向量t构造Sophus的李群变换矩阵 $SE3$

// 平移向量: 沿X轴平移1
Eigen::Vector3d t(1, 0, 0);
Sophus::SE3d SE3_rotation_translation(rotation_matrix, t);
cout << "用旋转矩阵R,平移向量t构造Sophus表示的李群变换矩阵SE3= \n" 
     << SE3_rotation_translation.matrix() << endl;

8. 对数映射 SE3 -> se3 : log()

由李群 $SE3$ 到李代数 $se3$

通过 $SE3$ 的log()运算, 求李群 $SE3$ 的李代数 $se3$
李代数 $se3$ 是一个六维向量

typedef Eigen::Matrix<double, 6, 1> Vector6d;
Vector6d se3 = SE3_rotation_translation.log();
cout << "李代数se3 是一个六维向量 李群SE3的对数映射求李代数se3 \n" << se3.transpose() << endl;

9. 指数映射 se3 -> SE3 : exp()

由李代数 $se3$ 到李群 $SE3$

通过 $se3$ 的exp() 运算, 求李代数 $se3$ 对应的李群变换矩阵 $SE3$

Sophus::SE3d SO3_rotation_matrix2 = Sophus::SE3::exp(se3);
cout << "指数映射求李代数se3的李群SE3 = \n" << SO3_rotation_matrix2.matrix() << endl;

10. 李代数se3 -> 反对称矩阵 : hat()

由李代数 $se3$ 向量的hat, 得到对应的旋转反对称矩阵

通过 $se3$ 的hat()运算, 求向量的反对称矩阵

cout << "se3 hat = \n" << Sophus::SE3::hat(se3) << endl;

第四部分： CMakeLists.txt文件

cmake_minimum_required(VERSION 3.2)

project(ProjectName)

set(CMAKE_BUILD_TYPE "Debug")

set(CMAKE_CXX_FLAGS "-std=c++14")

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS -O2 -g -Wall")

set(EXECUTABLE_OUTPUT_PATH ${PROJECT_NAME}/bin)


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


target_link_libraries(${PROJECT_NAME} ${Sophus_LIBRARIES})