使用Pangolin画出相机的轨迹(包括朝向)。

数据集结构data.csv:

 #timestamp, p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z [], v_RS_R_x [m s^-1], v_RS_R_y [m s^-1], v_RS_R_z [m s^-1], b_w_RS_S_x [rad s^-1], b_w_RS_S_y [rad s^-1], b_w_RS_S_z [rad s^-1], b_a_RS_S_x [m s^-2], b_a_RS_S_y [m s^-2], b_a_RS_S_z [m s^-2]
1403636580838555648,4.688319,-1.786938,0.783338,0.534108,-0.153029,-0.827383,-0.082152,-0.027876,0.033207,0.800006,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580843555328,4.688177,-1.786770,0.787350,0.534640,-0.152990,-0.826976,-0.082863,-0.029272,0.033992,0.804771,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580848555520,4.688028,-1.786598,0.791382,0.535178,-0.152945,-0.826562,-0.083605,-0.030043,0.034999,0.808240,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580853555456,4.687878,-1.786421,0.795429,0.535715,-0.152884,-0.826146,-0.084391,-0.030230,0.035853,0.810462,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580858555648,4.687727,-1.786240,0.799484,0.536244,-0.152821,-0.825731,-0.085213,-0.029905,0.036316,0.811406,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580863555328,4.687579,-1.786059,0.803540,0.536768,-0.152768,-0.825314,-0.086049,-0.029255,0.036089,0.811225,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580868555520,4.687435,-1.785881,0.807594,0.537289,-0.152725,-0.824896,-0.086890,-0.028469,0.035167,0.810357,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580873555456,4.687295,-1.785709,0.811642,0.537804,-0.152680,-0.824481,-0.087725,-0.027620,0.033777,0.808910,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593
1403636580878555648,4.687158,-1.785544,0.815682,0.538317,-0.152627,-0.824067,-0.088553,-0.026953,0.031990,0.806951,-0.003172,0.021267,0.078502,-0.025266,0.136696,0.075593

前八项分别为 时间戳,x,y,z,$q_0,q_1,q_2,q_3$。

 #include <iostream>
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <sophus/se3.h>
using namespace std; typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> VecSE3;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVec3d; string file = "./data.csv"; void Draw(const VecSE3 &poses);
int main(int argc, char **argv)
{
//读位姿
VecSE3 poses;
VecVec3d points;
ifstream fin(file);//位姿 string lineStr;
int j = ;
while(getline(fin,lineStr))//每行
{
j+=;//隔100个点取一个数据
if (j% != )
continue; //cout<<lineStr <<endl;
stringstream ss(lineStr);
string str;
double data[];
int i = ;
while (getline(ss, str, ','))
{
stringstream intg(str);
if (i >= )//只取前八个
continue;
intg >> data[i];
i+=;
}
poses.push_back(Sophus::SE3(
//eigen.tuxfamily.org/dox-devel/classEigen_1_1Quaternion.html
Eigen::Quaterniond(data[], data[], data[], data[]),//四元数
Eigen::Vector3d(data[], data[], data[])//位移
));
//cout << data[1] <<" "<< data[2]<<" "<< data[3]<<" "<< data[4]<<endl;//位移
if (!fin.good()) break; }
fin.close(); Draw(poses);
return ; } void Draw(const VecSE3 &poses)
{
if (poses.empty() )
{
cerr << "parameter is empty!" << endl;
return;
}
// 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, pangolin::Attach::Pix(), 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(0.0f, 0.0f, 0.0f, 0.0f);
// draw poses
//画相机朝向
for (auto &Twc: poses)//从poses中取位姿
{
glPushMatrix();
Sophus::Matrix4f m = Twc.matrix().cast<float>();
glMultMatrixf((GLfloat *) m.data()); const float w = 0.25;
const float h = w*0.75;
const float z = w*0.6;
glColor3f(, , );
glLineWidth();
glBegin(GL_LINES);
//画相机模型
glVertex3f(, , );
glVertex3f(w,h,z);
glVertex3f(, , );
glVertex3f(w,-h,z);
glVertex3f(, , );
glVertex3f(-w,-h,z);
glVertex3f(, , );
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(-w,h,z);
glVertex3f(w,h,z);
glVertex3f(-w,-h,z);
glVertex3f(w,-h,z); glEnd();
glPopMatrix();
}
//画轨迹
glLineWidth();
for (size_t i = ; i < poses.size() - ; i++)
{
glColor3f( - (float) i / poses.size(), 0.0f, (float) i / poses.size());
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
}
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( show ) set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" ) list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules ) # 寻找G2O Cholmod eigen3
find_package( G2O REQUIRED )
find_package( Cholmod )
include_directories(
${G2O_INCLUDE_DIRS} ${CHOLMOD_INCLUDE_DIR}
"/usr/include/eigen3"
) # sophus
find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} ) find_package( Pangolin REQUIRED)
include_directories( ${Pangolin_INCLUDE_DIRS} ) add_executable( draw draw.cpp ) target_link_libraries( draw
${CHOLMOD_LIBRARIES}
${Sophus_LIBRARIES}
${Pangolin_LIBRARIES}
)

