使用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 的位姿。我们用红、绿、蓝三种颜色画出每个位姿的三 个坐标轴（实际上我们计算了各坐标轴的世界坐标），然后用黑色线将轨迹连起来。