高翔视觉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
}
}