深蓝学院《从零开始手写VIO》作业七

深蓝学院《从零开始手写VIO》作业七

将第二讲中的仿真数据（视觉特征，imu数据）接入VINS系统，并运行出轨迹结果

第一步：将IMU数据传入VINS系统
这部分的代码实现在run_euroc.cpp中的PubSimImuData部分，如下：

void PubSimImuData()
{
	string sImu_data_file = sConfig_path + "imu_pose_noise.txt"; 
	cout << "1 PubImuData start sImu_data_file: " << 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;
	double tmp;
	Vector3d vAcc;
	Vector3d vGyr;
	while (std::getline(fsImu, sImu_line) && !sImu_line.empty()) // read imu data
	{
		std::istringstream ssImuData(sImu_line);
		ssImuData >> dStampNSec;
		for(int i=0;i<7;i++)
		     ssImuData>>tmp;   
		ssImuData>>vGyr.x() >> vGyr.y() >> vGyr.z() >> vAcc.x() >> vAcc.y() >> vAcc.z(); 
		pSystem->PubImuData(dStampNSec, vGyr, vAcc);
		usleep(5000*nDelayTimes);
	}
	fsImu.close();
}

第二步：将特征数据传入VINS系统
读入特征数据的代码实现在run_euroc.cpp中的PubSimImageData部分，如下：

void PubSimImageData()
{
	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;
	while (std::getline(fsImage, sImage_line) && !sImage_line.empty())
	{
		std::istringstream ssImgData(sImage_line);
		ssImgData >> dStampNSec;   //读入时间戳
		cout<<"cam time: "<<fixed<<dStampNSec<<endl;
		string all_points_file_name = sPointData_path + "all_points_" + to_string(n)+ ".txt";  //第n个相机对应的观测数据的文件名
        cout<<"points_file: "<<all_points_file_name<<endl;
		pSystem->PubSimImageData(dStampNSec, all_points_file_name);

		usleep(50000*nDelayTimes);
		n++;
	}
	fsImage.close();
}

第三步：修改System.cpp
通过函数loadPointData传入double时间戳和string文件名，如下：

    TicToc t_r;
    //trackerData[0].readImage(img, dStampSec);
    trackerData[0].loadPointData(points_file_name,dStampSec);

第四步：修改feature_tracker.cpp
由于没有图像，并且所有的特征点都在每一个相机中被成功跟踪，所以就不用光流函数追踪。直接读入当前时刻相机对应的观测值并存入forw_pts即可，并将每个特征点的track_cnt++。对于初始时刻，由于cur_pts是空的，需要对该时刻的相机观测值forw_pts进行初始化，将该时刻相机对特征点的观测值（像素坐标）存入n_pts中，然后调用函数addPoints来完成forw_pts,ids,track_cnt的初始化。最后，将forw_pts赋值给cur_pts。并通过函数undistoredSimPoints来计算像素点的速度，如下：

void FeatureTracker::undistortedSimPoints()
{
    cur_un_pts.clear();
    cur_un_pts_map.clear();
    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
    for (unsigned int i = 0; i < cur_pts.size(); i++)
    {
        Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
        cur_un_pts.push_back(cv::Point2f(cur_pts[i].x,cur_pts[i].y));
        cur_un_pts_map.insert(make_pair(ids[i], cv::Point2f(cur_pts[i].x,cur_pts[i].y)));
        //printf("cur pts id %d %f %f", ids[i], cur_un_pts[i].x, cur_un_pts[i].y);
    }
    // caculate points velocity
    if (!prev_un_pts_map.empty())
    {
        double dt = cur_time - prev_time;
        pts_velocity.clear();
        for (unsigned int i = 0; i < cur_un_pts.size(); i++)
        {
            if (ids[i] != -1)
            {
                std::map<int, cv::Point2f>::iterator it;
                it = prev_un_pts_map.find(ids[i]);
                if (it != prev_un_pts_map.end())
                {
                    double v_x = (cur_un_pts[i].x - it->second.x) / dt;
                    double v_y = (cur_un_pts[i].y - it->second.y) / dt;
                    pts_velocity.push_back(cv::Point2f(v_x, v_y));
                }
                else
                    pts_velocity.push_back(cv::Point2f(0, 0));
            }
            else
            {
                pts_velocity.push_back(cv::Point2f(0, 0));
            }
        }
    }
    else
    {
        for (unsigned int i = 0; i < cur_pts.size(); i++)
        {
            pts_velocity.push_back(cv::Point2f(0, 0));
        }
    }
    prev_un_pts_map = cur_un_pts_map;
}

实验结果
(1)无噪声仿真数据

（2）有噪声数据

（3）实验结果对比
a. 带噪声数据