C++学习笔记——Sophus模块（李群/李代数）

用于计算李群与李代数的C++模块，做一下用法的笔记。
参考书籍：《视觉SLAM十四讲-从理论到实践》——高翔

1. 使用方式

首先从git安装fmt与Sophus的第三方库。
CMakeLists.txt写法样例：

# 最低 c++ 版本要求
cmake_minimum_required( VERSION 2.8 )

# 创建项目
project(sophus_project)

# 添加一个运行程序
add_executable( sophus_test sophus_test.cpp )

# 添加额外库的路径
# eigen作为只有头文件没有二进制文件的库不需要链接
include_directories("/usr/include/eigen3")

# 链接fmt库
find_package(FMT REQUIRED)
target_link_libraries(sophus_test fmt::fmt)

# 添加Sophus库
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

3. 旋转矩阵的李群与李代数：SO3与so3

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    // 构建一个绕Z轴旋转45度的旋转矩阵
    Matrix3d rotation_matrix = AngleAxisd(M_PI / 4, Vector3d(0, 0, 1)).toRotationMatrix();
    // 一个等价的四元数
    Quaterniond q(rotation_matrix);

    // 基于旋转矩阵或四元数构建SO3
    Sophus::SO3d SO3_R(rotation_matrix);
    Sophus::SO3d SO3_q(q);

    // 打印SO3
    cout << "旋转矩阵转换的SO3：" << endl << SO3_R.matrix() << endl;
    cout << "四元数转换的SO3：" << endl << SO3_q.matrix() << endl;

    // 使用对数映射获得它的李代数
    Vector3d so3 = SO3_R.log();
    cout <<  "so3李代数为：" << endl << so3.transpose() << endl;

    // 使用hat将向量转换为反对称矩阵
    Matrix3d hat_matrix = Sophus::SO3d::hat(so3);
    cout << "李代数转换为反对称矩阵：" << endl << hat_matrix << endl;
    // 使用vee将反对称矩阵变回向量
    cout << "反对称矩阵转换为李代数：" << endl << Sophus::SO3d::vee(hat_matrix).transpose() << endl;

    // 用增量扰动模型更新so3
    // 设置增量扰动模型的更新量
    Vector3d update_so3(1e-4, 0, 0);
    // 更新so3
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
    // 显示更新结果
    cout << "更新后的SO3为：" << endl << SO3_updated.matrix() <<endl;

}

运行得到：

旋转矩阵转换的SO3：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
四元数转换的SO3：
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
so3李代数为：
       0        0 0.785398
李代数转换为反对称矩阵：
        0 -0.785398         0
 0.785398         0        -0
       -0         0         0
反对称矩阵转换为李代数：
       0        0 0.785398
更新后的SO3为：
    0.707107    -0.707107 -6.77626e-21
    0.707107     0.707107      -0.0001
 7.07107e-05  7.07107e-05            1

2. 变换矩阵的李群与李代数：SE3与se3

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    // 构造SE3
    // 构造平移向量，沿着Y轴移动1
    Vector3d t(0, 1, 0);
    // 构建一个绕Z轴旋转45度的旋转矩阵
    Matrix3d rotation_matrix = AngleAxisd(M_PI / 4, Vector3d(0, 0, 1)).toRotationMatrix();
    // 一个等价的四元数
    Quaterniond q(rotation_matrix);

    // 基于旋转矩阵/四元数，再加上平移向量构建SE3
    Sophus::SE3d SE3_Rt(rotation_matrix, t);
    Sophus::SE3d SE3_qt(q, t);

    // 打印SE3
    cout << "旋转矩阵加平移向量转换的SE3：" << endl << SE3_Rt.matrix() << endl;
    cout << "四元数加平移向量转换的SE3：" << endl << SE3_qt.matrix() << endl;

    // se3是个6x1的矩阵，为它定义一个新类型
    typedef Eigen::Matrix <double, 6, 1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    // 显示se3
    // 前三个数字是平移，后三个数字是旋转
    cout << "se3李代数为：" << endl << se3.transpose() << endl;
    
    // 同样使用hat和vee在李代数与反对称矩阵之间转换
    Matrix4d hat_matrix = Sophus::SE3d::hat(se3);
    cout << "李代数转换为反对称矩阵：" << endl << hat_matrix << endl;
    cout << "反对称矩阵转换为李代数：" << endl << Sophus::SE3d::vee(hat_matrix).transpose() << endl;

    // 用增量扰动模型更新se3
    // 设置增量扰动模型的更新量
    Vector6d update_se3;
    update_se3.setZero();
    update_se3(0, 0) = (double)1e-4;
    cout << "扰动更新向量：" << endl << update_se3.transpose() << endl;

    // 更新se3
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
    // 显示更新结果
    cout << "更新后的SE3为：" << endl << SE3_updated.matrix() <<endl;

}

运行得到：

旋转矩阵加平移向量转换的SE3：
 0.707107 -0.707107         0         0
 0.707107  0.707107         0         1
        0         0         1         0
        0         0         0         1
四元数加平移向量转换的SE3：
 0.707107 -0.707107         0         0
 0.707107  0.707107         0         1
        0         0         1         0
        0         0         0         1
se3李代数为：
0.392699 0.948059        0        0        0 0.785398
李代数转换为反对称矩阵：
        0 -0.785398         0  0.392699
 0.785398         0        -0  0.948059
       -0         0         0         0
        0         0         0         0
反对称矩阵转换为李代数：
0.392699 0.948059        0        0        0 0.785398
扰动更新向量：
0.0001      0      0      0      0      0
更新后的SE3为：
 0.707107 -0.707107         0    0.0001
 0.707107  0.707107         0         1
        0         0         1         0
        0         0         0         1