【深蓝学院】手写VIO第7章--VINS初始化和VIO系统--作业

0. 内容

在这里插入图片描述

1. T1

1. 下载EuRoc数据集(optional)

因为作业主要使用Ch2生成的数据,所以这一步也是可选的,但是为了整个系统的bring up,可以先用EuRoc数据集跑起来。

下载EuRoc数据集,SLAM相关数据集链接

2. 更换ceres版本(optional)

代码编译不过,发现是安装了最新的ceres 2.2,导致代码库里面有些东西被替换掉了,找不到,所以卸载掉2.2,安装2.1

在这里插入图片描述

在这里插入图片描述

卸载并安装新版本的ceres

3. 双版本OpenCV共存,切换(optional)

由于之前安装了OpenCV的4.6.0,导致编译不过,但是又不想卸载较新的版本,所以找了下使两个OpenCV版本共存的方法,参考博客,看了VINS-MONO使用的是3.3.1版本,就另外安装了这个版本,最后效果如下:
只需要在find_package前更改OpenCV_DIR即可找到另一个版本的OpenCV。
在这里插入图片描述
在这里插入图片描述
切换完之后就编译过了。

这是跑出来的效果图
在这里插入图片描述

4. 解答

4.1 问题分析与解答

本章代码主要有三个线程:

  1. IMU线程 从Euroc数据集的IMU文件读取IMU数据
  2. Tracking线程 从Euroc数据集的Image文件读取Image数据, 并做光流跟踪等特征处理操作
  3. Estimator线程 后端处理, 根据前端提供的IMU数据和特征数据, 做预积分和相关后端优化
4.1.1 pub_imu

在这里插入图片描述
要清楚Ch2中生成的数据的header都是什么,cam_pose.txtimu_pose.txt都分别是:

time_stamp, w, x, y, z, tx, ty, tz, gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z

all_points_xxx.txt共7维数据:

x,y,z,1,u,v

x,y,z是landmark在camera系下的3D坐标,1代表是齐次的,u,v是归一化平面上的坐标(注意不要被u,v的名字混淆,归一化平面还是在camera系下,只是深度变成了1,还不是在像素平面下,等会儿还要用内参转换到像素平面上)

IMU数据我们只需要time_stamp和6维IMU数据, 而与imu posetime和w, x, y, z, tx, ty, tz我们都不需要,在读取时直接刷掉即可。

//将IMU数据传入VINS系统
void PubImuData()
{string sImu_data_file = sConfig_path + "imu_pose.txt";//数据中的RSS不知道是什么,但是应该是6轴IMU数据cout << "1 PubImuData start sImu_data_filea: " << sImu_data_file << endl;ifstream fsImu;fsImu.open(sImu_data_file.c_str());if (!fsImu.is_open()){cerr << "Failed to open imu file! " << sImu_data_file << endl;return;}std::string sImu_line;double dStampNSec = 0.0;Vector3d vAcc;Vector3d vGyr;while (std::getline(fsImu, sImu_line) && !sImu_line.empty()) // read imu data{std::istringstream ssImuData(sImu_line);double tmp_data;//imu的pose不需要,所以不进行读取ssImuData >> dStampNSec >> tmp_data >> tmp_data >> tmp_data >> tmp_data >> tmp_data >> tmp_data >> tmp_data>> vGyr.x() >> vGyr.y() >> vGyr.z() >> vAcc.x() >> vAcc.y() >> vAcc.z();//读取时间戳,角速度,加速度pSystem->PubImuData(dStampNSec, vGyr, vAcc);//这里读出来的时间直接就是s,不用再转usleep(5000*nDelayTimes);}fsImu.close();printf("imu pub finish!!!\n");
}
4.1.1 pub_img

在这里插入图片描述
关于pub_img,由于我们直接有了匹配好的feature points,所有关于前端特征处理的都不用做(特征提取,LK光流匹配,去畸变等,也不用算光流的速度(虽然我算了一下)),我们需要做的核心的事情就是把这些feature喂给我们的特征点buffer(即代码中的feature_points)

代码如下:

run_euroc.cpp关于img的部分

