视觉SLAM14讲第三讲作业绘制轨迹解析

#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";   //相对路径，上一级文件夹
string estimated_file="/home/nature/file/estimated.txt";
string groundtruth_file="/home/nature/file/groundtruth.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)
    //读取估计轨迹
    ifstream estimated(estimated_file);
    if(!estimated)
    {
        cout<<"can't find file at "<<estimated_file<<endl;
        return 1;
    }
    while(!estimated.eof())
    {
        double t,tx,ty,tz,qx,qy,qz,qw; //qw为实部，t为时间
        estimated>>t>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Eigen::Quaterniond q(qw,qx,qy,qz);
        Eigen::Vector3d v(tx,ty,tz);
        Sophus::SE3d SE3_qt(q,v);  //由q，v构建变换矩阵SE3
        poses.push_back(SE3_qt);   //在Vector最后添加一个元素（参数为要插入的值）
    }
    cout<<"read total "<<poses.size()<<"pose entries"<<endl;
    // end your code here
    ///读取实际轨迹
    ifstream groundtruth(groundtruth_file);
    if(!groundtruth)
    {
        cout<<"can't find file at "<<groundtruth_file<<endl;
        return 1;
    }
    while(!groundtruth.eof())
    {
        double t,tx,ty,tz,qx,qy,qz,qw; //qw为实部，t为时间
        groundtruth>>t>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
        Eigen::Quaterniond q(qw,qx,qy,qz);
        Eigen::Vector3d v(tx,ty,tz);
        Sophus::SE3d SE3_qt1(q,v);  //由q，v构建变换矩阵SE3
        poses.push_back(SE3_qt1);   //在Vector最后添加一个元素（参数为要插入的值）
    }
    cout<<"read total "<<poses.size()<<"pose entries"<<endl;
    // 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
    }

}