背景知识：为什么需要李代数
首先旋转矩阵是特殊正交群;而变换矩阵是特殊欧氏群;群是一种几何加上一种运算，而特殊正交群以及特殊欧氏群的运算时确是乘，不支持加法运算，因此在优化过程中，在求导上遇见了问题。
因而我们引入了李代数的概念（具体不再介绍，反正在优化的过程中需要把李群转换为李代数，进而进行求导和优化）。特殊正交群和特殊欧氏群的之间存在着一个指数映射的关系，并如下所示：
接下来，这里介绍对李代数求导的模型：
首先是BCH公式，为求导奠定理论基础，但同时需要知道，BCH是近似公式，存在着误差。
利用李代数求导的思路分为两种，第一种是使用李代数表示姿态，然后根据李代数的加法求导;另外一种是建立左扰动和右扰动模型但第一种还是会涉及到雅可比矩阵的计算，太过于复杂，因而还是扰动模型应用较多。
对于扰动模型，有直接公式可以使用，不在介绍。
Sophus库的使用：库的使用就简单较多了。
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen / Core>
#include <Eigen / Geometry>
#include“sophus / so3.h”
#include“sophus / se3.h”
int main（int argc，char ** argv）
{
//沿Z轴转90度的旋转矩阵
Eigen :: Matrix3d R = Eigen :: AngleAxisd（M_PI / 2，Eigen :: Vector3d（0,0,1））。toRotationMatrix（）;
Sophus :: SO3 SO3_R（R）; // Sophus :: SO（3）可以直接从旋转矩阵构造
Sophus :: SO3 SO3_v（0，0，M_PI / 2）; //也可从旋转向量构造
Eigen :: Quaterniond q（R）; //或者四元数
Sophus :: SO3 SO3_q（q）;
//上述表达方式都是等价的
//输出SO（3）时，如此（3）形式输出
cout <<“SO（3）from matrix：”<
cout <<“SO（3）from vector：”<< SO3_v << endl;
cout <<“SO（3）from quaternion：”<< SO3_q << endl;
//使用对数映射获得它的李代数
Eigen :: Vector3d so3 = SO3_R.log（）;
cout <<“so3 =”<< so3.transpose（）<< endl;
// hat为向量到反对称矩阵
cout <<“so3 hat = \ n”<< Sophus :: SO3 :: hat（so3）<< endl;
//相对的，vee为反对称到向量
cout <<“so3 hat vee =”<< Sophus :: SO3 :: vee（Sophus :: SO3 :: hat（so3））.transpose（）<< endl; //转置纯粹是为了输出美观一些
//增量扰动模型的更新
Eigen :: Vector3d update_so3（1e-4，0，0）; //假设更新量为这么多
Sophus :: SO3 SO3_updated = Sophus :: SO3 :: exp（update_so3）* SO3_R;
COUT <<”
COUT << “************我是分割线*************” << ENDL;
//对SE（3）操作大同小异
Eigen :: Vector3d t（1,0,0）; //沿X轴平移1
Sophus :: SE3 SE3_Rt（R，t）; //从R，t构造SE（3）
Sophus :: SE3 SE3_qt（q，t）; //从q，t构造SE（3）
cout <<“SE3 from R，t =”<< endl << SE3_Rt << endl;
cout <<“来自q的SE3，t =”<< end1 << SE3_qt << end1;
//李代数se（3）是一个六维向量，方便起见先typedef一下
typedef Eigen :: Matrix <double，6,1> Vector6d;
Vector6d se3 = SE3_Rt.log（）;
cout <<“se3 =”<< se3.transpose（）<< endl;
//观察输出，会发现在Sophus中，se（3）的平移在前，旋转在后。
//同样的，有帽子和vee两个算符
cout <<
cout <<“se3 hat vee =”<< Sophus :: SE3 :: vee（Sophus :: SE3 :: hat（se3））.transpose（）<< endl;
//最后，演示一下更新
Vector6d update_se3; //更新量
update_se3.setZero（）;
update_se3（0,0）= 1e-4d; //对应与行和列的索引（）
Sophus :: SE3 SE3_updated = Sophus :: SE3 :: exp（update_se3）* SE3_Rt;
cout <<“SE3 updated =”<< endl << SE3_updated.matrix（）<< endl;
返回0;
}
CMakeLists.txt文件如下：
cmake_minimum_required（VERSION 2.8）
项目（useSophus）
＃为使用sophus，您需要使用find_package命令找到它
find_package（Sophus REQUIRED）
include_directories（$ {Sophus_INCLUDE_DIRS}）
add_executable（useSophus useSophus.cpp）
target_link_libraries（useSophus $ {Sophus_LIBRARIES}）
测试结果如下：