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

 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} )