轉載：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} )
 

畫出效果圖如下所示