【SLAM学习】基于Pangolin绘制运动轨迹

Pangolin是一个轻量级的跨平台视图控制库，主要用于可视化、交互和调试三维数据。该库提供了一系列图形界面工具，包括窗口、OpenGL渲染器、3D相机、图像显示等，可以方便地进行三维数据可视化和交互。

Pangolin库的主要特点如下：

1. 轻量级：Pangolin库的代码量很少，只依赖于少量的第三方库，因此非常轻量级，可以很容易地集成到其他项目中。

2. 跨平台：Pangolin库支持跨平台开发，可以在Windows、Linux和Mac OS等操作系统上使用。

3. 多功能：Pangolin库提供了丰富的图形界面工具，包括窗口、OpenGL渲染器、3D相机、图像显示等，可以满足不同需求的开发者。

4. 易于使用：Pangolin库的接口简单易用，可以快速搭建三维数据可视化和交互界面，同时还提供了丰富的示例程序和文档，方便开发者上手使用。

5. 开源免费：Pangolin库是开源的，使用MIT许可证发布，可以免费使用、修改和分发。

Pangolin库下载：

https://github.com/stevenlovegrove/Pangolin/tree/1ec721d59ff6b799b9c24b8817f3b7ad2c929b83

百度网盘下载链接：链接: https://pan.baidu.com/s/1W75vsmctFheksPfExzpU4A 提取码: rwg2 

// pangolin库
#include <pangolin/pangolin.h>
// Eigen库
#include <Eigen/Core>
#include <Eigen/Dense>

#include <unistd.h>

#include <string>
#include <vector>
#include <fstream>

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

std::string trajectory_file = "./trajectory.txt"; // 轨迹文件
// 格式为：time,tx,ty,tz,qx,qy,qz,qw


void DrawTrajectory(std::vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>> poses){
    // 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); // 设置混合函数

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000), //投影矩阵
        // 屏幕的宽度、高度、相机的水平视角、垂直视角、相机在z轴上的位置、相机到屏幕的距离的最小值和最大值。
        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, 0.0, 1.0, -1024.0f / 768.0f)
        // 表示窗口在x轴和y轴上的起点和终点位置，以及窗口的宽高比，宽高比为负数，则实际上是768：1024
        .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(); i++) {
            // 画每个位姿的三个坐标轴
            Eigen::Vector3d Ow = poses[i].translation(); // 获取相机位姿矩阵中的平移部分，即相机的位置。
            Eigen::Vector3d Xw = poses[i] * (0.1 * Eigen::Vector3d(1, 0, 0)); // 获取x轴方向的单位向量,乘以0.1是为了调整坐标轴线段的长度
            Eigen::Vector3d Yw = poses[i] * (0.1 * Eigen::Vector3d(0, 1, 0)); // 获取y轴方向的单位向量
            Eigen::Vector3d Zw = poses[i] * (0.1 * Eigen::Vector3d(0, 0, 1)); // 获取z轴方向的单位向量
            
            glBegin(GL_LINES); // 开始绘制线段
            glColor3f(1.0, 0.0, 0.0); // 设置线段颜色 rgb
            // 绘制线段的两个端点
            glVertex3d(Ow[0], Ow[1], Ow[2]); // 原点的坐标
            glVertex3d(Xw[0], Xw[1], Xw[2]); // x轴方向的坐标    ----> 绘制x轴线段 为红色

            glColor3f(0.0, 1.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);// 原点的坐标
            glVertex3d(Yw[0], Yw[1], Yw[2]);// y轴方向的坐标    ----> 绘制y轴线段 为绿色

            glColor3f(0.0, 0.0, 1.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);// 原点的坐标
            glVertex3d(Zw[0], Zw[1], Zw[2]);// z轴方向的坐标    ----> 绘制z轴线段 为蓝色
            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();
        }
        pangolin::FinishFrame(); // 结束当前帧的绘制
        usleep(5000);   // sleep 5ms ，每5ms绘画一次

    }
}

int main(int argc,char** argv)
{
    std::vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>> poses;
    
    std::ifstream fin(trajectory_file);
    if(!fin){
        std::cout << "cannot find trajectory file at " << trajectory_file << std::endl;
        return -1;
    }

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

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

 CMakeLists.txt文件：

# 声明要求的cmake最低版本
cmake_minimum_required(VERSION 2.8)

# 声明一个cmake工程
project(Examples)

find_package(Pangolin REQUIRED)

# 添加头文件
include_directories(
    "/usr/include/eigen3"
    ${Pangolin_INCLUDE_DIRS}
)

# 添加可执行程序
add_executable(plotTrajectory plotTrajectory.cc)
target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES})