7 * 轨迹的误差
在根目录下新建了build文件 把数据文件放入其中 ，编写了估计误差的ground_error.cpp文件和CMakeLists.txt 文件，代码如下
1 //ground_error.cpp writed by zhang ning 2018/3/15 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 14 // function for plotting trajectory, don't edit this code 15 // start point is red and end point is blue 16 void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>); 17 18 int main(int argc, char **argv) { 19 20 vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses; 21 22 /// implement pose reading code 23 // start your code here (5~10 lines) 24 25 ifstream infile; 26 infile.open("../data/trajectory.txt"); 27 if(!infile) cout<<"error"<<endl; 28 29 cout<<"存入数组"<<endl; //先将文件中的数据存入到一个二维数组中 30 double data; 31 double a[620][8]; 32 double *p=&a[0][0]; 33 while(infile>>data) //遇到空白符结束 34 { 35 *p=data; 36 p++; 37 } 38 infile.close(); 39 for(int i=0;i<620;i++) 40 { for(int j=0;j<8;j++) 41 cout<<a[i][j]<<" "; 42 cout<<endl; 43 } 44 for(int i=0;i<620;i++) //分别对每一行数据生成一个变换矩阵，然后存入动态数组poses中 45 { 46 Eigen::Quaterniond q = Eigen::Quaterniond(a[i][7],a[i][4],a[i][5],a[i][6]); 47 Eigen::Vector3d t; 48 t<<a[i][1],a[i][2],a[i][3]; 49 Sophus::SE3 SE3_qt(q,t); 50 //cout<<SE3_qt<<endl; 51 poses.push_back(SE3_qt); 52 } 53 // end your code here 54 55 // draw trajectory in pangolin 56 DrawTrajectory(poses); 57 return 0; 58 } 59 60 61 void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) { 62 if (poses.empty()) { 63 cerr << "Trajectory is empty!" << endl; 64 return; 65 } 66 67 // create pangolin window and plot the trajectory 68 pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768); 69 glEnable(GL_DEPTH_TEST); 70 glEnable(GL_BLEND); 71 glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); 72 73 pangolin::OpenGlRenderState s_cam( 74 pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000), 75 pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0) 76 ); 77 78 pangolin::View &d_cam = pangolin::CreateDisplay() 79 .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f) 80 .SetHandler(new pangolin::Handler3D(s_cam)); 81 82 83 while (pangolin::ShouldQuit() == false) { 84 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 85 86 d_cam.Activate(s_cam); 87 glClearColor(1.0f, 1.0f, 1.0f, 1.0f); 88 89 glLineWidth(2); 90 for (size_t i = 0; i < poses.size() - 1; i++) { 91 glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size()); 92 glBegin(GL_LINES); 93 auto p1 = poses[i], p2 = poses[i + 1]; 94 glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]); 95 glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]); 96 glEnd(); 97 } 98 pangolin::FinishFrame(); 99 usleep(5000); // sleep 5 ms 100 } 101 102 }
1 # writed by zhang ning 2018/3/15 2 cmake_minimum_required( VERSION 2.8 ) 3 4 project(trajectory.txt) 5 6 set( CMAKE_BUILD_TYPE "Debug" ) 7 8 set( CMAKE_CXX_FLAGS "-std=c++11 -O3" ) 9 10 11 12 find_package( Sophus REQUIRED) 13 find_package( Pangolin REQUIRED) 14 15 16 include_directories( "/usr/include/eigen3" ) 17 include_directories( ${Sophus_INCLUDE_DIRS} ) 18 include_directories( ${Pangolin_INCLUDE_DIRS} ) 19 20 add_executable( draw_trajectory draw_trajectory.cpp) 21 22 target_link_libraries( draw_trajectory ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES} ) 23 24 add_executable( ground_error ground_error.cpp) 25 target_link_libraries( ground_error ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES})
运行截图如下，与参考答案一致
