电子工程师技术服务社区
公告
登录
|
注册
首页
技术问答
厂商活动
正点原子
板卡试用
资源库
下载
文章
社区首页
文章
【7天搞定视觉SLAM】番外3——李代数库Sophus的使用
分 享
扫描二维码分享
【7天搞定视觉SLAM】番外3——李代数库Sophus的使用
slam
hero_chao
关注
发布时间: 2020-04-20
丨
阅读: 2611
# Sophus的介绍 > Eigen库是一个开源的C++线性代数库,它提供了快速的有关矩阵的线性代数运算,还包括解方程等功能。但是Eigen库提供了集合模块,但没有提供李代数的支持。一个较好的李群和李代数的库是Sophus库,它很好的支持了SO(3),so(3),SE(3)和se(3)。Sophus库是基于Eigen基础上开发的,继承了Eigen库中的定义的各个类。因此在使用Eigen库中的类时,既可以使用Eigen命名空间,也可以使用Sophus命名空间。 ## 安装 下载相关的源码,只需要编译,不用安装。 ```cpp //在cmake编译 mkdir build cd build cmake .. make ``` ![](https://cf01.ickimg.com/bbsimages/202004/c816014e1a9c8f64634aab88ae5c0bc2.png) ## 代码 ```cpp #include
#include
#include
#include
#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=\n" << 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); //假设更新量为这么多 Sophus::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轴平移1 Sophus::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; // 李代数se(3) 是一个六维向量,方便起见先typedef一下 typedef Eigen::Matrix
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-4d; Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt; cout << "SE3 updated=" << endl << SE3_updated.matrix() << endl; return 0; } ``` 运行此代码,就可以看到一些结果,分析代码,你会加深对相关库的使用和理解。 ## 一个SLAM小例子 使用绝对轨迹误差,对俩条轨迹计算误差: ```cpp #include
#include
#include
#include
#include
using namespace Sophus; using namespace std; string groundtruth_file = "./example/groundtruth.txt"; string estimated_file = "./example/estimated.txt"; typedef vector
> TrajectoryType; void DrawTrajectory(const TrajectoryType >, const TrajectoryType &esti); TrajectoryType ReadTrajectory(const string &path); int main(int argc, char **argv) { TrajectoryType groundtruth = ReadTrajectory(groundtruth_file); TrajectoryType estimated = ReadTrajectory(estimated_file); assert(!groundtruth.empty() && !estimated.empty()); assert(groundtruth.size() == estimated.size()); // compute rmse double rmse = 0; for (size_t i = 0; i < estimated.size(); i++) { Sophus::SE3d p1 = estimated[i], p2 = groundtruth[i]; double error = (p2.inverse() * p1).log().norm(); rmse += error * error; } rmse = rmse / double(estimated.size()); rmse = sqrt(rmse); cout << "RMSE=" << rmse << endl; DrawTrajectory(groundtruth, estimated); return 0; } TrajectoryType ReadTrajectory(const string &path) { ifstream fin(path); TrajectoryType trajectory; if (!fin) { cerr << "trajectory " << path << " not found." << endl; return trajectory; } while (!fin.eof()) { double time, tx, ty, tz, qx, qy, qz, qw; fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw; Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz)); trajectory.push_back(p1); } return trajectory; } void DrawTrajectory(const TrajectoryType >, const TrajectoryType &esti) { // create pangolin window and plot the trajectory pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768); glEnable(GL_DEPTH_TEST); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); pangolin::OpenGlRenderState s_cam( pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000), pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0) ); pangolin::View &d_cam = pangolin::CreateDisplay() .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f) .SetHandler(new pangolin::Handler3D(s_cam)); while (pangolin::ShouldQuit() == false) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); d_cam.Activate(s_cam); glClearColor(1.0f, 1.0f, 1.0f, 1.0f); glLineWidth(2); for (size_t i = 0; i < gt.size() - 1; i++) { glColor3f(0.0f, 0.0f, 1.0f); // blue for ground truth glBegin(GL_LINES); auto p1 = gt[i], p2 = gt[i + 1]; glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]); glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]); glEnd(); } for (size_t i = 0; i < esti.size() - 1; i++) { glColor3f(1.0f, 0.0f, 0.0f); // red for estimated glBegin(GL_LINES); auto p1 = esti[i], p2 = esti[i + 1]; glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]); glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]); glEnd(); } pangolin::FinishFrame(); usleep(5000); // sleep 5 ms } } ``` ![](https://cf01.ickimg.com/bbsimages/202004/e3abf47c452ff3db76e6cf5ff4158896.png) ### 参考代码 ```cpp #include
#include
#include
#include
using namespace std; //using namespace Eigen; //using namespace Sophus; int main(int argc, char **argv) { //沿着Z轴旋转90度的旋转矩阵 Eigen::AngleAxisd A1(M_PI / 2, Eigen::Vector3d(0, 0, 1));//以(0,0,1)为旋转轴,旋转180度 Eigen::Matrix3d R1 = A1.matrix(); Eigen::Quaterniond Q1(A1); //一、初始化的李群(SO3)的几种方式 //1.使用旋转矩阵初始化李群 Sophus::SO3 SO3_R(R1); //注意:尽管SO(3)是对应一个矩阵,但是输出SO(3)时,实际上是以so(3)形式输出,从输出的结果可以看到,其输出的值与旋转角对应的值相同,这也证证实了SO(3)对应的李代数so(3)就是旋转角。 cout << "SO(3) SO3_R from Matrix" << SO3_R << endl << endl; //2.使用四元数初始化李群 Sophus::SO3 SO3_Q(Q1); cout << "SO(3) SO3_Q from Quaterion" << SO3_Q << endl << endl; /@@**************************************************************************** 3.1 使用旋转角(轴角)的各个元素对应的代数值来初始化李群 注意:直接使用旋转角AngleAxis或是旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())对李群进行初始化是不行的,因为SO3李群没有对应的构造函数。 也即是使用下列方法是错误的: Sophus::SO3 SO3_A(A1);//直接使用旋转角对李群初始化 Sophus::SO3 SO3_A(A1.axis()*A1.angle());//直接使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())对李群进行初始化 只能使用旋转角对应的向量的每一个维度进行赋值,对应于SO3的这样一个构造函数SO3(double rot_x, double rot_y, double rot_z); *******************************************************************************/ //3.1.1 使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())中的各个元素对李群进行初始化 Sophus::SO3 SO3_A1((A1.axis() * A1.angle())(0), (A1.axis() * A1.angle())(1), (A1.axis() * A1.angle())(2)); cout << "SO(3) SO3_A1 from AngelAxis1" << SO3_A1 << endl << endl; //3.1.2 使用旋转角度对应的向量(Vector3d=AngleAxis.axis()*AngleAxis.angle())中的各个元素对李群进行初始化 Sophus::SO3 SO3_A2(M_PI / 2 * 0, M_PI / 2 * 0, M_PI / 2 * 1); cout << "SO(3) SO3_A2 from AngleAixs2" << SO3_A2 << endl << endl; //3.2 由于旋转角(轴角)与李代数so(3)对应,所以直接使用旋转角的值获得se(3),进而再通过Sophus::SO3::exp()获得对应的SO(3) Eigen::Vector3d V1(0, 0, M_PI / 2);//so3在Eigen中用Vector3d表示 Sophus::SO3 SO3_V1 = Sophus::SO3::exp(V1); cout << "SO(3) SO3_V1 from SO3::exp()" << SO3_V1 << endl << endl; //二、SO(3)与so(3)的相互转换,以及so3对应的hat和vee操作 Eigen::Vector3d so3_V1 = SO3_V1.log();//so(3)在Sophus(Eigen)中用vector3d表示,使用对数映射获得李群对应的李代数 cout << "so(3) so3_V1 from SO3_V1" << so3_V1.transpose() << endl << endl; Sophus::SO3 SO3_V2 = Sophus::SO3::exp(so3_V1);//使用指数映射将李代数转化为李群 cout << "SO(3) so3_V2 from so3_V1" <
参考:《视觉SLAM十四讲》
原创作品,未经权利人授权禁止转载。详情见
转载须知
。
举报文章
点赞
(
0
)
hero_chao
关注
评论
(0)
登录后可评论,请
登录
或
注册
相关文章推荐
MK-米客方德推出工业级存储卡
Beetle ESP32 C3 蓝牙数据收发
Beetle ESP32 C3 wifi联网获取实时天气信息
开箱测评Beetle ESP32-C3 (RISC-V芯片)模块
正点原子数控电源DP100测评
DP100试用评测-----开箱+初体验
Beetle ESP32 C3环境搭建
【花雕体验】16 使用Beetle ESP32 C3控制8X32位WS2812硬屏之二
X
你的打赏是对原创作者最大的认可
请选择打赏IC币的数量,一经提交无法退回 !
100IC币
500IC币
1000IC币
自定义
IC币
确定
X
提交成功 ! 谢谢您的支持
返回
我要举报该内容理由
×
广告及垃圾信息
抄袭或未经授权
其它举报理由
请输入您举报的理由(50字以内)
取消
提交