//将视觉特征传入VINS系统
void PubImageData()
{string sImage_file = sConfig_path + "cam_pose.txt";printf("start pub img msg...\n");printf("sImage_file: %s\n", sImage_file.c_str());cout << "1 PubImageData start sImage_file: " << sImage_file << endl;ifstream fsImage;fsImage.open(sImage_file.c_str());if (!fsImage.is_open()) {cerr << "Failed to open image file! " << sImage_file << endl;return;}std::string sImage_line;double dStampNSec;string sImgFileName;vector<Matrix<double,6,1>> xyz_uv_sum;int point_num = 0;// cv::namedWindow("SOURCE IMAGE", CV_WINDOW_AUTOSIZE);while (std::getline(fsImage, sImage_line) && !sImage_line.empty()){std::istringstream ssImgData(sImage_line);ssImgData >> dStampNSec;//读取camera时间戳Matrix<double,6,1> xyz_uv;string KF_PointsFile = sConfig_path + "keyframe/all_points_" + to_string(point_num++) + ".txt";//读all_points_xx.txtifstream fin(KF_PointsFile);if(!fin) {printf("[ERROR] KF file: %s do not exist", KF_PointsFile.c_str());}printf("KF_PointsFile: %s\n", KF_PointsFile.c_str());while(!fin.eof()) {fin >> xyz_uv(0) >> xyz_uv(1) >> xyz_uv(2) >> xyz_uv(3) >> xyz_uv(4) >> xyz_uv(5);xyz_uv_sum.push_back(xyz_uv);}pSystem->PubImageData(dStampNSec, xyz_uv_sum);//xyz_uv_sum.clear();// cv::imshow("SOURCE IMAGE", img);// cv::waitKey(0);usleep(50000*nDelayTimes);}fsImage.close();printf("pub img finish, point_num: %d\n", point_num);
}

system.cpp

void System::PubImageData(double dStampSec, vector<Matrix<double,6,1>> &img)
{PUB_THIS_FRAME = true;//直接pubif (PUB_THIS_FRAME){pub_count++;printf("pub count: %d\n", pub_count);shared_ptr<IMG_MSG> feature_points(new IMG_MSG());feature_points->header = dStampSec;vector<set<int>> hash_ids(NUM_OF_CAM);for (int i = 0; i < NUM_OF_CAM; i++){vector<cv::Point2f> tmp_pts;printf("img.size: %d\n", img.size());for (unsigned int j = 0; j < img.size(); j++){int p_id = j;hash_ids[i].insert(p_id);double x = img[j][4];double y = img[j][5];double z = 1;double u = 460 * x + 255; //u=fx * x / z + cx 使用内参将归一化坐标转换到像素平面下double v = 460 * y + 255; //u=fx * x / z + cxfeature_points->points.emplace_back(x, y, z);feature_points->id_of_point.push_back(p_id * NUM_OF_CAM + i);feature_points->u_of_point.push_back(u);//仿真数据直接用xy代替像素坐标feature_points->v_of_point.push_back(v);tmp_pts.emplace_back(u, v);if(prev_pts_.empty()) {feature_points->velocity_x_of_point.push_back(0);  //第一帧速度设为0feature_points->velocity_y_of_point.push_back(0);printf("prev_pts_ is empty set cur volecity to zero, cur_(u,v)=(%f, %f)\n", u, v);} else {//由于feature points都是一一对应的,所以直接取对应index的坐标相减然后处以时间即可得速度double v_x = (u - prev_pts_[j].x) / 0.0333333;double v_y = (v - prev_pts_[j].y) / 0.0333333;feature_points->velocity_x_of_point.push_back(v_x);feature_points->velocity_y_of_point.push_back(v_y);printf("prev_(u,v)=(%f, %f), cur_(u,v)=(%f, %f), v_x: %f, v_y: %f\n",prev_pts_[j].x, prev_pts_[j].y, u, v, v_x, v_y);}}prev_pts_.clear();prev_pts_ = tmp_pts;tmp_pts.clear();//}// skip the first image; since no optical speed on frist imageif (!init_pub){cout << "4 PubImage init_pub skip the first image!" << endl;init_pub = 1;}else{m_buf.lock();feature_buf.push(feature_points);// cout << "5 PubImage t : " << fixed << feature_points->header//     << " feature_buf size: " << feature_buf.size() << endl;m_buf.unlock();con.notify_one();}}}
}

