高博课程第三次作业：轨迹的误差

  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]<<"\t";
 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})