SLAM十四讲:trajectoryError.cpp Sophus库和Pangolin库使用_Charlesffff的博客-程序员秘密

技术标签: 算法  c++  开发语言  

**

TrajectoryError程序分析

**
**

一.介绍

**
本程序演示了使用Sophus库和pangolin库绘制预估轨迹和正确轨迹,同时计算绝对轨迹误差。
**

二.内容

**
1.Sophus库定义SE3d.
2.vector容器使用SE3d。
3.文件的读入。
4.Pangolin库中函数使用。
5.计算绝对轨迹误差。
**

三.具体函数以及每一步思路解答见函数注释

**
main.cpp

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>

using namespace std;
using namespace Sophus;

//文件路径
string groundtruth_file="groundtruth.txt";
string estimated_file="estimated.txt";

//定义vector容器,用于存储SE3d,其中aligned_allocator用来分配空间,注意:SE3d也可以替换为Isometry3d为旋转矩阵
typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

//回执轨迹图
void DrawTrajectory(const TrajectoryType &gt,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的作用是现计算表达式 expression ,如果其值为假(即为0),那么它先向stderr打印一条出错信息,然后通过调用 abort 来终止程序运行。
    assert(!groundtruth.empty()&&!estimated.empty());
    assert(groundtruth.size()==estimated.size());
    
    //计算绝对轨迹误差 p89
    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 std::__cxx11::string& path)
{
    
    //定义文件变量
    ifstream fin(path);
    TrajectoryType trajectory;
    //文件不存在
    if(!fin){
    
        cerr<<"trajectory"<<path<<"not found"<<endl;
        return trajectory;
    }
    //文件存在,eof()用于判断文件是否到结尾
    while(!fin.eof()){
    
        double time,tx,ty,tz,qx,qy,qz,qw;
        fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        //对比:typedef vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
        //这里注意,SE3d定义是一个变换矩阵+一个平移向量
        Sophus::SE3d p1(Eigen::Quaterniond(qw,qx,qy,qz),Eigen::Vector3d(tx,ty,tz));
        trajectory.push_back(p1);
    }
    return trajectory;
}

//画路径
 void DrawTrajectory(const TrajectoryType& gt, const TrajectoryType& esti)
{
    
    //创建一个“Trajectory Viewer“GUI界面窗口,大小为1024*768
    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,0.0,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);
            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);
            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);
    }
}

CMakeList.txt

cmake_minimum_required(VERSION 2.6)
project(trajectoryerror)

#添加Pangolin

find_package(Pangolin REQUIRED)
include_directories(${
    Pangolin_DIRECTORIES})

add_executable(trajectoryerror main.cpp)

#添加链接
target_link_libraries(trajectoryerror ${
    Pangolin_LIBRARIES})

install(TARGETS trajectoryerror RUNTIME DESTINATION bin)

**

四.运行结果

**
在这里插入图片描述

版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/weixin_44190648/article/details/121783827

智能推荐

安装ORB-SLAM3的过程_Donaghy的博客-程序员秘密

1.安装Pangolin下载源码:git clone https://github.com/stevenlovegrove/Pangolin.git根据https://github.com/stevenlovegrove/Pangolin#scheme-syntax-for-windowing-and-video安装依赖安装:git clone https://github.com/stevenlovegrove/Pangolin.gitcd Pangolinmkdir buildcd bu

2012 下半年规划---2012年年底备注_umitor的博客-程序员秘密

1. 工作上1.1 做好本职工作的情况下,花大力气深入研究WiFi,整理资料,好好积累一些实实在在的东西;Atheros、BCM、Marvell。。。深入阅读: 802.11无线网络权威指南并实践熟悉802.11和WPS规范并实践检验个芯片的相关实现程度;[2012-12-29 umitor] 所幸没有自欺欺人,这一点还是不错的,花了大力气研究Atheros、Marvell

微信小程序后台持续定位功能使用_微信小程序定位功能_暮 色的博客-程序员秘密

微信小程序团队在7月30日更新了 基础库 2.8.0其中新添加了小程序后台持续定位功能和联系定位的接口从上到下分别是1.wx.onLocationChange//监听位置实时变化2.wx.stopLocationUpdate//关闭监听实时位置变化,前后台都停止消息接收3.wx.startLocationUpdate//开启小程序进入前台时接收位置消息4.wx.startLocat...

window下安装MongoDB_--bind_ip 127.0.0.1 --bind_ip 0.0.0.0_qsya的博客-程序员秘密

背景:最近想在云服务器上装一个mongodb,配置时遇到一些问题,无法启动,无法连接,绑定IP无效等问题,特此记录一下。1.安装下载地址:https://www.mongodb.com/try/download/community下载完成后解压放到指定盘符,比如:C:\mongodb2.配置现在mongodb文件夹下创建data文件夹,再在data下创建db和log文件夹以管理员身份运行cmd,进入bin文件夹cd c:\mongodb\bin启动注册:mongod --

