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

这篇具有很好参考价值的文章主要介绍了【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 

cd Pagolin
mkdir build 
cd build
cmake ..
make

Eigen库是一个开源的C++线性代数库,它提供了快速的有关矩阵的线性代数运算,还包括解方程等功能。但没有提供李代数的支持。

Sophus库提供一个较好的李群和李代数的库,它很好的支持了SO(3),so(3),SE(3)和se(3)。是基于Eigen基础上开发的,继承了Eigen库中的定义的各个类。

安装Eigen库

sudo apt install libeigen3-dev

Eigen 头文件路径默认安装在:

"/usr/include/eigen3/"

注:Eigen是一个纯用头文件搭建起来的库,因此不需要链接库文件,只需要引入Eigen的头文件即可,后续的CMakeLists.txt 文件有体现到。

安装Sophus库

可以通过GitHub下载最新的:(现在是1.22.10版本)

git clone https://github.com/strasdat/Sophus.git

也可以通过百度网盘:(1.0.0)

链接: https://pan.baidu.com/s/1aCJzvDNRAfz_VEtH47DslA 提取码: xtak 

安装: 

cd Sophus
mkdir build 
cd build
cmake ..
make

若出现下面问题:

  Could not find a package configuration file provided by "fmt" with any of
  the following names:

    fmtConfig.cmake
    fmt-config.cmake

则需要再安装一个fmt库

git clone https://github.com/fmtlib/fmt.git
cd fmt
mkdir build
cd build
cmake ..
make
sudo make install

使用Pangolin绘制轨迹 

// 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})

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

估计轨迹的误差

在实际工程当中,需要估计一个算法估计轨迹(estimate trajectory)和真实轨迹(groundtruth trajectory) 的差异来评价算法的精度。

绝对轨迹误差(Absolute Trajectory Error,ATE

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

这实际上是每个位姿李代数的均方根误差(Root-Mean-Squared Error,RMSE

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <string>
#include <vector>

// Pangolin库
#include <pangolin/pangolin.h>

// Sophus库
#include <sophus/se3.hpp>

// 轨迹文件路径
std::string groundtruth_file = "./groundtruth.txt";
std::string estimated_file = "./estimated.txt";

using TrajectoryType = std::vector<Sophus::SE3d,Eigen::aligned_allocator<Sophus::SE3d> >;

// 读取轨迹txt文件
TrajectoryType ReadTrajectory(const std::string &path) {
  std::ifstream fin(path);
  TrajectoryType trajectory;
  if (!fin) {
    std::cerr << "trajectory " << path << " not found." << std::endl;
    return trajectory;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
    trajectory.push_back(p1);
  }
  return trajectory;
}

// 绘制两条轨迹(一个是真实轨迹,一个是估计轨迹)
void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti){
    // 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 < gt.size() - 1; i++) {
            glColor3f(0.0, 0.0, 1.0); // 蓝色
            glBegin(GL_LINES); // 开始绘制线段
            auto p1 = gt[i], p2 = gt[i + 1]; // 获取相邻相机位姿
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]); // 绘制线段的两个端点(相邻相机位姿的位置)
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }

        // 画出估计轨迹
        for (size_t i = 0; i < esti.size() - 1; i++) {
            glColor3f(1.0, 0.0, 0.0); // 红色
            glBegin(GL_LINES); // 开始绘制线段
            auto p1 = esti[i], p2 = esti[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)
{
    TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
    TrajectoryType estimated = ReadTrajectory(estimated_file);

    // assert(!groundtruth.empty() && !estimated.empty());
    // assert(groundtruth.size() == estimated.size());

    if(groundtruth.empty() && estimated.empty() && (groundtruth.size() == estimated.size())){
        // 判断读取的真实轨迹文件和估计轨迹文件是否为空,并且判断两个文件的数据大小是否一致
        std::cout << "groudtruth file or estimated_file is error!" << std::endl;
        return -1;
    }

    // 计算ATE(绝对轨迹误差)
    double ate = 0;
    for (size_t i = 0; i < estimated.size();i++){
        Sophus::SE3d p1 = estimated[i];
        Sophus::SE3d p2 = groundtruth[i];
        double error = (p2.inverse()*p1).log().norm();
        ate += (error*error);
    }
    ate = ate / double(estimated.size());
    ate = sqrt(ate);

    std::cout << "ATE: " << ate << std::endl;

    // 绘制两条轨迹
    DrawTrajectory(groundtruth,estimated);

    return 0;
}

  CMakeLists.txt文件:

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

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

# 查找系统的库路径
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)

# 添加头文件
include_directories("/usr/include/eigen3")

# 添加可执行程序
add_executable(trajectoryError trajectoryError.cc)

# 链接库文件
target_link_libraries(trajectoryError 
    Sophus::Sophus
    ${Pangolin_LIBRARIES}
)

【SLAM学习】基于Pangolin绘制运动轨迹文章来源地址https://www.toymoban.com/news/detail-438521.html

到了这里,关于【SLAM学习】基于Pangolin绘制运动轨迹的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包