当前位置：首页 > news >正文

视觉Slam14讲笔记第4讲李群李代数【更新中】

news 2025/9/20 7:34:18

一、李群李代数

二、使用示例

注意：
1.如果报编译Sophus报错: error: implicitly-declared
解决对策：在CMakeLists.txt中加入

set(CMAKE_CXX_FLAGS "-Wno-error=deprecated-declarations -Wno-deprecated-declarations ")

在这里插入图片描述
2.如果编译useSophus时报错：/usr/local/include/sophus/common.hpp:11:10: fatal error: Eigen/Core:
确保正确安装Eigen的前提下

sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen```**代码部分**```cpp
#include <cmath>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <iostream>#include "sophus/se3.hpp"using namespace std;
using namespace Eigen;////本程序演示Sophus的使用方法
int main(int argc, char **argv) {// 沿Z轴旋转90度的旋转矩阵Matrix3d R = AngleAxisd(M_PI_2, Vector3d(0, 0, 1)).toRotationMatrix();// 或者四元素Quaterniond q(R);Sophus::SO3d SO3_R(R);  // Sophus：：SO3d可以直接从旋转矩阵构造Sophus::SO3d SO3_q(q);  // 也可以从四元素构造// 二者是等价的cout << "SO(3) from matrix: \n" << SO3_R.matrix() << endl;cout << "SO(3) from quaternion: \n" << SO3_q.matrix() << endl;cout << "they are equal " << endl;// 使用对数映射获得它的李代数Vector3d so3 = SO3_R.log();cout << "so3 = " << so3.transpose() << endl;// hat 为向量反对阵矩阵cout << "so3 hat = " << Sophus::SO3d::hat(so3) << endl;// 相对的，vee为反对阵的向量cout << "so3 hat vee = "<< Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;// 增量扰动模型的更新Vector3d update_so3(1e-4, 0, 0);  // 假设更新量为1e-4Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;cout << "**************************************" << endl;// 对SE(3)操作大同小异Vector3d t(1, 0, 0);        // 沿X轴平移1Sophus::SE3d SE3_Rt(R, t);  // 从R,t构造SE(3)Sophus::SE3d SE3_qt(q, t);  // 从q,t构造SE(3)cout << "SE3 from R,t: \n" << SE3_Rt.matrix() << endl;cout << "SE3 from q,t: \n" << SE3_qt.matrix() << endl;cout << "they are equal " << endl;// 李代数se(3)是一个六维向量，方便一下先typedef一下typedef Eigen::Matrix<double, 6, 1> Vector6d;Vector6d se3 = SE3_Rt.log();cout << "se3 = " << se3.transpose() << endl;// 观察输出，会发现在Sophus中，se(3)的平移在前，旋转在后// 同样的，有hat和vee运算符cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;cout << "se3 hat vee = "<< Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;// 演示更新Vector6d update_se3;update_se3.setZero();update_se3(0, 0) = 1e-4;Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;cout << "SE3 updated = \n" << SE3_updated.matrix() << endl;return 0;
}

输出结果如下：

SO(3) from matrix: 
2.22045e-16          -1           01 2.22045e-16           00           0           1
SO(3) from quaternion: 
2.22045e-16          -1           01 2.22045e-16           00           0           1
they are equal 
so3 =      0      0 1.5708
so3 hat =       0 -1.5708       01.5708       0      -0-0       0       0
so3 hat vee =      0      0 1.5708
SO3 updated = 0          -1           01           0     -0.00010.0001 2.03288e-20           1
**************************************
SE3 from R,t: 
2.22045e-16          -1           0           11 2.22045e-16           0           00           0           1           00           0           0           1
SE3 from q,t: 
2.22045e-16          -1           0           11 2.22045e-16           0           00           0           1           00           0           0           1
they are equal 
se3 =  0.785398 -0.785398         0         0         0    1.5708
se3 hat = 0   -1.5708         0  0.7853981.5708         0        -0 -0.785398-0         0         0         00         0         0         0
se3 hat vee =  0.785398 -0.785398         0         0         0    1.5708
SE3 updated = 
2.22045e-16          -1           0      1.00011 2.22045e-16           0           00           0           1           00           0           0           1