从零手写VIO(7)

从零手写VIO(7)

---

---

前言

一、VINS-Course代码解析

这部分对自己比较难，很多看不太懂，主要参考其他大佬的解析，主要参考如下
[从零写VIO|第七节]——VINS-Mono代码精简版代码详解——run_euroc前端的数据处理(内容|代码)，写得特别详细，不管是前端，后端以及初始化部分都有详细的解析（后端及初始化部分可以根据这篇文章的链接顺序看下去）！！

二、作业（7）

将第二讲的仿真数据集(视觉特征,imu 数据)接入我们的 VINS代码,并运行出轨迹结果
• 仿真数据集无噪声
• 仿真数据集有噪声(不同噪声设定时,需要配置 vins 中 imu noise大小。)

这部分主要参考深蓝学院《从零开始手写VIO》作业7以及从零手写vio-作业7。

1.simulation-test.cpp修改

在VINS-Course下的test文件夹新建simulation-test.cpp代码，将vio_data_simulation文件夹放在在VINS-Course下的src文件夹下，并编译生成文件，修改sData_path ：string sData_path = “/home/yy/VINS-Course/src/vio_data_simulation/bin/”;并把vio_data_simulation生成的bin文件夹下的文件imu_pose_noise.txt，cam_pose.txt,imu_pose.txt复制到VINS-Course中的config文件夹下。

//
// Created by yy on 22-7-22.
//
//
// Created by yy on 22-7-22.
//
#include //函数名: usleep()
#include 
#include 
#include 
#include 
#include 
#include //　iomanip的作用:　主要是对cin,cout之类的一些操纵运算子，比如setfill,setw,setbase,setprecision等等

#include 
#include 
#include 
#include 
#include "System.h"

using namespace std;
using namespace cv;
using namespace Eigen;

const int nDelayTimes = 2;
string sData_path = "/home/yy/VINS-Course/src/vio_data_simulation/bin/";
string sConfig_path = "../config/";

std::shared_ptr<System> pSystem;
///读取imu数据文件中的IMU数据，并把数据打包成(时间dStampNSec，角速度vGyr，加速度vAcc)的形式调用系统函数PubImuData进行处理：
void PubImuData()///读IMU数据并调用系统接口去处理imu数据
{
    string sImu_data_file = sConfig_path +  "imu_pose_noise.txt";
    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;
    }
    double tmp;
    std::string sImu_line;
    double dStampNSec = 0.0;
    Vector3d vAcc;
    Vector3d vGyr;

///imu data :timestamp (1)，imu quaternion(4)，imu position(3)，imu gyro(3)，imu acc(3)
    while (std::getline(fsImu, sImu_line) && !sImu_line.empty()) // read imu data
    {
        // timestamp (1)，imu quaternion(4)，imu position(3)，imu gyro(3)，imu acc(3)
        std::istringstream ssImuData(sImu_line);
        ssImuData >> dStampNSec;///timestamp
        for(int i=0;i<7;i++)
            ssImuData>>tmp;///imu quaternion(4)，imu position(3)
        ssImuData>>vGyr.x() >> vGyr.y() >> vGyr.z() >> vAcc.x() >> vAcc.y() >> vAcc.z();
        // 时间单位为 s
        pSystem->PubImuData(dStampNSec, vGyr, vAcc);
        usleep(5000*nDelayTimes);
    }

    fsImu.close();
}
///先从MH_05_cam0.txt文件读取数据，打包成（时间dStampNSec，图像sImgFileName）。
///再调用系统的PubImageData函数进行图像数据的处理。
void PubImageData()///读图像数据并调用系统接口去处理图像数据
{///cam data:timestamp (1)，cam quaternion(4)，cam position(3)，imu gyro(3)，imu acc(3)
    string sImage_file = sConfig_path + "cam_pose.txt";// 含时间戳的文件

    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;

    int n=0;

    // cv::namedWindow("SOURCE IMAGE", CV_WINDOW_AUTOSIZE);
    //这个循环是遍历所有的相机
    while (std::getline(fsImage, sImage_line) && !sImage_line.empty())//sImage_line是cam_pose每行的数据流
    {
        std::istringstream ssImgData(sImage_line);//是cam_pose每行的内容
        ssImgData >> dStampNSec;   //读入时间戳
        cout<<"cam time: "<<fixed<<dStampNSec<<endl;
        // all_points_ 文件存储的是house模型的线特征，每行4个数，对应该线两端点在归一化平面的坐标
        //all_points_  文件每行的内容是 x, y, z, 1, u, v  这里的u v是归一化下的x ,y 不是像素坐标
        //在函数PubSimImageData中会算出具体特征点的像素坐标
        string all_points_file_name = "/home/yy/VINS-Course/src/vio_data_simulation/bin/keyframe/all_points_" + to_string(n)+ ".txt";  //第n个相机对应的观测数据的文件名
        cout<<"points_file: "<<all_points_file_name<<endl;//0-599 all_point_0-599.txt
        vector<cv::Point2f> FeaturePoints;//容器FeaturePoints存放一个相机的特征点(归一化坐标)
        std::ifstream f;
        f.open(all_points_file_name);

        //这个循环是遍历每个相机的特征点信息
        // file content in each line: x, y, z, 1, u, v
        //经过这个循环把all_points_的特征点都放在FeaturePoints了
        while(!f.eof())
        {
            std::string s;特征点:x，y，z，1，u，v
            std::getline(f,s);//得到all_points_的文件流s
            // 一行两个点连成线，获取每行点判断一下是否之前获取过
            if(!s.empty())
            {
                std::stringstream ss;//
                ss << s;//ss得到每行的内容

                double tmp;//跳过  x y z 1
                for(int i=0;i<4;i++)
                    ss>>tmp;

                float px,py;
                ss >> px;
                ss >> py;
                cv::Point2f pt( px, py);//归一化坐标

                FeaturePoints.push_back(pt);
            }
        }

//        cout << "All points:" << endl;
//        for(auto point : FeaturePoints){
//            cout << point << " ";
//        }
//        cout << endl;


        pSystem->PubSimImageData(dStampNSec, FeaturePoints);//把每一个图片的特征点 放进VINS系统里

        usleep(50000*nDelayTimes);
        n++;
    }


    fsImage.close();
}

