[从零写VIO|第七节]——VINS初始化(作业)

关于第七节VINS的初始化，其内容和代码均整理在下面列出的博客中。视频的主要讲解内容在初始化1，初始化3。

run_euroc前端的数据处理(内容|代码)
初始化1外参标定(内容|代码)
初始化2视觉初始化(内容|代码)
初始化3视觉IMU对齐(内容|代码)
初始化4visualInitialAlign()(内容|代码)
VINS_Mono知识点解析

具体步骤：

1. 数据预处理

仿照作业二：

1. 开一个终端运行roscore,即在终端输入：

roscore

若没有安装ROS，指路——在Ubuntu18.04上安装ROS Melodic

2. 将贺老师给的vio_data_simulation放在~/VINS-Course/src中:
   下载地址：https://github.com/HeYijia/vio_data_simulation
3. 编译vio_data_simulation-master

cd vio_data_simulation-master
mkdir build
cd build
cmake ..
make
cd ../bin
./data_gen

此时，VINS-Course/src/vio_data_simulation-master/bin下生成的文件有：
我们将用到这些生成的文件接入VINS代码，进行实验。
3. 用Python的matplotlib绘图

cd ../python_tool
python draw_trajcory.py

imu_pose.txt是由给定的轨迹方程和欧拉角，生成IMU的pose，imu_int_pose.txt是由给定的轨迹得到速度和加速度，再根据欧拉法和中值法得到IMU的pose，比较两个pose：
结果：

2. 测试代码

下载数据集EuRoC MAV Dataset
编译并执行：

cd build
../bin/run_euroc /home/dataset/EuRoC/MH-05/mav0/ ../config/

实验结果：

3. 修改代码

将仿真数据（视觉特征，imu数据）接入VINS系统。为了方便调试代码，我决定重新写一个simulation-test.cpp文件。

main函数如下：

int main(int argc, char **argv)
{
	if(argc != 3)
	{
		cerr << "./simulation-test 特征点文件路径 配置文件/config \n" 
			 << endl;
		return -1;
	}
	sData_path = argv[1];
	sConfig_path = argv[2];

	pSystem.reset(new System(sConfig_path));
	
	std::thread thd_BackEnd(&System::ProcessBackEnd, pSystem); // 最重要！！
		
	// sleep(5);
	std::thread thd_PubImuData(PubImuData); // 获取IMU数据的线程

	std::thread thd_PubImageData(PubImageData); //获取图像数据的线程
	
	std::thread thd_Draw(&System::Draw, pSystem); // 画图的线程


	thd_PubImuData.join();
	thd_PubImageData.join();
    thd_BackEnd.join();
    
    thd_Draw.join();

	cout << "main end... see you ..." << endl;
	return 0;
}

1. 将IMU数据传入VINS系统

void PubImuData()
{   // 获取IMU数据
	string sImu_data_file = sConfig_path + "imu_pose_noise.txt";
	...

2. 将图像数据传入VINS系统

void PubImageData()
{   // 获取图像数据
	string sImage_file = sConfig_path + "cam_pose.txt"; // 含时间戳的文件

需要注意的是，传入的是具有图像数据的文件而不是直接传入图像，并且所有的特征点都被成功跟踪，所以不需要进行LK光流追踪。直接读入当前时刻相机对应的观测值并存入forw_pts即可，并将每个特征点的track_cnt++。

再有就是，CMakeLists.txt的修改。

add_executable(simulation_test test/simulation-test.cpp)
target_link_libraries(simulation_test 
  MyVio  
  -lpthread) 

编译并执行：

../bin/simulation_test /home/VIO/Allcode/VINS-Course/src/vio_data_simulation-master/bin/ ../config/

4. 实验结果

有噪声：

无噪声：