视觉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。

左乘该李代数对应的李群，即扰动模型。最后输出扰动模型的矩阵形式。