#ifdef __APPLE__
// support for MacOS
void DrawIMGandGLinMainThrd(){
	string sImage_file = sConfig_path + "MH_05_cam0.txt";

	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;

	pSystem->InitDrawGL();
	while (std::getline(fsImage, sImage_line) && !sImage_line.empty())
	{
		std::istringstream ssImuData(sImage_line);
		ssImuData >> dStampNSec >> sImgFileName;
		// cout << "Image t : " << fixed << dStampNSec << " Name: " << sImgFileName << endl;
		string imagePath = sData_path + "cam0/data/" + sImgFileName;

		Mat img = imread(imagePath.c_str(), 0);
		if (img.empty())
		{
			cerr << "image is empty! path: " << imagePath << endl;
			return;
		}
		//pSystem->PubImageData(dStampNSec / 1e9, img);
		cv::Mat show_img;
		cv::cvtColor(img, show_img, CV_GRAY2RGB);
		if (SHOW_TRACK)
		{
			for (unsigned int j = 0; j < pSystem->trackerData[0].cur_pts.size(); j++)
			{
				double len = min(1.0, 1.0 *  pSystem->trackerData[0].track_cnt[j] / WINDOW_SIZE);
				cv::circle(show_img,  pSystem->trackerData[0].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
			}

			cv::namedWindow("IMAGE", CV_WINDOW_AUTOSIZE);
			cv::imshow("IMAGE", show_img);
		  // cv::waitKey(1);
		}

		pSystem->DrawGLFrame();
		usleep(50000*nDelayTimes);
	}
	fsImage.close();

}
#endif
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;
}

注意上述代码路径修改，以及void PubImuData()，void PubImageData()中函数修改。
且CMakeLists.txt文件添加如下：

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

2.Sysytem.cpp修改

在System.h中添加函数声明：

void PubSimImageData(double dStampSec, const vector<cv::Point2f> &FeaturePoints);

在System.cpp中添加函数,代码如下：

