OpenGL的pangolin库绘制3D轨迹图

示例代码

#include <pangolin/pangolin.h>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <unistd.h>

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "/home/lyn/Documents/test_code/slambook2/ch3/examples/trajectory.txt";


void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {

  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  ifstream fin(trajectory_file);
  if (!fin) {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    Twr.pretranslate(Vector3d(tx, ty, tz));
    poses.push_back(Twr);
  }
  cout << "read total " << poses.size() << " pose entries" << endl;

  // draw trajectory in pangolin
  DrawTrajectory(poses);
  return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);  //开启更新深度缓冲区的功能，如果通过比较后深度值发生变化了，会进行更新深度缓冲区的操作。启动它，OpenGL就可以跟踪它在Z轴上的像素了，它只会在那个像素前方没有东西时，才会绘画这个像素。绘画3D图的时候，开启它，视觉效果更真实。
  glEnable(GL_BLEND);  //启用混合，绘制透明或半透明的物体，将原色和目标色按a值比率混合。
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);  //表示源颜色乘以自身的alpha值（GL_SRC_ALPHA), 目标颜色乘以1.0减去源颜色的alpha值，这样一来，源颜色的alpha值越大，则产生的新颜色中源颜色的占比就越大，我们简单的理解为源颜色的alpha的值“不透明度”


  //创建一个观察相机视图      在完成视窗的创建后，我们需要在视窗中“放置”一个摄像机（注意这里的摄像机是用于观察的摄像机而非SLAM中的相机），我们需要给出摄像机的内参矩阵ProjectionMatrix从而在我们对摄像机进行交互操作时，Pangolin会自动根据内参矩阵完成对应的透视变换。此外，我们还需要给出摄像机初始时刻所处的位置，摄像机的视点位置（即摄像机的光轴朝向哪一个点）以及摄像机的本身哪一轴朝上。

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),  //h,w,fu,fv,cu,cv,znear,zfar 相机的参数
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0) //x,y,z,lx,ly,lz 分别为相机所在位置，相机所看到的视点位置（一般在原定）
  );


  //创建交互视图   接下来我们需要创建一个交互式视图（view）用于显示上一步摄像机所“拍摄”到的内容，这一步类似于OpenGL中的viewport处理。setBounds()函数前四个参数依次表示视图在视窗中的范围（下、上、左、右），可以采用相对坐标（0~1）以及绝对坐标（使用Attach对象）。
  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));  //Handler3D 交互相机视图句柄

  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(); i++) { //用红，绿，蓝三种颜色画出每个位姿的三个坐标轴
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1* Vector3d(1, 0, 0)); //0.1代表线长
      Vector3d Yw = poses[i] * ( 0.1 *Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * ( 0.1 *Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);  //画出顶点指向x轴上一点的方向的线段
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);  //线的颜色，黑色 ，如果glBegin()和glEnd()函数之间多次连续调用颜色函数，只会 使用最后一次的颜色
      glBegin(GL_LINES);
      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
  }
}

效果：