原文在这里

数据集格式为：时间+平移+旋转矩阵四元数。

#include<opencv2/opencv.hpp>#include<fstream>
#include<iostream>
#include<vector>
#include<Eigen/Geometry>
#include<Eigen/Core>using namespace std;
using namespace cv;int main()
{ifstream fin("KeyFrameTrajectory.txt");if (!fin){cerr << "error in openning the file !" << endl;return 0;}// visualizationcv::viz::Viz3d vis("Visual Odometry");cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);vis.setBackgroundColor(cv::viz::Color::black());// draw the traceint i = 0;Point3d point_begin(0.0, 0.0, 0.0);Point3d point_end;//cv::viz::WLine wline(cv::Point3f(0, 0, 0), (100, 100, 100), cv::Scalar(0, 0, 255));cv::Point3d cam_pos(0, -1.0, -1.0), cam_focal_point(0, 0, 0), cam_y_dir(0, 1, 0);cv::Affine3d cam_pose = cv::viz::makeCameraPose(cam_pos, cam_focal_point, cam_y_dir);vis.setViewerPose(cam_pose);world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0);vis.showWidget("World", world_coor);vis.showWidget("Camera", camera_coor);vector<int> count_color;vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;vector<vector<double>> vecs;for (int i = 0;; i++){if (fin.eof()){cout << "read over " << endl;break;}vector<double> vec(8,0); //获取每行数据for (auto & d : vec){fin >> d;//双向影响}vecs.push_back(vec);}if (false){for (int j = 0; j < vecs.size(); j++){for (int i = 1; i < 8; i++)// 获取位姿{cout << vecs[j][i] << "  ";}cout << endl;}}for (int j = 0; j < vecs.size(); j++){Eigen::Quaterniond q(vecs[j][7], vecs[j][4], vecs[j][5], vecs[j][6]);Eigen::Isometry3d T(q);T.pretranslate(Eigen::Vector3d(vecs[j][1], vecs[j][2], vecs[j][3]));poses.push_back(T);cv::Affine3d M(cv::Affine3d::Mat3(T(0, 0), T(0, 1), T(0, 2),T(1, 0), T(1, 1), T(1, 2),T(2, 0), T(2, 1), T(2, 2)),cv::Affine3d::Vec3(T.translation()(0, 0), T.translation()(1, 0),T.translation()(2, 0)));//cout << "x = " << T.translation()(0, 0) << " z = " << T.translation()(1, 0) << endl;//*******************************************************************************************// 画出轨迹//*******************************************************************************************vector<cv::viz::WLine> lines; point_end = Point3d( // 更新终点T.translation()(0, 0),T.translation()(1, 0),T.translation()(2, 0));viz::WLine line(point_begin, point_end, cv::viz::Color::green());lines.push_back(line); //收集  第一个 line 到 当前line// 每个循环 画出 第一个 line 到 当前linefor (vector<cv::viz::WLine>::iterator iter = lines.begin(); iter != lines.end(); iter++){string id = to_string(i);vis.showWidget(id, *iter);i++;}point_begin = point_end; // 更新 起始点vis.setWidgetPose("Camera", M);vis.spinOnce(1, false);}vis.saveScreenshot("KeyFrameTrajectory.png");return 1;
}

opencv - viz 画出SLAM轨迹相关推荐

matlab系统函数伯德图,利用matlab画出根轨迹图|伯德图bode
求G(s)=K/s(s+1)(s+3)的根轨迹图形若开环传递函数不是多项式乘积形式,则不需用conv函数,conv函数可用于多项式乘法以及卷积. num=[1,];%分子上的各项系数 %K=[1:1 ...
android运动轨迹怎么画,Android轻松画出触摸轨迹
本文实例介绍了Android如何画出触摸轨迹的方法,分享给大家供大家参考,具体内容如下效果图: 实现代码: package com.android.gameview5; import android ...
matlab求系统根轨迹代码_要想正确画出根轨迹，先搞清楚这8大法则再说！
话说宝刀君这几年给学弟学妹们辅导自控时,学弟学妹们面对根轨迹,经常是一脸懵逼. 常常是拿到题后,零极点都标出来了,法则也写出来了,但就是根轨迹画不出来. 有时好不容易画出来了,返回去和法则一对照,忍不 ...
android 画笔轨迹,Android中SurfaceView和view画出触摸轨迹
一.引言想实现一个空白的画板,上面可以画出手滑动的轨迹,就这么一个小需求.一般就来讲就两种实现方式,view或者surfaceview.下面看看两种是如何实现的. 二.实现原理先简单说一下实现原理 ...
拜师————python基础入门——守破离学习法，海龟绘图，画出奥运五环图，day3
第六节课:任务6:006.简单错误如何处理_守破离学习法_程序员修炼手册刚刚开始学习python,编程常常会出现一系列错误,建议采用一种学习方法来学习: 守破离 (这个是学习常识) 老师出的书: ...
Java基础知识运用，用对象，线程，Java窗口画出太阳系
一个项目让你搞懂对象:Java基础知识运用,用对象,线程,GUI画出太阳系一.需求分析: 用java基础知识画出太阳系八大行星的运行轨迹图,要求:运行轨迹图是动态图片,画出运行轨迹,画出地月系(不需 ...
opencv 图像上画出目标运动的轨迹_OpenCV视频分析-Meanshift、Camshift运动轨迹绘制...
基于均值迁移的对象移动分析(Meanshift) ✏️ ⛳️ 概述本质: ✔️ Mean Shift均值漂移算法是无参密度估计理论的一种,无参密度估计不需要事先知道对象的任何先验知识,完全依靠训练数 ...
【学习OpenCV】给轮廓画出矩形和圆形边界
OpenCV支持大量的轮廓.边缘.边界的相关函数,相应的函数有moments.HuMoments.findContours.drawContours.approxPolyDP.arcLength.bo ...
Opencv画出斜矩形
描述 opencv可以利用rect,绘制出与图像坐标系u轴v轴平行的,姿态正的矩形如果该矩形绕其原点旋转,如何画出呢? 思路利用原矩形的四个点,根据旋转角度,计算出新的四个端点.最后将四个端点利用 ...

opencv - viz 画出SLAM轨迹

原文在这里

opencv - viz 画出SLAM轨迹相关推荐

最新文章

热门文章