旋转矩阵是三维空间刚体运动的描述方式之一
李群SO(3)旋转矩阵
李代so(3) 三维向量实际上是旋转向量组成的空间
李群SE(3)变换矩阵
李代se(3) 六维向量 平移在前 旋转在后
旋转矩阵的导数由旋转向量指定，指导着如何在旋转矩阵中进行微积分运算。
每个李群都有与之对应的李代数，李代数描述了李群的局部性质。
Sophus(李群、李代数库)
Eigen库是一个开源的C++线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。Eigen库提供了集合模块，但没有提供李代数的支持。
Sophus库是基于Eigen基础上开发的，继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时，既可以使用Eigen命名空间，也可以使用Sophus命名空间，Sophus库很好的支持了李群SO3 李代so3 李群SE3 李代se3。
高博视觉SLAM十四讲的这张图总结的很好
用沿Z轴转90度的Eigen旋转向量构造Eigen的旋转矩阵
/******************************************* 原型 李代数* SO3 (3*3)=="R" --> so3 (3*3)* SE3 (4*4)=="T" --> se3 (6*1)********************************************/// 旋转向量: 第一个参数为旋转角度，第二个参数哪个为1就绕哪轴旋转Eigen::Matrix3d rotation_matrix = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();cout << "用沿Z轴转90度的Eigen旋转向量构造Eigen的旋转矩阵:\n" << rotation_matrix << endl;/** 用沿Z轴转90度的Eigen旋转向量构造Eigen的旋转矩阵:* 0.707107 -0.707107 0* 0.707107 0.707107 0* 0 0 1*/
用Eigen的旋转矩阵构造Sophus李群旋转矩阵SO3
Sophus::SO3d SO3_rotation_matrix(rotation_matrix);cout << "用Eigen的旋转矩阵构造Sophus李群旋转矩阵SO3:\n" << SO3_rotation_matrix.matrix() << endl;/** 用Eigen的旋转矩阵构造Sophus李群旋转矩阵SO3:* 0.707107 -0.707107 0* 0.707107 0.707107 0* 0 0 1*/
对数映射求李群SO3的李代数so3
Eigen::Vector3d so3 = SO3_rotation_matrix.log();cout << "对数映射求李群SO3的李代数so3 = " << so3.transpose() << endl;// 对数映射求李群SO3的李代数so3 = 0 0 0.785398
指数映射求李代数so3的李群SO3
Sophus::SO3d SO3_rotation_matrix1 = Sophus::SO3d::exp(so3);cout << "指数映射求李代数so3的李群SO3 = \n" << SO3_rotation_matrix1.matrix() << endl;/** 指数映射求李代数so3的李群SO3 =* 0.707107 -0.707107 0* 0.707107 0.707107 0* 0 0 1*/
so3 hat 求向量到反对称矩阵
cout << "hat 求向量到反对称矩阵 =\n" << Sophus::SO3d::hat(so3) << endl;/** hat 求向量到反对称矩阵 =* 0 -0.785398 0* 0.785398 0 -0* -0 0 0*/
so3 hat vee 求反对称矩阵到向量
cout << "vee 求反对称矩阵到向量 = " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;// vee 求反对称矩阵到向量 = 0 0 0.785398
更新李群的旋转矩阵SO3
// 思路是更新李群的李代数然后再求更新后的李代数的李群Eigen::Vector3d update_so3(1e-4, 0, 0);Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_rotation_matrix;cout << "更新李群SO3的旋转矩阵 = \n" << SO3_updated.matrix() << endl;/** 更新李群SO3的旋转矩阵 =* 0.707107 -0.707107 0* 0.707107 0.707107 -0.0001* 7.07107e-05 7.07107e-05 1*/
用旋转矩阵R,平移向量t构造Sophus表示的李群变换矩阵SE3
// 平移向量: 沿X轴平移1Eigen::Vector3d t(1, 0, 0);Sophus::SE3d SE3_rotation_translation(rotation_matrix, t);cout << "用旋转矩阵R,平移向量t构造Sophus表示的李群变换矩阵SE3= \n" << SE3_rotation_translation.matrix() << endl;/** 用旋转矩阵R,平移向量t构造Sophus表示的李群变换矩阵SE3=* 0.707107 -0.707107 0 1* 0.707107 0.707107 0 0* 0 0 1 0* 0 0 0 1*/
李代数se3 是一个六维向量 李群SE3的对数映射求李代数se3
typedef Eigen::Matrix<double, 6, 1> Vector6d;Vector6d se3 = SE3_rotation_translation.log();cout << "李代数se3 是一个六维向量 李群SE3的对数映射求李代数se3 \n" << se3.transpose() << endl;/** 李代数se3 是一个六维向量 平移在前 旋转在后 李群SE3的指数映射求李代数se3* 0.948059 -0.392699 0 0 0 0.785398*/
指数映射求李代数se3的李群SE3
Sophus::SE3d SO3_rotation_matrix2 = Sophus::SE3d::exp(se3);cout << "指数映射求李代数se3的李群SE3 = \n" << SO3_rotation_matrix2.matrix() << endl;/** 指数映射求李代数se3的李群SE3 =* 0.707107 -0.707107 0 1* 0.707107 0.707107 0 1.11022e-16* 0 0 1 0* 0 0 0 1*/
se3 hat 求向量到反对称矩阵
cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;/** se3 hat =* 0 -0.785398 0 0.948059* 0.785398 0 -0 -0.392699* -0 0 0 0* 0 0 0 0*/
se3 hat 求向量到反对称矩阵
cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;/** se3 hat =* 0 -0.785398 0 0.948059* 0.785398 0 -0 -0.392699* -0 0 0 0* 0 0 0 0*/
更新李群的变换矩阵SE3
// 思路仍然是更新李群的李代数然后再求更新后的李代数的李群Vector6d update_se3;// 定义一个六维向量表示李代数update_se3.setZero();// 六维向量表示的李代数初始化update_se3(0, 0) = 1e-4;// 设置六维向量表示的李代数的第一个值Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_rotation_translation;// 指数映射求李代数se3的李群SE3,从而达到了更新李群的目的cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;/** SE3 updated =* 0.707107 -0.707107 0 1.0001* 0.707107 0.707107 0 0* 0 0 1 0* 0 0 0 1*/
交流答疑微信群
视觉SLAM十四讲实践笔记
本文分享自微信公众号 - 小秋SLAM笔记（gh_df091a0565ac）。
如有侵权，请联系 support@oschina.cn 删除。
本文参与“OSC源创计划”，欢迎正在阅读的你也加入，一起分享。
来源：oschina
链接：https://my.oschina.net/u/4944140/blog/4920645