视觉slam14讲学习（一）之se3上的定位表示:轨迹显示与轨迹误差

文章目录

1.读出trajectory.txt中的轨迹信息
2. 用pangolin画出轨迹poses
3. 利用Eigen进行欧拉角和四元数的转化
4. 画出两条轨迹，对定位精度进行分析
5.结果显示

1.读出trajectory.txt中的轨迹信息

高翔博士的视觉slam14讲书籍下载资源
如何描述视觉定位的精度？一般会用定位误差来描述，有很多开源工具干这件事情，在这之前，我们先学习如何用Pangolin来画出机器人的定位轨迹。
首先需要读出trajectory.txt中的轨迹信息，其中txt中的轨迹格式是[time,tx,ty,tz,qx,qy,qz,qw]

#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>using namespace std;// path to trajectory file
string trajectory_file = "./trajectory.txt";// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);int main(int argc, char **argv) {vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;//读出的位姿存入该容器类vector中//读取轨迹文件中的位姿，T（t3,q4）//第一种方法，用fstream的getline分行读取stringstream按空格拆分传入数组/* ifstream infile;infile.open(trajectory_file, ios::in);if(!infile.is_open())cout<<"open file failture"<<endl;string line;while(!infile.eof() && std::getline(infile, line)){//是否读到文件的最后,是否读取一行stringstream ss(line);//使用getlne分行，使用stringstream切割空格double str;vector<double> arr;while(ss >> str){//传入字符cout<<str<<endl;arr.push_back(str);//传入数组arr}Eigen::Quaterniond q(arr[7], arr[8], arr[5], arr[6]);Eigen::Vector3d t(arr[1], arr[2], arr[3]);Sophus::SE3 SE3(q,t);poses.push_back(SE3);cout<<endl;cout<<line<<endl;}infile.close();*///*第二种方法，参考点云地图传入,gaoxiang书的第五讲/ifstream in(trajectory_file);//创建输入流if(!in){cout<<"open posefile failture!!!"<<endl;return 0;}for(int i=0; i<620; i++){double data[8]={0};for(auto& d:data) in>>d;//按行依次去除数组中的值Eigen::Quaterniond q(data[7], data[8], data[5], data[6]);Eigen::Vector3d t(data[1], data[2], data[3]);Sophus::SE3 SE3(q,t);poses.push_back(SE3);}//*/// draw trajectory in pangolinDrawTrajectory(poses);return 0;
}

2. 用pangolin画出轨迹poses

