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

news2024/11/16 13:54:46

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.txt和imu_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.txt
        ifstream 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;//直接pub
    if (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 + cx
                feature_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);  //第一帧速度设为0
                    feature_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 image
            if (!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_vins
        Sophus::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.coloradmin.cn/o/1125642.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

windows平台下Qt Creator的下载与安装流程

windows平台下Qt Creator的下载与安装流程

下载 下载地址&#xff1a;https://download.qt.io/archive/ 下载界面 进入qt或者qtcreator都可以 版本选择 这里我选择进入qt进行下载&#xff0c;进入之后有多个版本可以选择。 注意&#xff1a;从Qt5.15版本开始&#xff0c;Qt公司不在提供开源离线安装程序&#xff0c;此…
阅读更多...
2.6.C++项目：网络版五子棋对战之数据管理模块-游戏房间管理模块的设计

2.6.C++项目：网络版五子棋对战之数据管理模块-游戏房间管理模块的设计

文章目录 一、意义二、功能三、作用四、游戏房间类基本框架五、游戏房间管理类基本框架七、游戏房间类代码八、游戏房间管理类代码 一、意义 对匹配成功的玩家创建房间&#xff0c;建立起一个小范围的玩家之间的关联关系&#xff01; 房间里一个玩家产生的动作将会广播给房间里…
阅读更多...
寻找一罐app里的隐藏海

寻找一罐app里的隐藏海

一、前言 &#xff08;一&#xff09;一罐app简介 一罐app 是一款小众交友软件&#xff0c;可以匿名or真身发布动态 &#xff08;二&#xff09;开发目的 因为某些原因&#xff0c;某些板块被隐藏起来了。&#xff08;一罐称板块为xxx海&#xff09; &#xff08;三&#…
阅读更多...
常见面试题-Netty专栏（一）

常见面试题-Netty专栏（一）

typora-copy-images-to: imgs Netty 是什么呢&#xff1f;Netty 用于做什么呢&#xff1f; 答&#xff1a; Netty 是一个 NIO 客户服务端框架&#xff0c;可以快速开发网络应用程序&#xff0c;如协议服务端和客户端&#xff0c;极大简化了网络编程&#xff0c;如 TCP 和 UDP …
阅读更多...
手搭手Ajax经典基础案例省市联动

手搭手Ajax经典基础案例省市联动

环境介绍 技术栈 springbootmybatis-plusmysql 软件 版本 mysql 8 IDEA IntelliJ IDEA 2022.2.1 JDK 1.8 Spring Boot 2.7.13 mybatis-plus 3.5.3.2 pom.xml <?xml version"1.0" encoding"UTF-8"?> <project xmlns"http:/…
阅读更多...
MYSQL(事务+锁+MVCC+SQL执行流程)理解

MYSQL(事务+锁+MVCC+SQL执行流程)理解

一)事务的特性: 一致性:主要是在数据层面来说&#xff0c;不能说执行扣减库存的操作的时候用户订单数据却没有生成 原子性:主要是在操作层面来说&#xff0c;要么操作完成&#xff0c;要么操作全部回滚&#xff1b; 隔离性:是自己的事务操作自己的数据&#xff0c;不会受到到其…
阅读更多...
有哪些好用的程序员接私活平台?

有哪些好用的程序员接私活平台?

程序员如何在苦逼的生活中&#xff0c;呼吸一口富贵又自由的空气? 接单的话&#xff0c;如何脱颖而出&#xff1f; 又该用什么平台呢&#xff1f;哪些平台会更靠谱一点呢&#xff1f; 会不会被坑? balabalabalabala......太多问题了&#xff0c;核心还是不了解这里面的详情。…
阅读更多...
从理解概念开始，彻底学会linux下的磁盘扩容操作

从理解概念开始，彻底学会linux下的磁盘扩容操作

对于linux磁盘空间不足需要扩容的情况&#xff0c;其他文章一般只介绍要如何操作&#xff0c;使用什么样的命令&#xff0c;但是不去介绍为什么要这么做&#xff0c;搞得好多小白一头雾水。本文从linux的文件系统开始讲起&#xff0c;帮你彻底学会linux系统中的磁盘扩容操作。 …
阅读更多...
科学指南针iThenticate自助查重系统重磅上线

科学指南针iThenticate自助查重系统重磅上线

科学指南针&#xff0c;一直致力于为科研工作者提供高效、专业的学术支持&#xff0c;近日推出了全新的iThenticate自助查重系统。这一系统的上线&#xff0c;旨在为广大科研工作者提供更加便捷、准确的论文查重服务&#xff0c;进一步规范英文使用&#xff0c;提升科研质量。 …
阅读更多...
使用Packstack安装器安装一体化OpenStack云平台

使用Packstack安装器安装一体化OpenStack云平台

