使用pangolin库画出轨迹

https://github.com/stevenlovegrove/Pangolin

cmake_minimum_required(VERSION 2.8)
project(chapter3)

set(CMAKE_CXX_STANDARD 11)
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 1;
  }

  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 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<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),
    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)
    .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++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      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();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}

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

原文地址:https://www.cnblogs.com/long5683/p/11988220.html

时间: 2024-10-25 06:03:28

使用pangolin库画出轨迹的相关文章

原来python还能这么用,使用turtle库画出漂亮的画!

Tuttle库是python内置库,今天就利用它画出各种漂亮的图! 如何画出多重五角星? 效果展示如下:是不是很酷呢,下面还有更酷的玩法.多重五角星的代码比较简单,用到了循环跟turtle库一些简单的方法就实现了. 如何画出一颗漂亮的树呢? 效果展示如下:是不是可以用beatiful来形容,自己一个个敲出来更有成就感,赶紧动手试一下吧!!! 还有更高阶的玩法:加入一些花瓣是不是感觉像自己完成的一幅佳作,以前要用笔画几个小时,现在你只需要几行代码就可以完成一幅漂亮的画: 欢迎点击右上角关注?转发,

Python中如何用turtle库画出小猪佩奇社会人

首先送上传送门: https://www.zhihu.com/question/275611095/answer/382959285 接下来贴代码 1 from turtle import * 2 3 def nose(x,y):#鼻子 4 penup()#提起笔 5 goto(x,y)#定位 6 pendown()#落笔,开始画 7 setheading(-30)#将乌龟的方向设置为to_angle/为数字(0-东.90-北.180-西.270-南) 8 begin_fill()#准备开始填充

如何在论文中画出漂亮的插图?

知乎用户的回答(1259票)]: 强烈推荐 Python 的绘图模块 matplotlib: python plotting .画出来的图真的是高端大气上档次,低调奢华有内涵~ 适用于从 2D 到 3D,从标量到矢量的各种绘图.能够保存成从 eps, pdf 到 svg, png, jpg 的多种格式.并且 Matplotlib 的绘图函数基本上都与 Matlab 的绘图函数名字都差不多,迁移的学习成本比较低.开源免费.如图所示(题目描述中的图在最后): (以下图片均引用自 Thumbnail

如何在控制台下画出一个正线曲线

昨晚一个同学在网上找了一段傅里叶变换的代码,但是需要验证代码的正确性.于是就打算生成一个正弦波.他找到了我,代码本身是没有难度的,因为基本任何语言都有math库,库中肯定有三角函数.我当时想,能不能在Windows的控制台下画出一个正弦波.需要解决的问题只有一个,如何控制Console的光标位置.所幸的是,Windows提供了这样的API给我们使用.在windows.h的头文件中,有这样几个函数. HANDLE WINAPI GetStdHandle( _In_ DWORD nStdHandle

精华总结:网络工程师必备技能-如何用PPT画出精美拓扑视频教程

具备5年IT从业经验,资深IT经理,项目经理,产品经理.精通思科.H3C.华为.深信服.信锐.迪普.网康.Juniper.山石等网络设备:精通虚拟化.云计算.云桌面.服务器.存储.数据备份等项目实施交付,具有丰富的网络规划设计.项目实施经验. 课程目标本课程讲解如何用PPT画出精美拓扑,帮助大家解除画不出漂亮拓扑的困扰,免除被领导批评的痛苦,加速职位晋升,PPT提供华为.H3C.思科.深信服常用图标库,同时直接提供常用场景网络拓扑原图,可以直接修改或者直接使用,方便快捷!课程链接http://e

matlplotlib根据函数画出图形

根据函数画出函数的轨迹 import matht = np.linspace(0, math.pi, 1000)x = np.sin(t)y = np.cos(t) + np.power(x, 2.0/5)plt.plot(x, y, color='red', linewidth=1, label='h')plt.plot(-x, y, color='red', linewidth=1, label='-h')plt.xlabel('t')plt.ylabel('h')plt.ylim(-2,

Python3 Tkinter基础 Canvas bind 绑定左键 鼠标左键点击时,在当前位置画出一个椭圆形

镇场诗: 清心感悟智慧语,不着世间名与利.学水处下纳百川,舍尽贡高我慢意. 学有小成返哺根,愿铸一良心博客.诚心于此写经验,愿见文者得启发.------------------------------------------ code: from tkinter import * root=Tk() w=Canvas(root,width=200,height=200,background='white') w.pack() def paint(event): #event.x 鼠标左键的横坐标

在资源里面画出你的界面

在资源里面画出你的界面.在添加类.然后在视图里定义这个类的指针,DIALOG的指针,还有就是区域对象, 如下Cwindows *tw;  CRect m_cle;CDialogEx *ph;在CPP文件构造函数里初始化ZeroMemory(&m_cle,sizeof(m_cle));tw = new Cwindows(this);:在重写OnCreate(LPCREATESTRUCT lpCreateStruct)函数在里面加上 { if (CView::OnCreate(lpCreateStr

全面总结:MATLAB如何画出漂亮的图

在绘图命令中应该如何确定横坐标和纵坐标的标度尺寸(自己定范围) 如何在一个坐标中绘制多条曲线,并用不同的颜色区别. 希望得到答案,谢谢!!!!!!!!!!! help plot help axis axis ('xlabel','ylabel') a1=plot(); hlod on a2=plot(); legend([a1 a2],'图1名',‘图2名') hold off 还可以更多 x1=-pi:pi/12:pi; x2=-pi:pi/12:pi; y1=sin(x1); y2=cos(