深蓝学院《从零开始手写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 velocityif (!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));}elsepts_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）有噪声数据

其中加速度计的bias随机游走的方差为0.0001，高斯噪声的方差为0.019; 陀螺仪的bias随机游走的方差为为1e-5,高斯噪声的方差为0.015

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

b.不带噪声数据

结论：带噪声的数据比不带噪声的数据效果声好。