使用Pangolon在同一副图中，画出两个轨迹，比较误差

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

画出效果图如下所示