视觉SLAM十四讲 Sophus库应用（李群与李代数）
1. SO3 和 SE3 的声明
（1）SO3
可以由旋转矩阵/欧拉角/四元数/声明，但是不可以由角轴声明。
具体做法：先用角轴声明旋转矩阵，再用旋转矩阵声明SO3
Eigen::AngleAxisd v_rotation (M_PI/2, Eigen::Vector3d(0,0,1));
Eigen::Matrix3d R_rotation = v_rotation.matrix();
Sophus::SO3 SO3_R(R_rotation); 如代码所示，v_rotation为角轴，R_rotation为旋转矩阵，SO3_R为声明的李群。
值得注意，不可直接用cout输出SO3，其不是矩阵形式，必须经过转换才能用cout输出，例如：
cout << "SO3 = \n" << SO3_R << endl; //错误
cout << "SO3 = \n" << SO3_R.matrix() << endl; //正确可以发现转换为矩阵形式后的输出就是旋转矩阵。
（2）SE3
同理，可以由旋转矩阵/欧拉角/四元数声明，但还需要加上平移部分。
具体做法：用旋转矩阵和平移矢量声明。
Eigen::Vector3d t (1,0,0);//平移矢量
Sophus::SE3 SE3_Rt (R_rotation,t); //从R，t （旋转矩阵和平移矢量）构造 2. so3 和 se3 的声明
so3可以由SO3取log得到。具体做法如下：
Eigen::Vector3d so3 = SO3_R.log();可以注意到，在声明so3时用的是Eigen::Vector3d 而不是 Sophus::SO3，SO3李群对应的李代数本就是一个三维矢量。
同理，se3也可以由SE3取log得到，如下：
typedef Eigen::Matrix Vector6d; //定义6维向量
Vector6d se3 = SE3_Rt.log(); se3在so3的基础上增加了三维，即对应的平移矢量部分。
3. hat 和 vee 的应用
Eigen::Matrix3d S_hat = Sophus::SO3::hat(so3); Eigen::Vector3d v_vee = Sophus::SO3::vee(S_hat); 对李代数取hat（上尖儿）得到反对称矩阵，对反对称矩阵取vee（下尖儿）得到李代数。
4. 扰动模型
（1）SO3的扰动模型
Eigen::Vector3d update_so3(1e-4,0,0);
Sophus::SO3 update_SO3 = Sophus::SO3::exp(update_so3);
//而不是 Sophus::SO3 update_SO3 = update_so3.exp()
Sophus::SO3 SO3_updated = update_SO3 * SO3_R;
cout << " so3_updated = \n" << SO3_updated.log().transpose() << endl; 首先声明扰动的李代数（1e-4,0,0），由李代数得到李群，要注意这里的方式与李群得到李代数的方式不同。
第三行，左乘扰动，得到的结果也是SO3李群中的一个，故也在Sophus命名空间中。
第四行将左乘扰动后的扰动模型取李代数并输出到屏幕。
（2）SE3的扰动模型
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; 过程与SO3的类似。
第一行 首先声明一个六维矢量，即李代数。设置李代数的前两个（？是否正确）为微小量1e-4d。
左乘该李代数对应的李群，即扰动模型。最后输出扰动模型的矩阵形式。