初学《视觉SLAM 十四讲》（高翔）--- 第四讲 李群与李代数 笔记+实践

笔记部分

实践部分

• SO(3)、SE(3)、so(3)、se(3)代码里的相互转换
  
• 代码部分

#include 
#include 
using namespace std;

#include          //Eigen线性代数库
#include 

#include "sophus/so3.h"       //Sophus李代数库
#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);
              /* SO3的常用构造函数：
               * 1、 explicit SO3 (const Matrix3d & _R);    从旋转矩阵Matrix3d构造
               * 2、 explicit SO3 (const Quaterniond & unit_quaternion);  从单位四元数构造
               * 3、 explicit SO3 (double rot_x,
               *                   double rot_y,
               *                   double tot_z);     从欧拉角构造，注意顺序，依次旋转xyz轴 */
    //上述表达方式都是等价的

    //输出SO(3)时，以so(3)形式输出
    cout << "SO(3) from matrix: " << SO3_R << endl;
    cout << "SO(3) from vector: " << SO3_v << endl;
    cout << "SO(3) from quaternion : " << SO3_q << endl;

    //使用对数映射获得它的李代数
              /* Vector3d log() const;       返回对应的李代数旋转向量
               * static Vector3d log (const SO3 &so3);      返回SO3对应的李代数 */
    Eigen::Vector3d so3 = SO3_R.log();
    cout << "so3 = " << so3.transpose() << endl;
    //hat为向量到反对称矩阵
              /* static Matrix3d hat( const Vector3d & omega );
               * 返回旋转向量对应的反对称矩阵(rot_x, rot_y, rot_z),其中rot_x、rot_y、rot_z用弧度表示 */
    cout << "so3 = " << endl << Sophus::SO3::hat(so3) << endl;
    //相对的，vee为反对称到向量
              /* static Vector3d vee( const Matrix3d & omega );
               * 返回反对称矩阵对应的旋转向量 */
    cout << "so3 hat vee = " << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl;

    //增量扰动模型的更新
    Eigen::Vector3d update_so3(1e-4, 0, 0); //假设更新量有这么多
              /* 定义扰动更新量，三维向量 update_so3 */
    Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3) * SO3_R; //左乘更新
    cout << "SO3 updated = " << SO3_updated << endl;

    /*********************萌萌的分割线**********************/
    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)
              /* SE3常用的构造函数：
               * 1、 SE3 (const SO3 & so3, const Vector3d & translation);
               *     从李群SO3+平移向量构造
               * 2、 SE3 (const Matrix3d & rotation_matrix, const Vector3d & translation);
               *     从旋转矩阵+平移向量构造
               * 3、 SE3 (const Quaterniond & unit_quaternion, const Vector3d & translation_);
               *     从单位四元数+平移向量构造 */
    cout << "SE3 from R,t = " << endl << SE3_Rt << endl;
    cout << "SE3 from q,t = " << endl << SE3_qt << endl;
    //李代数se(3)是一个六维向量，方便起见先typedef一下
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
              /* typedef MatrixVector6d;
               * typedef MatrixMatrix6d; */
    Vector6d se3 = SE3_Rt.log();
              /* 使用对数映射获得相应的李代数§
               * Vector6d log() const;
               * static Vector6d log(const SE3 & se3); */
    cout << "se3 = " << se3.transpose() << endl;
    //观察输出，会发现在Sophus中，se(3)平移在前，旋转在后。与我们书中是一致的。
    //同样地，有hat和vee两个运算符
              /* static Matrix4d hat(const Vector6d & omega);
               *                 hat()从向量到矩阵的变换
               * static Vector6d vee(const Matrix4d & omega);
               *                 vee()从矩阵到向量的变换 */
    cout << "se3 hat = " << endl << Sophus::SE3::hat(se3) << endl;
    cout << "se3 hat vee = " << Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose() << endl;

    //最后，演示一下更新
    Vector6d update_se3;   //更新量
    update_se3.setZero();
              /* Derived & Eigen::DenseBase::setZero();
               * 使用类方法setZero()将表达式中的所有系数设置为0
               * 初始化扰动量 */
    update_se3(0, 0) = 1e-4d;
              /* 将扰动量的平移增量的第一维赋值 */
    Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3) * SE3_Rt;
              /* static SE3 exp(const Vector6d & update);
               * 指数映射函数exp()返回李代数(se3)对应的李群(SE3) */
    cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;

    return 0;
}