使用 code/ground-truth.txt 和 code/estimate.txt 两条轨迹。请你根据上面公式,实现 RMSE
的计算代码,给出最后的 RMSE 结果。作为验算,参考答案为:2.207。用上题的画图程序将两条轨迹画在同一个图里,看看它们相差多少。

完整题目描述

ground-truth.txt和estimate.txt放在了源文件夹下的data目录下,编写了用于画图的trajectory_compare.cpp文件

相关代码及程序可在我的github中获取,地址:https://github.com/feifanrensheng/trajectory_compare

代码如下:

 // trajectory_compare.cpp created by zhang ning 2018/3/23
#include <sophus/se3.h>
#include <string>
#include <iostream>
#include <fstream> // need pangolin for plotting trajectory
#include <pangolin/pangolin.h> using namespace std; // 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>>,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>); int main(int argc, char **argv) { vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1,poses2; /// implement pose reading code
// start your code here (5~10 lines)
ifstream infile;
infile.open("../data/estimated.txt");
if(!infile) cout<<"error"<<endl; cout<<"存入数组"<<endl; //先将文件中的数据存入到一个二维数组中
double data;
double a[][];
double *p=&a[][];
while(infile>>data) //遇到空白符结束
{
*p=data;
p++;
}
infile.close();
for(int i=;i<;i++) //分别对每一行数据生成一个变换矩阵,然后存入动态数组poses中
{
Eigen::Quaterniond q1 = Eigen::Quaterniond(a[i][],a[i][],a[i][],a[i][]);
Eigen::Vector3d t1;
t1<<a[i][],a[i][],a[i][];
Sophus::SE3 SE3_qt1(q1,t1);
poses1.push_back(SE3_qt1);
}
ifstream truth;
truth.open("../data/groundtruth.txt");
if(!truth) cout<<"error"<<endl; cout<<"存入数组"<<endl; //先将文件中的数据存入到一个二维数组中
double data1;
double b[][];
double *p1=&b[][];
while(truth>>data1) //遇到空白符结束
{
*p1=data1;
p1++;
}
truth.close();
for(int i=;i<;i++) //分别对每一行数据生成一个变换矩阵,然后存入动态数组poses中
{
Eigen::Quaterniond q2 = Eigen::Quaterniond(b[i][],b[i][],b[i][],b[i][]);
Eigen::Vector3d t2;
t2<<b[i][],b[i][],b[i][];
Sophus::SE3 SE3_qt2(q2,t2);
poses2.push_back(SE3_qt2);
}
// end your code here // draw trajectory in pangolin
DrawTrajectory(poses1,poses2);
return ;
} /*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses2) {
if (poses1.empty() || poses2.empty() ) {
cerr << "Trajectory is empty!" << endl;
return;
} // create pangolin window and plot the trajectory
//创建一个窗口
pangolin::CreateWindowAndBind("Trajectory Viewer", , );
//启动深度测试
glEnable(GL_DEPTH_TEST);
//启动混合
glEnable(GL_BLEND);
//混合函数glBlendFunc( GLenum sfactor , GLenum dfactor );sfactor 源混合因子dfactor 目标混合因子
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
// Define Projection and initial ModelView matrix
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrix(, , , , , , 0.1, ),
//对应的是gluLookAt,摄像机位置,参考点位置,up vector(上向量)
pangolin::ModelViewLookAt(, -0.1, -1.8, , , , 0.0, -1.0, 0.0)
); pangolin::View &d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(), 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();
for (size_t i = ; i < poses1.size() - ; i++) {
glColor3f( - (float) i / poses1.size(), 0.0f, (float) i / poses1.size());
glBegin(GL_LINES);
auto p1 = poses1[i], p2 = poses1[i + ];
glVertex3d(p1.translation()[], p1.translation()[], p1.translation()[]);
glVertex3d(p2.translation()[], p2.translation()[], p2.translation()[]);
glEnd();
} for (size_t j = ; j < poses2.size() - ; j++) {
glColor3f( - (float) j / poses2.size(), 0.0f, (float) j / poses2.size());
glBegin(GL_LINES);
auto p3 = poses2[j], p4 = poses2[j + ];
glVertex3d(p3.translation()[], p3.translation()[], p3.translation()[]);
glVertex3d(p4.translation()[], p4.translation()[], p4.translation()[]);
glEnd(); }
pangolin::FinishFrame();
usleep(); // sleep 5 ms
} }
#CMakeLists.txt
# writed by zhang ning //
cmake_minimum_required( VERSION 2.8 ) project(trajectory_compare) set( CMAKE_BUILD_TYPE "Debug" ) set( CMAKE_CXX_FLAGS "-std=c++11 -O3" ) find_package( Sophus REQUIRED)
find_package( Pangolin REQUIRED) include_directories( "/usr/include/eigen3" )
include_directories( ${Sophus_INCLUDE_DIRS} )
include_directories( ${Pangolin_INCLUDE_DIRS} ) add_executable( trajectory_compare trajectory_compare.cpp) target_link_libraries( trajectory_compare ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES} )

画出效果图如下所示

使用Pangolon在同一副图中,画出两个轨迹,比较误差的更多相关文章

  1. python—networkx:在一张图中画出多个子图

    通过plt.subplot能够在一张图中画出多个子图 #coding: utf-8 #!/usr/bin/env python """ Draw a graph with ...

  2. 机器学习入门-随机森林温度预测-增加样本数据 1.sns.pairplot(画出两个关系的散点图) 2.MAE(平均绝对误差) 3.MAPE(准确率指标)

    在上一个博客中,我们构建了随机森林温度预测的基础模型,并且研究了特征重要性. 在这个博客中,我们将从两方面来研究数据对预测结果的影响 第一方面:特征不变,只增加样本的数据 第二方面:增加特征数,增加样 ...

  3. wpf 在不同DPI下如何在DrawingVisual中画出清晰的图形

    环境Win10 VS2017 .Net Framework4.7.1   本文仅讨论在DrawingVisual中进行的画图.   WPF单位,系统DPI,显示器DPI三者的定义及关系 WPF单位:一 ...

  4. 如何在canvas中画出一个太极图

    先放一个效果图: 代码如下 <!DOCTYPE html> <html> <head> <meta charset="utf-8" /&g ...

  5. matplotlib中在for中画出多张图

    import matplotlib.pyplot as plt import numpy as np fig, axes = plt.subplots(2, 2) def showim(): for ...

  6. Latex中画出函数文件的调用关系拓扑图

    流程图,思维导图,拓扑图通常能把我们遇到的一些复杂的关系结构用图形的方式展现出来.在Latex中要想画这样的拓扑图,有一个很好用的绘图工具包 pgf/tikz . 1.pgf/tikz的安装:pgf/ ...

  7. 百度地图API实时画出动态运行轨迹(一条行驶轨迹),车头实时指向行驶方向,设置角度偏移

    参考网址:https://blog.csdn.net/skywqnan/article/details/79036262 改变车的方向:http://www.cnblogs.com/peixuanzh ...

  8. 使用Eclipse在Excel中找出两张表中相同证件号而姓名或工号却出现不同的的项

    1:首先把Excel中的文本复制到txt中,复制如下: A表: 证件号                           工号  姓名 310110xxxx220130004 101 傅家宜3101 ...

  9. 给定一个值S,在有序数组中找出两个元素A和B,使 A+B = S.

    在网上看到过一个面试题,感觉挺有意思,看别人的代码写的逻辑不够谨慎,重写了一个,较真了又... package com.array7.algorithm; public class Algorithm ...

随机推荐

  1. selenium +chrome headless Manual 模式渲染网页

    可以看看这个里面的介绍,写得很好.https://duo.com/blog/driving-headless-chrome-with-python from selenium import webdr ...

  2. python使用pyqt写带界面工具

    上篇介绍的使用python自带tkinter包,来写带界面的工具. 此篇介绍使用pyqt来开发测试工具. tkinter的好处是python官方自带,上手容易(但手写控件复杂),布局和摆放都不直观和容 ...

  3. 使用Ajax long polling实现简单的聊天程序

    关于web实时通信,通常使用长轮询或这长连接方式进行实现. 为了能够实际体会长轮询,通过Ajax长轮询实现了一个简单的聊天程序,在此作为笔记. 长轮询 传统的轮询方式是,客户端定时(一般使用setIn ...

  4. ios开发之--tableview刷新某一个区和某一行

    在开发中,有时候,我们不需要刷新整个表,只需要刷新局部数据即可,具体代码如下: //section刷新 NSIndexSet *indexSet=[[NSIndexSet alloc]initWith ...

  5. 代码审计之DocCms漏洞分析

    0x01 前言 DocCms[音译:稻壳Cms] ,定位于为企业.站长.开发者.网络公司.VI策划设计公司.SEO推广营销公司.网站初学者等用户 量身打造的一款全新企业建站.内容管理系统,服务于企业品 ...

  6. IIS 7安装ModSecurity实现WAF功能

    ModSecurity 是一款开源Web应用防火墙,支持Apache/Nginx/IIS,可作为服务器基础安全设施,还是不错的选择. 系统环境:window 2008 R2+IIS 7 0X01 Mo ...

  7. CharacterMotor_刚体角色驱动

    using UnityEngine; //this class holds movement functions for a rigidbody character such as player, e ...

  8. lua中的字符串操作(模式匹配)

    (一). 模式匹配函数在string库中功能最强大的函数是:string.find(字符串查找)string.gsub(全局字符串替换)string.gfind(全局字符串查找)string.gmat ...

  9. php curl那点事儿

    curl是最常用功能之一初始化句柄 $ch = curl_init(); post 传$data 1. 如果$data是字符串,则Content-Type是application/x-www-form ...

  10. java十年技术栈[总结复习用]

    以下摘自http://www.tvtv223.com/so/8/default/8.html#36-数据库的分库分表mycat java技术栈 参考了众多资料,这里就不再详细列举了,可以自行去搜索 1 ...