使用Pangolon在同一副图中，画出两个轨迹，比较误差_将slam2多个轨迹绘制在同一张图

转载：https://www.cnblogs.com/feifanrensheng/p/8628900.html

使用 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

代码如下：

  1 // trajectory_compare.cpp created by zhang ning 2018/3/23
  2 #include <sophus/se3.h>
  3 #include <string>
  4 #include <iostream>
  5 #include <fstream>
  6 
  7 // need pangolin for plotting trajectory
  8 #include <pangolin/pangolin.h>
  9 
 10 using namespace std;
 11 
 12 
 13 // function for plotting trajectory, don't edit this code
 14 // start point is red and end point is blue
 15 void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
 16 
 17 int main(int argc, char **argv) {
 18 
 19     vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1,poses2;
 20 
 21     /// implement pose reading code
 22     // start your code here (5~10 lines)
 23     ifstream infile;
 24     infile.open("../data/estimated.txt");
 25     if(!infile) cout<<"error"<<endl;
 26     
 27     cout<<"存入数组"<<endl;   //先将文件中的数据存入到一个二维数组中
 28     double data;
 29     double a[612][8];
 30     double *p=&a[0][0];
 31     while(infile>>data)             //遇到空白符结束
 32     {
 33         *p=data;
 34          p++;
 35     }
 36     infile.close();
 37     for(int i=0;i<620;i++)   //分别对每一行数据生成一个变换矩阵，然后存入动态数组poses中
 38     {    
 39     Eigen::Quaterniond q1 = Eigen::Quaterniond(a[i][7],a[i][4],a[i][5],a[i][6]);
 40         Eigen::Vector3d t1;
 41     t1<<a[i][1],a[i][2],a[i][3];
 42         Sophus::SE3 SE3_qt1(q1,t1);
 43         poses1.push_back(SE3_qt1);
 44     }
 45     ifstream truth;
 46     truth.open("../data/groundtruth.txt");
 47     if(!truth) cout<<"error"<<endl;
 48     
 49     cout<<"存入数组"<<endl;   //先将文件中的数据存入到一个二维数组中
 50     double data1;
 51     double b[612][8];
 52     double *p1=&b[0][0];
 53     while(truth>>data1)             //遇到空白符结束
 54     {
 55         *p1=data1;
 56          p1++;
 57     }
 58     truth.close();
 59     for(int i=0;i<620;i++)   //分别对每一行数据生成一个变换矩阵，然后存入动态数组poses中
 60     {    
 61     Eigen::Quaterniond q2 = Eigen::Quaterniond(b[i][7],b[i][4],b[i][5],b[i][6]);
 62         Eigen::Vector3d t2;
 63     t2<<b[i][1],b[i][2],b[i][3];
 64         Sophus::SE3 SE3_qt2(q2,t2);
 65         poses2.push_back(SE3_qt2);
 66     }
 67     // end your code here
 68 
 69     // draw trajectory in pangolin
 70     DrawTrajectory(poses1,poses2);
 71     return 0;
 72 }
 73 
 74 /*******************************************************************************************/
 75 void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses1,vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses2) {
 76     if (poses1.empty() || poses2.empty() ) {
 77         cerr << "Trajectory is empty!" << endl;
 78         return;
 79     }
 80 
 81     // create pangolin window and plot the trajectory
 82     //创建一个窗口
 83     pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
 84     //启动深度测试
 85     glEnable(GL_DEPTH_TEST);
 86     //启动混合
 87     glEnable(GL_BLEND);
 88     //混合函数glBlendFunc( GLenum sfactor , GLenum dfactor );sfactor 源混合因子dfactor 目标混合因子
 89     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 90     // Define Projection and initial ModelView matrix
 91     pangolin::OpenGlRenderState s_cam(
 92             pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
 93       //对应的是gluLookAt,摄像机位置,参考点位置,up vector(上向量)      
 94             pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
 95     );
 96 
 97     pangolin::View &d_cam = pangolin::CreateDisplay()
 98             .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
 99             .SetHandler(new pangolin::Handler3D(s_cam));
100 
101 
102     while (pangolin::ShouldQuit() == false) {
103         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
104 
105         d_cam.Activate(s_cam);
106         glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
107 
108 
109         glLineWidth(2);
110         for (size_t i = 0; i < poses1.size() - 1; i++) {
111             glColor3f(1 - (float) i / poses1.size(), 0.0f, (float) i / poses1.size());
112             glBegin(GL_LINES);
113             auto p1 = poses1[i], p2 = poses1[i + 1];
114             glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
115             glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
116             glEnd();
117         }
118         
119         for (size_t j = 0; j < poses2.size() - 1; j++) {
120             glColor3f(1 - (float) j / poses2.size(), 0.0f, (float) j / poses2.size());
121             glBegin(GL_LINES);
122             auto p3 = poses2[j], p4 = poses2[j + 1];
123             glVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);
124             glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);
125             glEnd();
126         
127         }
128         pangolin::FinishFrame();
129         usleep(5000);   // sleep 5 ms
130     }
131 
132 }

#CMakeLists.txt
# writed by zhang ning 2018/3/22
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} )

画出效果图如下所示