使用pangolin库画出轨迹

https://github.com/stevenlovegrove/Pangolin

cmake_minimum_required(VERSION 2.8)
project(chapter3)

set(CMAKE_CXX_STANDARD )
include_directories("/usr/include/eigen3")

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(plotTrajectory plotTrajectory.cpp)
target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES})

#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>
#include <fstream>
// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

// path to trajectory file
string trajectory_file = "/home/qian/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 ;
  }

  while (!fin.eof()) {  //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 ;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", , );
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(, , , , , , 0.1, ),
    pangolin::ModelViewLookAt(, -0.1, -1.8, , , , 0.0, -1.0, 0.0)
  );

  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));

  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();
    for (size_t i = ; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(, , ));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(, , ));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(, , ));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[], Ow[], Ow[]);
      glVertex3d(Xw[], Xw[], Xw[]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[], Ow[], Ow[]);
      glVertex3d(Yw[], Yw[], Yw[]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[], Ow[], Ow[]);
      glVertex3d(Zw[], Zw[], Zw[]);
      glEnd();
    }
    // 画出连线
    for (size_t i = ; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + ];
      glVertex3d(p1.translation()[], p1.translation()[], p1.translation()[]);
      glVertex3d(p2.translation()[], p2.translation()[], p2.translation()[]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep();   // sleep 5 ms
  }
}

该程序演示了如何在 Panglin 中画出 3D 的位姿。我们用红、绿、蓝三种颜色画出每个位姿的三 个坐标轴（实际上我们计算了各坐标轴的世界坐标），然后用黑色线将轨迹连起来。