我为你准备了一个轨迹文件(code/trajectory.txt)。该文件的每一行由若干个数据组成,格式为
[t, t x , t y , t z , q x , q y , q z , q w ],
其中 t 为时间,t x , t y , t z 为 T W C 的平移部分,q x , q y , q z , q w 是四元数表示的 T W C 的旋转部分,q w
为四元数实部。同时,我为你提供了画图程序 draw_trajectory.cpp 文件。该文件提供了画图部分
的代码,请你完成数据读取部分的代码,然后书写 CMakeLists.txt 以让此程序运行起来。注意我
们需要用到 Pangolin 库来画图,所以你需要事先安装 Pangolin(如果你做了第一次作业,那么现
在已经安装了)。CMakeLists.txt 可以参照 ORB-SLAM2 部分。
代码如下
1 #include <sophus/se3.h> 2 #include <string> 3 #include <iostream> 4 #include <fstream> 5 6 // need pangolin for plotting trajectory 7 #include <pangolin/pangolin.h> 8 9 using namespace std; 10 11 // path to trajectory file 12 string trajectory_file = "./trajectory.txt"; 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("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++) 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 }
CMakeLists.txt文件如下所示
1 cmake_minimum_required( VERSION 2.8 ) 2 3 project(trajectory.txt) 4 5 set( CMAKE_BUILD_TYPE "Debug" ) 6 #是 O3 不是 03 7 set( CMAKE_CXX_FLAGS "-std=c++11 -O3" ) 8 9 10 11 find_package( Sophus REQUIRED) 12 find_package( Pangolin REQUIRED) 13 14 15 include_directories( "/usr/include/eigen3" ) 16 include_directories( ${Sophus_INCLUDE_DIRS} ) 17 include_directories( ${Pangolin_INCLUDE_DIRS} ) 18 19 add_executable( draw_trajectory draw_trajectory.cpp) 20 21 target_link_libraries( draw_trajectory ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES} )
结果如图