AVL树-平衡二叉树_C++辅导袁老师的博客-程序员秘密

平衡二叉树是高度平衡的二叉树:1 左右子树的高度差最多为1.2 主要的实现地方是插入平衡和删除平衡。3 为了实现平衡,每个节点保存了一个高度h成员。4 当插入和删除破坏了平衡的时候需要进行旋转;5 根据左右子树高度差的不同进行四中不同的旋转:左左、右右、左右、右左百度云下载sln文件:https://pan.baidu.com/s/1pxPGQDYhbcG-E6sE26Wf...

数据结构---并查集_inferno devil的博客-程序员秘密

1.01 并查集:并查集是一种树型的数据结构,用于处理一些不相交集合(Disjoint Sets)的合并及查询问题。常常在使用中以森林来表示。转的一个超级有意思,好懂的并查集解释, 膜拜大神~~并查集:由孩子指向父亲的树结构,主要解决连接问题网络中结点的连接状态网络是一个抽象的概念:用户之间形成的社交网络商品,图书,音乐,火车…很多很多数学中的集合类实现 并集 查询连...

随便推点

机器学习----使用python仿写theano中RBM的源代码_liugan5371375的博客-程序员秘密

# -*- coding: utf-8 -*-'''Created on 2016年4月1日@author: LIU'''import sysimport numpyimport matplotlib.pylab as pltimport numpy as npimport randomfrom scipy.linalg import normimport PIL.Imagef

java书面_Java程序猿的书面采访String3_李白涛的博客-程序员秘密

public class SameString {//思想二:每个字符都相应着自己的ASC码,第一个思想的算法复杂度为O(nlogn)。一般能够利用空间来减少时间复杂度//能够开辟一个大小为256的数组空间,而且将256个数组元素都置为0,然后遍历第一个字符串把字符的ASC作为数组下标。数组元素都加1,//然后遍历第二个字符串将数组元素的各个值都减1,假设最后数组元素的值为0的话说明就是同样的字符...

优化Elasticsearch 每个索引应该有多少个分片?_索引碎片总计多少个_该用户快成仙了的博客-程序员秘密

大多数Elasticsearch用户在创建索引时的一个关键问题是“我应该使用多少个分片?”在本文中,我将介绍在分片分配时的一些权衡以及不同设置带来的性能影响.。如果你想学习如何神秘化和优化你的分片策略请继续阅读。为什么优化?这是一个重要的话题, 很多用户对如何分片都有所疑惑, 有个最好的理由就是. 在生产环境中, 随着数据集的不断增长, 不合理的分配策略可能会给系统的扩展带来严重的问题.同时, 这方面的文档介绍也非常少. 很多用户只想要明确的答案-他们需要特定的答案,而不是模糊的数字范围和任

Java架构师职位常见面试题,看完面试不再慌!(未完待续...)_图灵程序员的博客-程序员秘密

一、架构师的日常职责是什么 ?总体而言,架构师负责软件领域的顶层设计。架构师需要根据公司的发展,规划企业未来若干年的架构,制定可落地的架构方案,解决技术难题,做技术选型与攻关,落地具体的架构。优秀的架构师既能做架构方案,也能写具体的架构代码。二、开发工程师和架构师有何区别?工作重点不同:架构师重点在于前期的架构规划,需要制定可落地的架构方案,结合公司的业务场景、团队的技术水平等因素做技术选型,解决技术难题等等;而开发工程师重点在于具体的落地,特别的, 开发工程师的工作重点落地具体的功能。能力要求不同

SDK如何选择--NRF52840/52832/52810/NRF52805和NRF51822/51802_Wang13631676419的博客-程序员秘密

SDK和Softdevice的区别是什么?怎么选择SDK和softdevice版本?芯片,SDK和softdevice有没有版本兼容问题?一般推荐大家使用nRF5 SDK来开发Nordic nRF51/nRF52系列产品。从形式上来说,nRF5 SDK其实就是一个产品压缩包,如下:除了nRF5 SDK,Nordic还针对某些特殊应用领域推出了一些专门的SDK,这些SDK和nRF5 SDK采用了相同的软件架构,相同的驱动和库,以及相同的编码风格。对开发者来说,只要熟悉了nRF5 SDK,这些特殊SDK上

初学者利用git 上传代码到Coding的简单操作步骤_aoyanliu4278的博客-程序员秘密

1.首先登陆coding网站注册账号https://coding.net/(注册完后需登陆邮箱激活邮件)2.登陆刚注册的coding账号 ,添加项目添加项目—〉输入项目名称—〉输入对项目的简单描述---〉选择"公开"—〉创建项目到此为止,后面需要将本次实验内容push到该项目chap1下。此处,需要记录http://git.co...