【实训目的】 初步掌握OpenStack快捷安装的方法。掌握OpenStack图形界面的基本操作。 【实训准备】 &#xff08;1&#xff09;准备一台能够安装OpenStack的实验用计算机&#xff0c;建议使用VMware虚拟机。 &#xff08;2&#xff09;该计算机应安装CentOS 7&#xff0c;建…
阅读更多...
【网络协议】聊聊网络路由相关算法

【网络协议】聊聊网络路由相关算法

如何配置路由 路由器是一台网络设备&#xff0c;多张网卡&#xff0c;当一个入口的网络包到达路由器时&#xff0c;会根据本地的信息库决定如何正确的转发流量&#xff0c;通常称为路由表 路由表主要包含如下 核心思想是根据目的 IP 地址来配置路由 目的网络&#xff1a;要去…
阅读更多...
电影评分数据分析案例-Spark SQL

电影评分数据分析案例-Spark SQL

# cording:utf8from pyspark.sql import SparkSession from pyspark.sql.types import IntegerType, StringType, StructType import pyspark.sql.functions as Fif __name__ __main__:# 0.构建执行环境入口对象SparkSessionspark SparkSession.builder.\appName(movie_demo)…
阅读更多...
DDOS直接攻击系统资源

DDOS直接攻击系统资源

DDOS ——直接攻击系统资源 思路&#xff1a; 攻击机利用三次握手机制&#xff0c;产生大量半连接&#xff0c;挤占受害者系统资源&#xff0c;使其无法正常提供服务。 1、先体验下受害者的正常网速。在受害者主机上执行以下命令 (1)开启Apache。 systemctl start apache2 (2…
阅读更多...
C++数据结构X篇_20_选择排序

C++数据结构X篇_20_选择排序

文章目录 1. 选择排序原理2. 选择排序原理核心代码3. 选择排序时间消耗 1. 选择排序原理 选择排序&#xff1a;相对于冒泡排序&#xff0c;减少了交换次数&#xff0c;下图展示了选择排序的原理&#xff0c;具体仍需要结合代码分析。 2. 选择排序原理核心代码 //选择排序 v…
阅读更多...
运行报错（三）git bash报错fatal: detected dubious ownership in repository at

运行报错（三）git bash报错fatal: detected dubious ownership in repository at

报错现象 在运行git 命令时&#xff0c;出现报错 “fatal: detected dubious ownership in repository at” 报错原因 文件夹的所有者和现在的用户不一致 栗子&#xff1a; 文件夹的所有者是root&#xff0c;而当前用户是admin 解决方案 方法一、 将文件夹的所有者替换成ad…
阅读更多...
九章云极DataCanvas公司入选Forrester AI/ML权威报告

九章云极DataCanvas公司入选Forrester AI/ML权威报告

日前&#xff0c;全球研究机构Forrester最新发布了《The Forrester Wave™: AI/ML Platforms In China, Q4 2023》报告&#xff08;以下简称“报告”&#xff09;。凭借DataCanvas APS机器学习平台这一人工智能核心基础软件的持续研发和广泛应用&#xff0c;九章云极DataCanvas…
阅读更多...
数据集-特征降维

数据集-特征降维

1、降维 降维是指在某些限定条件下&#xff0c;降低随机变量(特征)个数&#xff0c;得到一组“不相关”主变量的过程 降低随机变量的个数 相关特征(correlated feature) 相对湿度与降雨量之间的相关等等 正是因为在进行训练的时候&#xff0c;我们都是使用特征进行学习。如果…
阅读更多...
基于 Android 的文件同步设计方案

基于 Android 的文件同步设计方案

1、背景 随着用户对自身数据保护意识的加强&#xff0c;让用户自己维护自己的数据也成了独立开发产品时的一个卖点。若只针对少量的文件进行同步&#xff0c;则实现起来比较简单。当针对一个多层级目录同步时&#xff0c;情况就复杂多了。鉴于相关的文章甚少&#xff0c;本文我…
阅读更多...
MODIS数据产品预处理方法

MODIS数据产品预处理方法

1 MCTK重投影 第一步&#xff1a;安装ENVI的MCTK扩展工具 解压压缩包&#xff0c;将其中的mctk.sav与modis_products.scsv文件复制到如图所示&#xff0c;相应的ENVI安装路径中去。 第二步&#xff1a;打开ENVI5.3标准版如图所示 在右边的工具栏处打开最下方的Extensions工具…
阅读更多...
代码随想录笔记--单调栈篇

代码随想录笔记--单调栈篇

1--单调栈 使用单调栈的特征&#xff1a;寻找第一个比当前元素大或者小的元素。 2--每日温度 主要思路&#xff1a; 基于单调栈&#xff0c;单调栈从栈顶开始递增&#xff1b;单调栈存储的是元素对应的索引。 当遇到一个元素大于栈顶元素i时&#xff0c;计算 answer[i]。 #incl…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023