//gaoxiang提供的画轨迹的函数
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {if (poses.empty()) {cerr << "Trajectory is empty!" << endl;return;}// create pangolin window and plot the trajectorypangolin::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);//窗口，rgbaglLineWidth(2);//线宽//cout<<"pose.size()="<<poses.size();for (size_t i = 0; i < poses.size() - 1; i++) {glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());//颜色随位置变化而变化glBegin(GL_LINES);auto p1 = poses[i], p2 = poses[i + 1];//只显示tx,ty,tzglVertex3d(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}}

3. 利用Eigen进行欧拉角和四元数的转化

Eigen::AngleAxisd rollAngle(AngleAxisd(roll_ - roll1_,Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(AngleAxisd(pitch_ - pitch1_,Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(AngleAxisd(yaw_ - yaw1_,Vector3d::UnitZ()));
Eigen::Quaterniond q;
q= yawAngle * pitchAngle * rollAngle;
q.normalized();

4. 画出两条轨迹，对定位精度进行分析

对于第i位姿误差定义：ei=∥ei=\parallelei=∥log(Tqt−1^{-1}−1.Test)υ^\upsilonυ∥\parallel∥（从4x4的矩阵变成6x1的向量）;
总的误差和：RMSE=1/n∑i=0nei2RMSE=\sqrt{1/n\sum_{i=0}^{n}ei^{2}}RMSE=1/n∑i=0nei2

//author:jiangcheng
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream>// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>using namespace std;// path to trajectory file
string gt_file = "/home/ubuntu/DL/深蓝slam/L4/draw_trajectory/groundtruth.txt";
string est_file = "/home/ubuntu/DL/深蓝slam/L4/draw_trajectory/estimated.txt";// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> get_pose(string& pose_file);void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &gt_poses,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &est_poses);void compare_difference(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &gt_poses,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &est_poses);int main(int argc, char **argv) {vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> gt_pose=get_pose(gt_file);vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> est_pose=get_pose(est_file);//继续往poses全局变量里面传数据// draw trajectory in pangolin//DrawTrajectory(gt_pose,est_pose);//打印两条轨迹//计算误差compare_difference(gt_pose,est_pose);return 0;
}/****************************************************************************************/
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> get_pose(string& pose_file){vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;//读出的位姿存入该容器类vector中,局部变量//*第二种方法，参考点云地图传入,gaoxiang书的第五讲/ifstream in(pose_file);//创建输入流if(!in){cout<<"open posefile failture!!!"<<endl;return poses;}for(int i=0; i<620; i++){double data[8]={0};for(auto& d:data) in>>d;//按行依次去除数组中的值Eigen::Quaterniond q(data[7], data[4], data[5], data[6]);Eigen::Vector3d t(data[1], data[2], data[3]);Sophus::SE3 SE3(q,t);poses.push_back(SE3);}return poses;
}/*******************************************************************************************/
//计算两个轨迹的误差
void compare_difference(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &gt_poses,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &est_poses){double rmse_square=0;double rmse=0;for (int i = 0; i <612; i++) {auto p1 = gt_poses[i];Sophus::SE3 p2 = est_poses[i];cout<<"p1.matrix "<<p1.matrix()<<"\n,p2.matrix"<<p2.matrix()<<endl;Eigen::Matrix<double,4,4> m = (p1.matrix().inverse())*p2.matrix();cout<<"m.matrix"<<m.matrix()<<endl;Eigen::Matrix<double,3,3> R = m.topLeftCorner<3,3>();Eigen::Matrix<double,3,1> t = m.topRightCorner<3,1>();Sophus::SE3 SE3_dot(R,t);//构造T12的李群SE3，从4x4的矩阵变成6x1的向量cout<<"se3 is "<<SE3_dot.matrix()<<endl;//打印矩阵形式//李代数是6维向量，se3Sophus::Vector6d se3_log= SE3_dot.log();//对数映射//累加误差double error=se3_log.squaredNorm();//二范数的平方,squaredNorm;二范数，norm()cout<<"se3 squarNorm is "<<error<<endl;rmse_square=rmse_square+error;//累加}rmse=sqrt((rmse_square)/612);cout<<"RSME is :"<<rmse<<endl;
}/*******************************************************************************************/
//gaoxiang提供的画轨迹的函数,画两条轨迹
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &gt_poses,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> &est_poses){if (gt_poses.empty() || est_poses.empty()) {cerr << "Trajectory is empty!" << endl;return;}// create pangolin window and plot the trajectorypangolin::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);//窗口，rgbaglLineWidth(2);//线宽//cout<<"pose.size()="<<poses.size();for (size_t i = 0; i < est_poses.size() - 1; i++) {glColor3f(1 - (float) i / est_poses.size(), 0.0f, (float) i / est_poses.size());//颜色随位置变化而变化glBegin(GL_LINES);auto p1 = est_poses[i], p2 = est_poses[i + 1];//只显示tx,ty,tzglVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);glColor3f(0.f, 0.8f, 0.f);//绿色glBegin(GL_LINES);auto p3 = gt_poses[i], p4 = gt_poses[i + 1];//只显示tx,ty,tzglVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);glEnd();}pangolin::FinishFrame();usleep(5000);   // sleep 5 ms}}

5.结果显示

两条轨迹显示
误差计算显示

视觉slam14讲学习（一）之se3上的定位表示:轨迹显示与轨迹误差相关推荐

SLAM14讲学习笔记（一) 李群李代数基础
第二次翻看<视觉SLAM十四讲>,发现第一次看过的都忘记了,认真分析了一下,我认为是第一次学习没有弄清楚整体的脉络.因此这次做一个小小的总结,希望不要再过几天又忘掉.前面的内容就没有总结的 ...
视觉slam14讲ch5 opencv安装 ubuntu20.04
视觉slam14讲ch5 opencv安装 ubuntu20.04 最近在学习视觉slam14讲第五章时,由于opencv需要较多依赖项,安装过程经历了一些曲折,在此进行记录. 1.下载opencv工 ...
SLAM14讲学习笔记（三）非线性优化基础
这部分的内容,第一次看觉得很难看懂,不知所云:最近第二次看,可以看明白了.本着"先赶理论,后赶代码,借着代码复习C++"的原则,现在先不去学习代码的内容. 这一章的内容,应该先从宏 ...
SLAM14讲学习笔记（十五）卡尔曼滤波器的直观理解
之前在SLAM14讲学习笔记(六)后端(最难一章:卡尔曼滤波器推导.理解以及扩展)中,介绍了卡尔曼滤波器的推导. 但是感觉不太直观,因此这次用了几个简单的图,希望能一目了然卡尔曼滤波器是在干什么. 先 ...
视觉SLAM14讲第三章学习笔记
文章目录刚体运动旋转矩阵变换矩阵旋转向量欧拉角四元数如何表示旋转刚体运动一个物体,运动过后除位置与姿态外的自身条件不会变,变换后与变换前相差一个旋转和平移. 旋转矩阵向量的内积:a ...
视觉SLAM14讲——视觉里程计1（特征点法）
前面说过视觉SLAM系统分为前端和后端两个内容,前端也叫做视觉里程计.视觉里程计的主要作用是根据相邻的两张图像的信息粗略的估计出相机运动,给后端一个较好的初始值.视觉里程计的两大算法为:特征点法和直接 ...
【视觉SLAM14讲】ch3课后题答案
1.验证旋转矩阵是正交矩阵感觉下面这篇博客写的不错 http://www.cnblogs.com/caster99/p/4703033.html 总结一下:旋转矩阵是一个完美的矩阵--正交矩阵.①行 ...
视觉SLAM14讲笔记分享——第四章【李群与李代数】
李群与李代数基础群直观的理解:群就是一种特殊的代数结构,这种代数结构是由一种集合加上一种运算组成,我们把集合记作A,运算记为 ⋅ \cdot ⋅,那么群记为 G = ( A , ⋅ ) G=(A, ...
视觉SLAM14讲——李群与李代数
前面的文章说过三维空间的刚体运动,介绍了旋转的表示.在SLAM中相机的位姿是不确定的,需要我们进行优化,常用的优化方法便是求导,但是旋转矩阵由于自身的特殊性(行列式为1的正交矩阵),使我们无法直接对旋 ...