文章目录
前言：这部分内容很基础很重要，得看开。
一、引入
旋转矩阵或者变换矩阵对加法是不封闭的，即两个旋转矩阵相加或者两个变换矩阵相加之后的结果并不满足旋转矩阵或者变换矩阵。如果要对关于旋转矩阵 R 的相关函数求导是无法完成的。
二、基础
1、群
群（group）是一种集合加上一种运算的代数结构。
旋转矩阵R构成特殊正交群(Special Orthogpnal Group), SO(3);
变换矩阵T构成特殊欧式群(Special Euclidean Group), SE(3)。
定义：李群是具有连续（光滑）性质的群，在空间上具有流形。
注意：由于李群对加法是非封闭的，导致求导和取极限很困难，且具有连续光滑的性质。对于三维空间流形（三维空间曲面）必定存在切空间（三维向量）（三维向量的加法是封闭的），可以利用切空间表示李群进行运算。
2、李代数
李代数描述了李群的局部性质，是单位元附近的正切空间。
李代数so(3):
理解: 若已知R对应的SO(3)原点附近的正切空间上的一3D点，指数关系可以将点映射到对应的李群空间中。同时，对于任意的R都可以找到一个与之对应的三维向量，之间的关系称为指数映射（Exponential Map）。
李代数se(3):
三、指数与对数映射
1、SO(3) 指数映射
so(3)实际上是由所谓的旋转向量组成的空间。
2、SO(3), SE(3), so(3), se(3) 的对应关系
四、李代数求导和扰动模型
1、李群乘法和李代数加法的基本认识
对旋转矩阵（李群）左乘扰动，关系：
对李代数加法，近似李群上带左右雅可比乘法：
2、导数模型和扰动模型
（1）导数模型：R 对应的李代数的变化率。导数模型有复杂的Jasobian Matrix，不希望计算。
（2）扰动模型：R 左乘或者右乘扰动，对该扰动求导。扰动模型求导结果都仅和旋转矩阵和3D点位置坐标有关。
so(3) 扰动模型：
五、激动人心的英文符号
1、Sophus的使用
SO(3)
//声明旋转矩阵或四元数
Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
Quaterniond q(R);
//分别构造李群SO(3)
Sophus::SO3d SO3_R(R);
Sophus::SO3d SO3_q(q);
//输出一样的 2.22045e-16 -1 0
// 1 2.22045e-16 0
// 0 0 1
//利用对数映射获得李代数so(3)
Vector3d so3 = SO3_R.log();
//输出 so3 = 0 0 1.5708
Sophus::SO3d::hat(so3) //hat是向量到反对称矩阵；
Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose();//vee是反对称矩阵到向量
//扰动模型
Vector3d updated_so3(1e-4, 0, 0);//李代数的扰动
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(updated_so3) * SO3_R;
//扰动的李代数转换成扰动李群再乘以原来的旋转矩阵
SE(3)
Vectored t(1, 0, 0);
//R,t 和 q,t 构造
Sophus::SE3d SE3_Rt(R, t);
Sophus::SE3d SE3_qt(q, t);
//输出，一样的：
//2.22045e-16 -1 0 1
// 1 2.22045e-16 0 0
// 0 0 1 0
// 0 0 0 1
//注意李代数se(3)是六维向量
typedef Eigen::Matrix<double, 6, 1> Vector6d;
Vector6d se3 = SE3_qt.log();
//输出，平移在前，旋转在后
//se3 = 0.785398 -0.785398 0 0 0 1.5708
//同样的hat 和 vee
Sophus::SE3d::hat(se3);
Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose();
//扰动模型
Vector6d update_se3;
update_se3.setZero();
update_se3(0,0) = 1e-4d; // 4个0构成 0.0001
Sophus::SE3d SE_updated = Sophus::SE3d::exp(update_se3) * SE3_qt;
//输出 SE3 updated =
//2.22045e-16 -1 0 1.0001
// 1 2.22045e-16 0 0
// 0 0 1 0
// 0 0 0 1
