深藍(lán)學(xué)院《從零開始手寫VIO》作業(yè)七

• 深藍(lán)學(xué)院《從零開始手寫VIO》作業(yè)七

深藍(lán)學(xué)院《從零開始手寫VIO》作業(yè)七

將第二講中的仿真數(shù)據(jù)（視覺特征，imu數(shù)據(jù)）接入VINS系統(tǒng)，并運(yùn)行出軌跡結(jié)果

第一步：將IMU數(shù)據(jù)傳入VINS系統(tǒng)
這部分的代碼實(shí)現(xiàn)在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(); }

第二步：將特征數(shù)據(jù)傳入VINS系統(tǒng)
讀入特征數(shù)據(jù)的代碼實(shí)現(xiàn)在run_euroc.cpp中的PubSimImageData部分，如下：

void PubSimImageData() {string sImage_file = sConfig_path + "cam_pose.txt"; //包含時(shí)間戳的文件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; //讀入時(shí)間戳cout<<"cam time: "<<fixed<<dStampNSec<<endl;string all_points_file_name = sPointData_path + "all_points_" + to_string(n)+ ".txt"; //第n個(gè)相機(jī)對(duì)應(yīng)的觀測(cè)數(shù)據(jù)的文件名cout<<"points_file: "<<all_points_file_name<<endl;pSystem->PubSimImageData(dStampNSec, all_points_file_name);usleep(50000*nDelayTimes);n++;}fsImage.close(); }

第三步：修改System.cpp
通過函數(shù)loadPointData傳入double時(shí)間戳和string文件名，如下：

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

第四步：修改feature_tracker.cpp
由于沒有圖像，并且所有的特征點(diǎn)都在每一個(gè)相機(jī)中被成功跟蹤，所以就不用光流函數(shù)追蹤。直接讀入當(dāng)前時(shí)刻相機(jī)對(duì)應(yīng)的觀測(cè)值并存入forw_pts即可，并將每個(gè)特征點(diǎn)的track_cnt++。對(duì)于初始時(shí)刻，由于cur_pts是空的，需要對(duì)該時(shí)刻的相機(jī)觀測(cè)值forw_pts進(jìn)行初始化，將該時(shí)刻相機(jī)對(duì)特征點(diǎn)的觀測(cè)值（像素坐標(biāo)）存入n_pts中，然后調(diào)用函數(shù)addPoints來完成forw_pts,ids,track_cnt的初始化。最后，將forw_pts賦值給cur_pts。并通過函數(shù)undistoredSimPoints來計(jì)算像素點(diǎn)的速度，如下：

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

實(shí)驗(yàn)結(jié)果
(1)無噪聲仿真數(shù)據(jù)

（2）有噪聲數(shù)據(jù)

其中加速度計(jì)的bias隨機(jī)游走的方差為0.0001，高斯噪聲的方差為0.019; 陀螺儀的bias隨機(jī)游走的方差為為1e-5,高斯噪聲的方差為0.015

（3）實(shí)驗(yàn)結(jié)果對(duì)比
a. 帶噪聲數(shù)據(jù)

b.不帶噪聲數(shù)據(jù)


結(jié)論：帶噪聲的數(shù)據(jù)比不帶噪聲的數(shù)據(jù)效果聲好。

總結(jié)

以上是生活随笔為你收集整理的深蓝学院《从零开始手写VIO》作业七的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。