void System::PubSimImageData(double dStampSec, const vector<cv::Point2f> &FeaturePoints)
{
    if (!init_feature)
    {
        cout << "1 PubImageData skip the first detected feature, which doesn't contain optical flow speed" << endl;
        init_feature = 1;
        return;
    }

    if (first_image_flag)
    {
        cout << "2 PubImageData first_image_flag" << endl;
        first_image_flag = false;
        first_image_time = dStampSec;
        last_image_time = dStampSec;
        return;
    }
    // detect unstable camera stream 发现时间戳不连续甚至倒退，提示重新输入
    if (dStampSec - last_image_time > 1.0 || dStampSec < last_image_time)
    {
        cerr << "3 PubImageData image discontinue! reset the feature tracker!" << endl;
        first_image_flag = true;
        last_image_time = 0;
        pub_count = 1;
        return;
    }
    last_image_time = dStampSec;
    // frequency control 控制频率设定小于某一阈值
//    if (round(1.0 * pub_count / (dStampSec - first_image_time)) <= FREQ)
//    {
//        PUB_THIS_FRAME = true;
//        // reset the frequency control TODO question:若当前连续图像序列的频率与 FREQ=10 误差在一定范围内重置？
//        if (abs(1.0 * pub_count / (dStampSec - first_image_time) - FREQ) < 0.01 * FREQ)
//        {
//            first_image_time = dStampSec;
//            pub_count = 0;
//        }
//    }
//    else
//    {
//        PUB_THIS_FRAME = false;
//    }
    PUB_THIS_FRAME = true;

    TicToc t_r;
    // cout << "3 PubImageData t : " << dStampSec << endl;
    // TODO Bookmark:获取图像特征点
//    trackerData[0].readImage(img, dStampSec);
//    trackerData[0].readPoints(FeaturePoints, dStampSec);

//    for (unsigned int i = 0;; i++)
//    {
//        bool completed = false;
//        completed |= trackerData[0].updateID(i);
//
//        if (!completed)
//            break;
//    }
    if (PUB_THIS_FRAME)
    {
        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++)
        {
//            auto &un_pts = trackerData[i].cur_un_pts;// 去畸变的归一化图像坐标
//            auto &cur_pts = trackerData[i].cur_pts;// 当前追踪到的特征点
//            auto &ids = trackerData[i].ids;
//            auto &pts_velocity = trackerData[i].pts_velocity;
            for (unsigned int j = 0; j < FeaturePoints.size(); j++)
            {
//                if (trackerData[i].track_cnt[j] > 1)
//                {
//                    int p_id = ids[j];
                int p_id = j;
                hash_ids[i].insert(p_id);
                double x = FeaturePoints[j].x;
                double y = FeaturePoints[j].y;
                double z = 1;
                feature_points->points.push_back(Vector3d(x, y, z));
                feature_points->id_of_point.push_back(p_id * NUM_OF_CAM + i);
//                    feature_points->u_of_point.push_back(cur_pts[j].x); // 像素坐标
//                    feature_points->v_of_point.push_back(cur_pts[j].y);
//                    // TODO Bookmark:速度项用于对齐imu时间戳 作业不考虑可以设为0
//                    feature_points->velocity_x_of_point.push_back(pts_velocity[j].x);
//                    feature_points->velocity_y_of_point.push_back(pts_velocity[j].y);

                cv::Point2f pixel_point;
                pixel_point.x = 460 * x + 255;
                pixel_point.y = 460 * y + 255;

                feature_points->u_of_point.push_back(pixel_point.x); // 像素坐标
                feature_points->v_of_point.push_back(pixel_point.y);
                feature_points->velocity_x_of_point.push_back(0);
                feature_points->velocity_y_of_point.push_back(0);
//                }
            }

            // 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();
            }
        }
    }
}

3.config文件夹下euroc_config.yaml参数修改

如下所示需要修改的地方：

image_width: 640
image_height: 640
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 460
   fy: 460
   cx: 255
   cy: 255

extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [0, 0, -1,
          -1, 0, 0,
          0, 1, 0]
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [0.05,0.04,0.03]

acc_n: 0.019          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.015         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.0001         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 1.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5

4.param.h修改

param.h在vio_data_simulation的src文件夹下

4.1无噪声

void PubImuData()///读IMU数据并调用系统接口去处理imu数据
{
    string sImu_data_file = sConfig_path +  "imu_pose.txt";

4.2小噪声

void PubImuData()///读IMU数据并调用系统接口去处理imu数据
{
    string sImu_data_file = sConfig_path +  "imu_pose_noise.txt";

  // noise

    double gyro_bias_sigma = 1.0e-6;
    double acc_bias_sigma = 0.0001;

    double gyro_noise_sigma = 0.015;    // rad/s * 1/sqrt(hz)
    double acc_noise_sigma = 0.019;      //　m/(s^2) * 1/sqrt(hz)

4.3大噪声

  // noise

    double gyro_bias_sigma = 1.0e-5;
    double acc_bias_sigma = 0.0001;

    double gyro_noise_sigma = 0.015;    // rad/s * 1/sqrt(hz)
    double acc_noise_sigma = 0.019;      //　m/(s^2) * 1/sqrt(hz)

这里比较麻烦点的是imu_pose_noise.txt要重新生成并放在config下

---

总结

越学越迷糊了属于是。。。。。。
暑假放了半个月的假期，松懈了一段时间，后面要继续努力了！！