使用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 的位姿。我们用红、绿、蓝三种颜色画出每个位姿的三 个坐标轴(实际上我们计算了各坐标轴的世界坐标),然后用黑色线将轨迹连起来。
使用pangolin库画出轨迹的更多相关文章
- Android教程:在百度地图上画出轨迹
[日期:2013-04-14] 来源:Linux社区 作者:crazyxin1988 [字体:大 中 小] 接着上面的项目<Android访问webservice.客户端登录注册> ...
- 课程作业——Python基础之使用turtle库画出红旗
代码如下: import turtle # 设置画笔和背景颜色 turtle.color('yellow') turtle.bgcolor('red') # 通过偏移量和尺寸大小画星星 def dra ...
- 一篇文教你使用python Turtle库画出“精美碎花小清新风格树”快来拿代码!
Turtle库手册可以查询查询 python图形绘制库turtle中文开发文档及示例大全,手册中现有示例,不需要自己动手就可以查看演示. 使用Turtle画树,看了一下网上的代码,基本上核心的方法是使 ...
- 使用Pangolon在同一副图中,画出两个轨迹,比较误差
使用 code/ground-truth.txt 和 code/estimate.txt 两条轨迹.请你根据上面公式,实现 RMSE的计算代码,给出最后的 RMSE 结果.作为验算,参考答案为:2.2 ...
- 教你用开源 JS 库快速画出 GitHub 章鱼猫
本文作者:HelloGitHub-kalifun 在上一篇文章我们介绍了 Zdog 如何使用,接下来这篇文章我将带领各位利用 Zdog 画出一个 GitHub 章鱼猫(和官方的还是有些差别的). Zd ...
- Pangolin库的使用
使用Pangolin画出相机的轨迹(包括朝向). 数据集结构data.csv: #timestamp, p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w ...
- 在移动端画出真正的1px边框
一.问题 写H5的样式时候,设置元素的边框为1px,不幸的事情在IOS设备上发生了,设计师会说,咦,边框怎么那么大,这是2px了吧?改成1px.我明明设置成1px了啊. 二.为什么边框变粗了? ...
- matlplotlib根据函数画出图形
根据函数画出函数的轨迹 import matht = np.linspace(0, math.pi, 1000)x = np.sin(t)y = np.cos(t) + np.power(x, 2.0 ...
- win2d 画出好看的图形
本文告诉大家,win2d 不需要从零开始做,以前做出来的很多库其实只需要做很小修改就可以做出好看的效果,而且用在 UWP 上.本文修改原先 大神写的 GDI 图形到 win2d 上,而且可以运行起来 ...
随机推荐
- linux find grep
find是文件查找, grep是文件内容查找. 1. find find path expression -options[-print -exec -ok] -print 将结果输出到标准输出: - ...
- MailUtils
/** *包名:com.thinkgem.jeesite.test *描述:package com.thinkgem.jeesite.test; */ package com.thinkgem.jee ...
- 为什么不能在shell脚本中执行source /etc/profile或者source ~/.bashrc问题?
执行脚本时,其中的命令是在一个子shell中执行的.子shell继承了父shell的环境变量,但无法修改他们,或者说所做的修改仅对子shell有效.
- 判断系统是否安装了flash插件
方法1: uses comobj; procedure TForm1.Button1Click(Sender: TObject); var v:variant; begin v:=CreateOleO ...
- SQL语句之-计算字段/分组
五.计算字段 1.拼接字段 MySQL:使用函数concat SqlServer:使用加号+ oracle:使用|| SELECT CONCAT(vend_name,'(',vend_country, ...
- poi各种jar包作用和导入
poi各种jar包作用和导入 目前POI的最新发布版本是poi-bin-3.17-20170915. 下载地址: Apache POI - Download Release Artifacts ht ...
- wrtnode板
Arduino技术交流:www.openjumper.com QQ群 ArduinoCN I : 180646674,ArduinoCN II : 203870250 商品详情 产品介绍 : WRT ...
- python requests函数封装方法
python requests函数封装方法 上代码 import requests import json """ 封装request请求, 1.post:my_pos ...
- alerttemplate 时间戳转换
/*时间戳转换*/ template.defaults.imports.dateFmt = function(ns){ return new Date(parseInt(ns)).toLocaleSt ...
- 7.使用mysql_export监控mysql
ok,docker监控,宿主机CPU.磁盘.网络.内存监控我们都已讲过,是时候讲一波mysql监控了.本次mysql部署在客户端. 架构 客户端 MySql安装 ##下载mysql的repo源: [r ...