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