除此之外,还需添加cmake_modules。

运行结果:

Pangolin库的使用的更多相关文章

  1. 使用pangolin库画出轨迹

    https://github.com/stevenlovegrove/Pangolin cmake_minimum_required(VERSION 2.8) project(chapter3) ) ...

  2. Pangolin的使用

    Pangolin 是一个可用于SLAM可视化的openGL库.目前有很多SLAM系统都用它作为可视化的工具.它的编译与安装过程问题不大,依赖也比较少.但最近我在单独使用它的时候,碰到了很奇怪的现象:我 ...

  3. error: No rule to make target '/usr/lib/libOpenNI.so', needed by 'bin/euroc_rectify'。 停止。

    这类问题的出现说明程序在编译时,CMakeLists.txt 文件没有找到OpenNI.so, 即 Pangolin库未安装或破损. 重新安装pangolin库即可.(亲测)

  4. Ubuntu下安装与卸载opencv模块

    opencv安装 因工程需要,想在python中调用opencv import cv2 现在记录一下如何在Linux系统(ubutun)下安装该模块: 参考了一篇博客:http://blog.csdn ...

  5. CMake命令之export

    CMake中与export()相关的命令 (注:红色字体是标题,粉色是需要特别需要注意的地方) 总的来说,export()命令想要做的事情可以用一句话概括:Export targets from th ...

  6. 第三方库的安装:Pangolin

    Pangolin: 一款开源的OPENGL显示库,可以用来视频显示.而且开发容易. 代码我们可以从Github 进行下载:https://github.com/stevenlovegrove/Pang ...

  7. ORB_SLAM2之Pangolin的安装与问题处理

    前言 本篇博客中,我们主要介绍了在安装ORB_SLAM2所需的第三方库Pangolin的过程中遇到的一些问题及解决方法. 1.Pangolin是什么? Pangolin是一个用于OpenGL显示/交互 ...

  8. ubuontu16.04安装Opencv库引发的find_package()错误信息处理及其简单使用

    在安装完Opencv库之后,打算测试一下Opencv库是否成功安装.下面是用的例子对应的.cpp代码以及对应的CMakeLists.txt代码: .cpp文件: #include <stdio. ...

  9. 安装Pangolin

    Pangolin是一个用于OpenGL显示/交互以及视频输出的一个轻量级 快速开发库 一:安装必要的库 1.Glew sudo apt-get install libglew-dev 2.Cmake ...

随机推荐

  1. 在mybatis中模糊查询有三种写法

    <select id="selectStudentsByName" resultType="Student"> <!--第一种-->  ...

  2. Hello Vizhub

    VizHub.com 一.介绍 Vizhub是一个使用D3.js和svg进行数据可视化的教学练三位一体的平台. 并且可以把在线编辑的代码保存到网站中. 右上角可以使用github账号登录. 二.Get ...

  3. NGUI中显示DrawCall详细信息

    [NGUI显示DrawCall详细信息] UIDrawCall中有个宏,SHOW_HIDDEN_OBJECTS,默认为关闭状态.将此宏打开,NGUI即会将DrawCall对象显示在Hierarchy中 ...

  4. PHP GD库

    <?php $file = '12.jpg'; //打开图片 $im = imagecreatefromjpeg($file); //设置水印字体颜色 $color = imagecoloral ...

  5. Yii2 windows 安装步骤

    安装 Yii2 高级版应用 基础版可以看官方文档 今天安装了YII2高级版应用 写下步骤 留个记号,以备查询! 在 Windows 中,你首先需要下载并运行 Composer-Setup.exe 查看 ...

  6. 全球数据库-->基金/管理产品-->分类/行业平均-->开放式分类

    SecID 招募书中所定净费率 换手率% 回报日期(每日) 计价货币 回报-本月以来(每日)计价货币 回报-本季以来(每日)计价货币 回报-本年以来(每日)计价货币 回报-1日(每日)计价货币 回报- ...

  7. [SoapUI]怎样保存response到本地文件夹

    def myOutFile = "D:/AUS/Aspect Huntley feed URLs/Automation Save Responses/ahresearch.xml" ...

  8. SFTP 安装与配置

    SFTP 安装与配置 sftp 是 Secure File Transfer Protocol 的缩写,安全文件传送协议.可以为传输文件提供一种安全的加密方法.SFTP 为 SSH 的一部分,由于这种 ...

  9. idea中代码补全

    在IDEA中,默认的代码自动提示不够智能,现在配置成更加智能的方式. File-Settings-Editor-General-Code Completion中 把最上面的大小写敏感度改成none,下 ...

  10. 整理用js实现tab标签页

    首先是css样式,比如这样的: <style> *{ ; ; list-style: none; font-size: 12px; } .notice{ width: 298px; hei ...