视觉slam14讲学习(一)之se3上的定位表示:轨迹显示与轨迹误差_蒋成的博客-程序员宅基地

技术标签: 视觉slam学习  视觉slam  

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 pangolin
    DrawTrajectory(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 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);//窗口,rgba

        glLineWidth(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,tz
            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
    }

}


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位姿误差定义: e i = ∥ ei=\parallel ei=log(Tqt − 1 ^{-1} 1.Test) υ ^\upsilon υ ∥ \parallel (从4x4的矩阵变成6x1的向量);
总的误差和: R M S E = 1 / n ∑ i = 0 n e i 2 RMSE=\sqrt{1/n\sum_{i=0}^{n}ei^{2}} RMSE=1/ni=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维向量,se3
            Sophus::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 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);//窗口,rgba

        glLineWidth(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,tz
            glVertex3d(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,tz
            glVertex3d(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.结果显示

  1. 两条轨迹显示
    在这里插入图片描述
  2. 误差计算显示
    在这里插入图片描述
版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/orange_littlegirl/article/details/88418954

智能推荐

linux安装weblogic9.0,SSL证书安装指南 - BEA Weblogic 9.0_weixin_39534100的博客-程序员宅基地

SSL证书安装指南 - BEA Weblogic 9.0请安装以下步骤在BEA Weblogin 9.0上安装SSL证书:A:一旦您申请的SSL证书成功颁发,您会收到一个邮件通知您取回证书,点击邮件中的取回证书链接,就可以得到您的证书文件。此文件是一个打包文件,包含了您申请的证书类型的证书链文件、公钥证书。或 直接给您的是包含证书公钥和私钥的打包文件,有支持所有服务器类型的3种格式证书文件和各级根...

MongoDB 学习第二课--MongoDB linux 环境安装以及用户配置_大龄码农生活的博客-程序员宅基地

一、下载mongodb前往mongodb官网下载页面:https://www.mongodb.com/download-center/community下载相应的版本,比如目前的Linux x64位最新版:mongodb-linux-x86_64-ubuntu1604-4.2.0.tgz 1 、下载好后通过Xftp将压缩包传入服务器,其实也可以使用wget命令将上面的安装包地址直接...

zabbix mysql 优化_Zabbix配置文件的参数优化和Zabbix的数据库优化_今日解股的博客-程序员宅基地

Zabbix配置文件的参数优化StartPollers=60StartPollersUnreacheable=80StartTrappers=20StartPingers=100StartDiscoverers=120 #zabbix提示进程繁忙时修改此参数,最大二百五,建议100左右,值大消耗cpu性能大CacheSize=1024MStartDBSyncers=16HistoryCacheSi...

css 去掉浏览器记住账号密码后 input输入框 黄色背景_违规昵称001的博客-程序员宅基地

给input框设置&amp;:-webkit-autofillinput{ width: 100%; background: inherit; box-shadow: inherit; border: none; outline: none; color: $white; &amp;:-webkit-autofi

毕业设计成品价格_计算机毕业设计成品.docx_HelloGithub的博客-程序员宅基地

计算机毕业设计成品【篇一:毕业设计成品】国昌220kv降压变电所电气部分设计引 言电气工程及其自动化专业的毕业设计是培养学生综合运用大学四年所学理论知识,独立分析和解决工程实际问题的初步能力的一个重要环节。本设计是根据毕业设计的要求,针对220/60kv降压变电所毕业设计论文。本次设计主要是一次变电所电气部分的设计,并做出阐述和说明。论文包括选择变电所的主变压器的容量、台数和形式,选择待设计变电...

c语言中求单链表的最大值,编写算法,要求删除单链表中元素最大的结点_九千步的博客-程序员宅基地

满意答案89兄弟2016.08.20采纳率:49%等级:9已帮助:466人//删除单链表中最大元素Del-max(link a){int tmp;element *p;element *max;p=a; //指针,用于遍历链表,取数与当前最大结点值比较max=a; //指针,用于记录最大元素所在位置(未考虑有多个最...

随便推点

matlab fpga in loop,FPGA in the Loop在环测试_weixin_39802519的博客-程序员宅基地

关于FPGA在环测试功能一直是被我忽视的一点,幸得坛友提供资料,使得能够完善在环测试功能,关于在环测试实现将在MBDCORE的手册中更新。有兴趣关于FPGA在环测试功能一直是被我忽视的一点,幸得坛友 2980518171 提供资料,使得能够完善在环测试功能,关于在环测试实现将在MBDCORE的手册中更新。有兴趣的MBD爱好者可以看一下原帖是怎么写的https://www.cnblogs.com/l...

Oracle快照的创建_阳光下的青柠小镇的博客-程序员宅基地_oracle创建快照

1、什么是数据库快照?oracle数据库的快照是一个表,它包含有对一个本地或远程数据库上一个或多个表或视图的查询的结果,也就是说快照根本的原理就是将本地或远程数据库上的一个查询结果保存在一个表中。2、创建快照的步骤:2.1、在目标数据库上创建database links:create database link TEST1 connect to TEST identified by 123 using 'orcl';其中test1是database links的名字,test是目标数

idea 调试java技巧_Intellij IDEA调试功能使用总结_ChrisJimmel的博客-程序员宅基地

点击上方“Java知音”,选择“置顶公众号”技术文章第一时间送达!作者:WB4S链接:www.cnblogs.com/Bowu这段时间一直在使用Intellij IDEA, 今天把调试区工具的使用方法记录于此。先编译好要调试的程序。1.设置断点选定要设置断点的代码行,在行号的区域后面单击鼠标左键即可。2.开启调试会话点击红色箭头指向的小虫子,开始进入调试。IDE下方出现Debug视图,红色的箭头指...

深入理解java8_深入理解Java8新特性与源码剖析_weixin_40006965的博客-程序员宅基地

资源介绍资源名称:深入理解Java8新特性与源码剖析教程内容:xa08可谓Java语言历史上变化最大的一个版本,其承诺要调整Java编程向着函数式风格迈进,这有助于编写出更为简洁、表达力更强,并且在很多情况下能够利用并行硬件的代码。本门课程将会深入介绍Java 8新特性,学员将会通过本门课程的学习深入掌握Java 8新增特性并能灵活运用在项目中。学习者将学习到如何通过Lambda表达式使用一行代码...

mybatis-plus代码生成_林一.的博客-程序员宅基地

代码生成器主类GenteratorCodeimport com.baomidou.mybatisplus.generator.AutoGenerator;import com.baomidou.mybatisplus.generator.InjectionConfig;import com.baomidou.mybatisplus.generator.config.*;import com.baomidou.mybatisplus.generator.config.converts.MySqlTy

android viewgroup 事件,android中viewgroup的事件传递分析_AMD中国的博客-程序员宅基地

在上一篇中我们分析了从view的dispatchTouchEvent到onTouchListener的onTouch回调到onTouchEvent到onClickLisener的onClickandroid中view事件传递,在后面遗留了两个问题,那就是在onTouchEvent中返回false的时候,只触发到action_down事件,以及在dispatchTouchEvent中返回false也...

推荐文章

热门文章

相关标签