总是会需要debug的,clion传参:
在这里插入图片描述
关于噪声的操作,我看很多博客都没有明说,不同噪声需要配置Ch2的参数先生成带有noise的文件imu_pose_noise.txt,然后将参数配置给VINS系统(比如乘了10倍就在VINS配置中也乘10),跑出来即可。

ch2生成noise数据配置:
在这里插入图片描述
VINS imu noise配置,这里我使用了离散的噪声(连续噪声就不用下面最后4行即可):
在这里插入图片描述

4.1.2 踩过的坑(optional)

分别在noise为0, 1,10, 30, 45, 60倍时进行了实验,需要说明一下,因为没有fix第一帧,所以会使得整体的pose在GT的world系下有漂移,我当时使用Ch2的可视化工具可视化出来output和GT发现差了个T,但是把T算出来感觉也不是固定的(事实证明T确实是固定的,但是不知道怎么align起来的,这部分evo直接给出了T),当时对着python脚本可视化出来的结果发愁了好一会儿,后来发现他们的作业都是使用evo可视化的(evo下文会讲)
在这里插入图片描述
当时计算T的代码

void cal_diff() {Quaterniond q;Eigen::Matrix<double, 3, 1> tmp_mat;double tmp_var;vector<Sophus::SE3d> vec_T_gt;std::string cam_pose_tum = "../config/cam_pose_tum_delete_first11rows.txt";ifstream fin(cam_pose_tum);if(!fin) {printf("[ERROR] cam_pose_tum file: %s do not exist\n", cam_pose_tum.c_str());}printf("cam_pose_tum file: %s\n", cam_pose_tum.c_str());while(!fin.eof()) {fin >> tmp_var >>tmp_mat(0) >> tmp_mat(1) >> tmp_mat(2)>> q.x() >> q.y() >> q.z() >> q.w();vec_T_gt.emplace_back(Sophus::SE3d{q, tmp_mat});}vector<Sophus::SE3d> vec_T_vins;std::string vins_cam_pose_tum = "../bin/VINS_pose_output_asTUM2.txt";ifstream fin_vins(vins_cam_pose_tum);if(!fin_vins) {printf("[ERROR] vins_cam_pose_tum file: %s do not exist\n", vins_cam_pose_tum.c_str());}printf("vins_cam_pose_tum file: %s\n", vins_cam_pose_tum.c_str());while(!fin_vins.eof()) {fin_vins >> tmp_var >> q.x() >> q.y() >> q.z() >> q.w() >>tmp_mat(0) >> tmp_mat(1) >> tmp_mat(2);vec_T_vins.emplace_back(Sophus::SE3d{q, tmp_mat});}int index_min = min(vec_T_gt.size(), vec_T_vins.size());for(int i = 0; i < index_min; ++i) {//Tw_gt^(-1) * Tw_vins = Tgt_vinsSophus::SE3d se3_diff = vec_T_gt[i].inverse() * vec_T_vins[i];cout << "i = " << i << ",\t se3_diff_ypr = \t" << Utility::R2ypr(se3_diff.rotationMatrix()).transpose()<< ",\t t_diff = \t" << se3_diff.translation().transpose() << endl;}
}

计算出来的T是这样的,R我转为了ypr
在这里插入图片描述

evo计算出来的align的T
在这里插入图片描述
后来想了一下,我的计算方法中VINS和GT的world可能不是一个world,所以我那样算可能是有问题的。

这个Umeyama方法和evo后面有时间看一下,挖个坑。

4.2 实验结果和分析

4.2.1 整体实验结果

在这里插入图片描述

noise在60倍时,噪声过大,看LM迭代一直没有停止,说明一直没有达到停止条件,优化失败,没有比较的意义了,此时轨迹也无参考意义:
在这里插入图片描述

在这里插入图片描述

4.2.2 可视化对比

使用evo绘制出优化出的pose和GT的曲线,
evo安装:https://github.com/MichaelGrupp/evo
绘制指令:

tum cam_pose_tum.txt VINS_pose_output_asTUM2_noise_free_distcrete.txt  -va --plot --plot_mode xyz

其中cam_pose_tum.txt是GT,VINS_pose_output_asTUM2_noise_free_distcrete.txt是VINS的输出,GT在图中以虚线表示。

将noise_free,noise * 1 , noise * 45进行联合对比,发现noise越大,误差越大:

在这里插入图片描述

4.3 结论

从上述实验中可以看出,随着imu数据noise的增大,VINS-MONO优化的误差也在增加,当误差大到一定程度时,数据就不能再使用了,需要重新采集数据。


本章作业代码重要性还是很高的,需要多花些时间看透整个代码的框架,包括系统初始化,变量管理,前端特征处理,后端求解(前几章主要工作),以及一些trick(如使用了VINS-MONO中的marg策略),如果要fix第一帧应该怎么做等等。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/115658.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【LeetCode刷题(数据结构与算法)】:数据结构中的常用排序实现数组的升序排列

现在我先将各大排序的动图和思路以及代码呈现给大家 插入排序 直接插入排序是一种简单的插入排序法&#xff0c;其基本思想是&#xff1a; 把待排序的记录按其关键码值的大小逐个插入到一个已经排好序的有序序列中&#xff0c;直到所有的记录插入完为 止&#xff0c;得到一个…

c语言练习91:合并两个有序链表

合并两个有序链表 力扣&#xff08;LeetCode&#xff09;官网 - 全球极客挚爱的技术成长平台 代码1&#xff1a; /*** Definition for singly-linked list.* struct ListNode {* int val;* struct ListNode *next;* };*/typedef struct ListNode ListNode; struct Li…

Appium+python+unittest搭建UI自动化框架!

阅读本小节&#xff0c;需要读者具备如下前提条件&#xff1a; 1. 掌握一种编程语言基础&#xff0c;如java、python等。 2. 掌握一种单元测试框架&#xff0c;如java语言的testng框架、python的unittest框架。 3. 掌握目前主流的UI测试框架&#xff0c;移动端APP测试框架Appiu…

应用3之Like运算符的应用

《VBA经典应用69例》&#xff08;10178981&#xff09;&#xff0c;是我推出的第九套教程&#xff0c;教程是专门针对初级、中级学员在学习VBA过程中可能遇到的案例展开&#xff0c;这套教程案例众多&#xff0c;紧贴“实战”&#xff0c;并做“战术总结”&#xff0c;以便大家…

医药电商行业想要精准获客?媒介盒子分享三大技巧

随着医疗需求的不断增长&#xff0c;健康成为社会关注的重点&#xff0c;消费者对医药保健产品和需求正在不断增长&#xff0c;数字化时代的来临使医药行业逐渐电商化&#xff0c;线上零售渠道成为医药行业销售额的主要来源&#xff0c;那当下医药电商行业如何抓住机遇&#xf…

AB试验(六)A/B实验常见知识点的Python计算

AB试验&#xff08;六&#xff09;A/B实验常见知识点的Python计算 前面理论知识上提到了很多的知识点需要计算&#xff0c;作为一个实用主义的博主&#xff0c;怎么可以忍受空谈呢&#xff1f;所以本期就给大家分享如何利用Python对这些知识点进行计算。 均值类指标 import …

一起学数据结构(11)——快速排序及其优化

上篇文章中&#xff0c;解释了插入排序、希尔排序、冒泡排序、堆排序及选择排序的原理及具体代码实现本片文章将针对快速排序&#xff0c;快速排序的几种优化方法、快速排序的非递归进行解释。 目录 1. 快速排序原理解析以及代码实现&#xff1a; 2. 如何保证相遇位置的值一…

嵌入式硬件库的基本操作方式与分析

本次要介绍的开源软件是 c-periphery&#xff1a; https://github.com/vsergeev/c-periphery一个用 C 语言编写的硬件外设访问库。 我们可以用它来读写 Serial、SPI、I2C 等&#xff0c;非常适合在嵌入式产品上使用。 我们可以基于它优秀的代码框架&#xff0c;不断地扩展出更…

Prometheus接入AlterManager配置邮件告警(基于K8S环境部署)

文章目录 一、配置AlterManager告警发送至邮箱二、Prometheus接入AlterManager配置三、部署PrometheusAlterManager(放到一个Pod中)四、测试告警 注意&#xff1a;请基于 PrometheusGrafana监控K8S集群(基于K8S环境部署)文章之上做本次实验。 一、配置AlterManager告警发送至邮…

C++——特殊类设计

目录 一.不能被拷贝的类 1.C98做法 2.C11做法 二.只能在堆上实例化的类 1.实现方式一 2.实现方式二 三.只能在栈上创建的对象 四.不能被继承的类 1.C98方式 2.C11方式 五.只能创建一个对象的类 1.设计模式 2.单例模式 一.不能被拷贝的类 拷贝只会放在两个场景中&a…

visual studio Qt 开发环境中手动添加 Q_OBJECT 导致编译时出错的问题

问题简述 创建项目的时候&#xff0c;已经添加了类文件&#xff0c;前期认为不需要信号槽&#xff0c;就没有添加宏Q_OBJECT,后面项目需要&#xff0c;又加入了宏Q_OBJECT&#xff0c;但是发现只是添加了一个宏Q_OBJECT&#xff0c;除此之外没有改动其它的代码&#xff0c;原本…

基于springboot实现地方废物回收机构平台管理系统【项目源码+论文说明】

基于springboot实现地方废物回收机构管理系统演示 摘要 网络的广泛应用给生活带来了十分的便利。所以把地方废物回收机构管理与现在网络相结合&#xff0c;利用java技术建设地方废物回收机构管理系统&#xff0c;实现地方废物回收机构的信息化。则对于进一步提高地方废物回收机…

如何提高广告投放转化率?Share Creators 资产库与Appsflyer营销数据的全面结合

如何提高广告投放转化率&#xff1f;Share Creators 资产库与Appsflyer营销数据的全面结合 全球经济进入了低迷期。 营销成本越来越高&#xff0c; 营销需要更务实&#xff0c;注重投入产出比。众所周知&#xff0c;除了渠道、客群画像以外&#xff0c; 优秀的广告设计图&#…

c进阶测试题

选择题 1.请问该程序的输出是多少&#xff08;C&#xff09; #include<stdio.h> int main(){unsigned char i 7;int j 0;for(;i > 0;i - 3){ j;} printf("%d\n", j);return 0; }A. 2 B. 死循环 C. 173 D. 172 首先unsigned char型是不会为负数&#xff…

flask入门(四)前后端数据传输

文章目录 1、flask后端接收来自前端的数据1&#xff09;如果前端提交的方法为POST2&#xff09;如果前段提交的方法是GET 2、flask后端向前端传数据3、案例参考文献 1、flask后端接收来自前端的数据 1&#xff09;如果前端提交的方法为POST 后端接收时的代码&#xff1a; xx…

我试图扯掉这条 SQL 的底裤。只能扯一点点,不能扯多了

之前不是写分页嘛,分页肯定就要说到 limit 关键字嘛。 然后我啪的一下扔了一个链接出来: https://dev.mysql.com/doc/refman/8.0/en/limit-optimization.html 这个链接就是 MySQL 官方文档,这一章节叫做“对 Limit 查询的优化”,针对 limit 和 order by 组合的场景进行了较…

【MySQL】存储引擎

存储引擎 查看存储引擎设置表的存储引擎创建表时指定存储引擎修改表的存储引擎 引擎介绍InnoDB引擎: 具备外键支持的十五存储引擎MyISAM引擎: 主要的非事务处理存储引擎Archive引擎: 用于数据存档Blackhole引擎: 丢弃写操作,读操作返回空内容CSV引擎: 读取数据时,以逗号分隔各个…

redis底层数据结构

总所周知&#xff0c;redis支持五种数据类型String、Hash、List、Set、ZSet。在支持这些复杂数据结构的同时&#xff0c;redis不仅需要保证读写的性能&#xff0c;还能提供各种微操作&#xff0c;比如直接修改Hash字典中的某个field的值&#xff0c;或者直接往ZSet中插入某个值…

Failed to start The nginx HTTP and reverse proxy server.

本章教程主要分享一下&#xff0c;当nginx 启动时&#xff0c;遇到报这个错误时的一个解决问题思路。 目录 1、观察报错信息 2、尝试性解决 1、观察报错信息 根据日志的信息&#xff0c;我们至少可以知道2个比较信息。 1、操作用户执行命令是在非root权限下进行操作的。 2、Ad…

Xcode14创建github远程仓库Token

1.点击Create a Token on GitHub 2.在打开的网页中,登陆GitHub 3.点击生成Token 这是不能为空 4.Token创建成功如下: 5.复制Token到Xcode然后点击Sign In登陆 正在创建远程我仓库 正在将本地仓库代码推入远程仓库 创建成功