使用 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} )
画出效果图如下所示
