plotTrajectory

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


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

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "./src/ch01/src/trajectory.txt";
//定义一个画轨迹的函数
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) 
{
  setlocale(LC_ALL,"");
  ros::init(argc,argv,"plotTrajectory");

  // Isometry3d 欧式变换矩阵   aligned_allocator用来分配内存空间 。
  // 定义容器  装poses（r到world坐标的变换矩阵）。
  // Eigen::aligned_allocator<Isometry3d> Eigen管理内存和C++11中的方法是不一样的，
  // 所以需要单独强调元素的内存分配和管理。
  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;


  ifstream fin(trajectory_file);  //输入文件
  if (!fin)   // fin = false
  {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }
  //eof()函数可以帮助我们用来判断文件是否为空，或是判断其是否读到文件结尾。
  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 赋值四元数
    Twr.pretranslate(Vector3d(tx, ty, tz));  // Twr 设置平移向量
    poses.push_back(Twr);
  }
  cout << "read total " << poses.size() << " pose entries" << endl;

  // 调用DrawTrajectory()函数
  DrawTrajectory(poses);
  return 0;
}

/*****************************************************************************/
//创建 pangolin 窗口并绘制轨迹
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses)
{
  // 创建一个名为"trajectory viewer"GUI图形界面窗口，大小为1024*768；
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768); // 启动窗口，并设置尺寸
  // glEnable()用于启用各种功能。功能由参数决定。
  // glEnable(GL_DEPTH_TEST)启用了之后，OpenGL在绘制的时候就会检查，当前像素前面是否有别的像素，
  // 如果别的像素挡道了它，那它就不会绘制，也就是说，OpenGL就只绘制最前面的一层。
  // 当我们需要绘制透明图片时，就需要关闭它glDisable(GL_DEPTH_TEST);
  // 并且打开混合 glEnable(GL_BLEND);
  glEnable(GL_DEPTH_TEST);  // GL_DEPTH_TEST启用深度测试。
  glEnable(GL_BLEND); // GL_BLEND启用颜色混合,后面跟着使用glBlendFunc()。

  // 启动混合
  // void glBlendFunc(GLenum sfactor, GLenum dfactor)
  // 源因子和目标因子是可以通过glBlendFunc函数来进行设置的。
  // glBlendFunc有两个参数，前者sfactor表示源因子，后者dfactor表示目标因子。
  // 前者sfactor表示源颜色，后者dfactor表示目标颜色
  // GL_SRC_ALPHA：表示使用源颜色的alpha值来作为因子
  // GL_ONE_MINUS_SRC_ALPHA：表示用1.0减去源颜色的alpha值来作为因子（1-alpha)
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  // OpenGlRenderState 创建一个相机的观察视图，模拟相机
  // 定义投影和初始模型视图矩阵
  // 创建一个观察相机视图
  // ProjectMatrix(int h, int w, int fu, int fv, int cu, int cv, int znear, int zfar) 
  // ProjectionMatrix 前两个参数是相机的宽高，紧接着四个参数是相机的内参，最后两个是最近和最远视距
  // ModelViewLookAt(double x, double y, double z,double lx, double ly, double lz, AxisDirection Up)
  // ModelViewLookAt 前三个参数是相机的位置,紧接着三个是相机所看的视点的位置,最后三个参数是一个向量,表示相机的的朝向
  pangolin::OpenGlRenderState s_cam(  
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );
  // 在窗口创建交互式视图   //创建视角窗口
  // SetBounds() 前四个参数是视图在视窗中的范围（下 上 左 右）
  // SetHandle 设置相机的视图句柄，需要用它来显示前面设置的 “相机” 所 “拍摄” 的内容
  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));

  // ShouldQuit()检测你是否关闭OpenGL窗口
  while (pangolin::ShouldQuit() == false) 
  {
    // 清除屏幕（颜色缓冲、深度缓冲）
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);  // 启动相机 激活显示，并设置状态矩阵
    // 背景设置为白色，glClearColor(red, green, blue, alpha）数值范围(0, 1)
    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(); // 取出平移向量
      // 画出X轴，0.1是缩小0.1倍。这个地方有点小难理解，最好写程序测试一下，
      // 此处是旋转矩阵T*(1,0,0),结果是旋转后的X轴位置。
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0)); 
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0)); // 画出Y轴
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1)); // 画出Z轴
      glBegin(GL_LINES);  // 开始画线,GL_LINES：线,GL_POINTS：点
      glColor3f(1.0, 0.0, 0.0); // 设置x轴的颜色
      glVertex3d(Ow[0], Ow[1], Ow[2]);  // 设置线起始点
      glVertex3d(Xw[0], Xw[1], Xw[2]);  // 设置线结束点 把两个点连线，后面同理
      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(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();
    }
    //运行帧循环以推进窗口事件,在绘制完成后，需要使用FinishFrame命令刷新视窗。
    pangolin::FinishFrame();
    usleep(5000);   // 休眠5ms

  }
}