高翔视觉SLAM十四讲第三节课的课后作业中，解决由于模板类Sophus而导致的问题，第6题draw_trajectory.cpp新版代码_drawtrajectory

高翔视觉SLAM十四讲，在第三节课的课后作业中，第6题要求补全draw_trajectory.cpp代码（放在和课后作业包中）中的数据读取部分代码，由于课程当年使用的是老版（非模板类）的Sophus库，故代码中很多地方不适用于现在的模板类Sophus库，故将代码修改后如下：

#include <sophus/se3.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
 
using namespace std;
 
// path to trajectory file
string trajectory_file = "./trajectory.txt";   

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>>);
//Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理,上述为标准的vector定义方法，用来存储SE3
int main() {
 
    vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses;
 
    /// implement pose reading code
    // start your code here (5~10 lines)
   


    // end your code here
   

   
    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}
/*******************************************************************************************/
 
void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses) {
    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }
 
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);//创建一个窗口，名字，宽，高
    glEnable(GL_DEPTH_TEST);  //启动深度测试
    glEnable(GL_BLEND);       //启动色彩混合
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);  //最常使用的混合函数
    //glBlendFunc( GLenum sfactor , GLenum dfactor );         // 混合函数
    //sfactor 源混合因子   GL_SRC_ALPHA
    //dfactor 目标混合因子   GL_ONE_MINUS_SRC_ALPHA
 
    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            //对应的是gluLookAt,摄像机位置,参考点位置,up vector(上向量)
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );
 
    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));
 
 
    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 
        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
 
        glLineWidth(2);
        for (size_t i = 0; i < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            //glColor3f(1.0, 0.0, 0.0);  --> 红色 glColor3f(0.0, 0.0, 1.0);  --> 蓝色 
            glBegin(GL_LINES);
            //GL_LINES：   把每个顶点作为一个独立的线段，顶点2n-1和2n之间定义了n条线段，绘制N/2条线段
            auto p1 = poses[i], p2 = poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
 
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84